#include <stdint.h>
#include <stdio.h>

#include "bsp/irq.h"
#include "bsp/mmio.h"
#include "bsp/plic.h"
#include "bsp/soc.h"
#include "bsp/uart.h"
#include "lsm6dsox.h"

#ifndef LSM6DSOX_ODR_HZ
#define LSM6DSOX_ODR_HZ 104u
#endif

#ifndef CAPTURE_DURATION_S
#define CAPTURE_DURATION_S 60u
#endif

#ifndef LSM6DSOX_IRQ_QUEUE_CAPACITY
#define LSM6DSOX_IRQ_QUEUE_CAPACITY 256u
#endif

#if (LSM6DSOX_IRQ_QUEUE_CAPACITY < 2u)
#error "LSM6DSOX_IRQ_QUEUE_CAPACITY must be at least 2"
#endif

typedef struct {
  lsm6dsox_sample_t sample;
  uint32_t sequence;
} queued_sample_t;

static queued_sample_t sample_queue[LSM6DSOX_IRQ_QUEUE_CAPACITY];
static volatile unsigned sample_head;
static volatile unsigned sample_tail;
static volatile uint64_t interrupt_count;
static volatile uint64_t interrupt_sample_count;
static volatile uint64_t interrupt_read_fail_count;
static volatile uint64_t queue_overflow_count;
static volatile uint64_t unexpected_irq_count;
static volatile uint64_t unexpected_trap_cause;

void sensor_irq_handler(void)
  __attribute__((interrupt("machine"), aligned(4)));

static uint64_t time_now(void)
{
  return bsp_read64(BSP_CLINT_MTIME);
}

static unsigned next_queue_index(unsigned index)
{
  index++;
  return (index == LSM6DSOX_IRQ_QUEUE_CAPACITY) ? 0u : index;
}

static void queue_push_isr(const lsm6dsox_sample_t *sample,
                           uint32_t sequence)
{
  unsigned head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
  const unsigned next = next_queue_index(head);
  if (next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) {
    __atomic_fetch_add(&queue_overflow_count, 1u, __ATOMIC_RELAXED);
    return;
  }
  sample_queue[head].sample = *sample;
  sample_queue[head].sequence = sequence;
  __atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
}

static int queue_pop(queued_sample_t *queued)
{
  unsigned tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
  if (tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) {
    return 0;
  }
  *queued = sample_queue[tail];
  __atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
  return 1;
}

void sensor_irq_handler(void)
{
  asm volatile(
    ".option push\n"
    ".option norelax\n"
    "la gp, __global_pointer$\n"
    ".option pop\n"
    : : : "memory");

  const uint64_t interrupt_timestamp = time_now();
  const uint64_t mcause = bsp_read_mcause();
  if (((mcause >> 63) == 0u) ||
      ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL)) {
    unexpected_trap_cause = mcause;
    return;
  }

  const uint32_t irq_id = bsp_plic_claim();
  if (irq_id == 0u) {
    __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
    return;
  }
  if (irq_id == BSP_PMOD_IRQ) {
    __atomic_fetch_add(&interrupt_count, 1u, __ATOMIC_RELAXED);
    lsm6dsox_sample_t sample;
    if (lsm6dsox_read_interrupt_sample(&sample, interrupt_timestamp)) {
      const uint64_t sample_number =
          __atomic_fetch_add(&interrupt_sample_count, 1u, __ATOMIC_RELAXED);
      queue_push_isr(&sample, (uint32_t)sample_number);
    } else {
      __atomic_fetch_add(&interrupt_read_fail_count, 1u, __ATOMIC_RELAXED);
    }
  } else {
    __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
  }
  bsp_plic_complete(irq_id);
}

static void print_identity_probe(void)
{
  printf("WHO_AM_I probes:");
  for (unsigned i = 0u; i < 8u; ++i) {
    printf(" %02x", lsm6dsox_read_who_am_i());
  }
  printf(" (expected 6c)\n");
}

