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

#include "bsp/mmio.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

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

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 SPI polling 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; INT is not used\n");
  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));

  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;
  uint64_t previous_tick = 0u;
  uint64_t maximum_gap = 0u;

  while ((time_now() - start) < duration_ticks) {
    lsm6dsox_sample_t sample;
    if (!lsm6dsox_poll_sample(&sample)) {
      continue;
    }

    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)samples,
           (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++;
  }

  lsm6dsox_power_down();
  printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu\n",
         (unsigned long)samples, (unsigned)CAPTURE_DURATION_S,
         (unsigned long)maximum_gap);
  bsp_uart_wait_tx_idle();
  return 0;
}