int main(void)
{
  printf("\nSTEVAL-MKI217V1 / LSM6DSOX INT1 data-ready test\n");
  printf("Genesys2 JC: CS=JC1 SDA/MOSI=JC2 SDO/MISO=JC3 SCL/SCK=JC4\n");
  printf("Power: VDD=3.3V VDDIO=3.3V common GND; sensor INT1=JB1/PLIC%lu\n",
         (unsigned long)BSP_PMOD_IRQ);
  printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n",
         (unsigned long)BSP_PMOD_SPI_BASE,
         (unsigned long)BSP_CLINT_TIMEBASE_HZ,
         (unsigned long)bsp_uart_baudrate());

  if (!lsm6dsox_init(LSM6DSOX_ODR_HZ)) {
    print_identity_probe();
    printf("INIT FAIL: check CS, MOSI, MISO, SCK, 3.3V and common GND.\n");
    printf("0x00 usually means MISO held low; 0xff usually means no selected device.\n");
    bsp_uart_wait_tx_idle();
    return 1;
  }

  print_identity_probe();
  printf("CONFIG CTRL1_XL=%02x CTRL2_G=%02x CTRL3_C=%02x STATUS=%02x\n",
         lsm6dsox_read_register(0x10u),
         lsm6dsox_read_register(0x11u),
         lsm6dsox_read_register(0x12u),
         lsm6dsox_read_register(0x1eu));

  sample_head = 0u;
  sample_tail = 0u;
  interrupt_count = 0u;
  interrupt_sample_count = 0u;
  interrupt_read_fail_count = 0u;
  queue_overflow_count = 0u;
  unexpected_irq_count = 0u;
  unexpected_trap_cause = 0u;
  bsp_plic_disable_all();
  bsp_plic_set_threshold(0u);
  bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
  /* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped
   * after claim but before complete. The sensor route is still disabled. */
  bsp_plic_complete(BSP_PMOD_IRQ);
  bsp_set_mtvec(sensor_irq_handler);
  bsp_plic_enable(BSP_PMOD_IRQ);
  bsp_enable_machine_external_interrupts();
  if (!lsm6dsox_enable_data_ready_interrupt()) {
    printf("INT INIT FAIL: INT1_CTRL=%02x INT2_CTRL=%02x TAP_CFG2=%02x "
           "COUNTER_BDR_REG1=%02x\n",
           lsm6dsox_read_register(0x0du),
           lsm6dsox_read_register(0x0eu),
           lsm6dsox_read_register(0x58u),
           lsm6dsox_read_register(0x0bu));
    bsp_plic_disable(BSP_PMOD_IRQ);
    lsm6dsox_power_down();
    bsp_uart_wait_tx_idle();
    return 2;
  }
  printf("INT_CONFIG,pin=INT1,INT1_CTRL=%02x,INT2_CTRL=%02x,"
         "TAP_CFG2=%02x,COUNTER_BDR_REG1=%02x,mode=latched\n",
         lsm6dsox_read_register(0x0du),
         lsm6dsox_read_register(0x0eu),
         lsm6dsox_read_register(0x58u),
         lsm6dsox_read_register(0x0bu));

  const lsm6dsox_scale_t *scale = lsm6dsox_scale();
  printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu,"
         "accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%lu,"
         "temp_offset_millic=%ld\n",
         (unsigned long)BSP_CLINT_TIMEBASE_HZ,
         (unsigned long)scale->odr_hz,
         (unsigned long)scale->accel_ug_per_lsb,
         (unsigned long)scale->gyro_udps_per_lsb,
         (unsigned long)scale->temperature_lsb_per_c,
         (long)scale->temperature_offset_millic);
  printf("IMU_COLUMNS,sensor,seq,tick,ax_raw,ay_raw,az_raw,"
         "gx_raw,gy_raw,gz_raw,temp_raw\n");

  const uint64_t start = time_now();
  const uint64_t duration_ticks =
      (uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S;
  uint32_t samples = 0u;
  uint32_t sequence_discontinuities = 0u;
  uint32_t expected_sequence = 0u;
  uint64_t previous_tick = 0u;
  uint64_t maximum_gap = 0u;

  while ((time_now() - start) < duration_ticks) {
    /* This diagnostic must still time out and report zero IRQs when INT1 is
     * disconnected. The production application can sleep on its other IRQs. */
    asm volatile("nop");
    queued_sample_t queued;
    while (queue_pop(&queued)) {
      const lsm6dsox_sample_t *sample = &queued.sample;
      if (queued.sequence != expected_sequence) {
        sequence_discontinuities++;
      }
      expected_sequence = queued.sequence + 1u;

      if (previous_tick != 0u) {
        const uint64_t gap = sample->timestamp_ticks - previous_tick;
        if (gap > maximum_gap) {
          maximum_gap = gap;
        }
      }
      previous_tick = sample->timestamp_ticks;

      printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
             (unsigned long)queued.sequence,
             (unsigned long)sample->timestamp_ticks,
             (int)sample->accel[0], (int)sample->accel[1],
             (int)sample->accel[2], (int)sample->gyro[0],
             (int)sample->gyro[1], (int)sample->gyro[2],
             (int)sample->temperature);
      samples++;
    }
  }

  bsp_plic_disable(BSP_PMOD_IRQ);
  lsm6dsox_disable_data_ready_interrupt();
  lsm6dsox_power_down();
  printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu,"
         "irq=%lu,irq_samples=%lu,read_fail=%lu,queue_overflow=%lu,"
         "seq_discontinuities=%lu,unexpected_irq=%lu,trap=%lu\n",
         (unsigned long)samples, (unsigned)CAPTURE_DURATION_S,
         (unsigned long)maximum_gap,
         (unsigned long)interrupt_count,
         (unsigned long)interrupt_sample_count,
         (unsigned long)interrupt_read_fail_count,
         (unsigned long)queue_overflow_count,
         (unsigned long)sequence_discontinuities,
         (unsigned long)unexpected_irq_count,
         (unsigned long)unexpected_trap_cause);
  bsp_uart_wait_tx_idle();
  return 0;
}