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

#include "bsp/mmio.h"
#include "bsp/pmod_spi.h"
#include "bsp/soc.h"
#include "bsp/uart.h"

/* ICM-42686-P/ICM-42688-P user bank 0 registers. */
#define ICM_DEVICE_CONFIG 0x11u
#define ICM_TEMP_DATA1    0x1du
#define ICM_INT_STATUS    0x2du
#define ICM_PWR_MGMT0     0x4eu
#define ICM_GYRO_CONFIG0  0x4fu
#define ICM_ACCEL_CONFIG0 0x50u
#define ICM_WHO_AM_I      0x75u

#define ICM_SPI_READ      0x80u
#define ICM_SPI_DUMMY     0xffu

#define ICM42686_WHO_AM_I 0x44u
#define ICM42688_WHO_AM_I 0x47u

static void delay_us(uint32_t us)
{
  const uint64_t ticks =
      ((uint64_t)BSP_CLINT_TIMEBASE_HZ * us + 999999u) / 1000000u;
  const uint64_t start = bsp_read64(BSP_CLINT_MTIME);
  while ((bsp_read64(BSP_CLINT_MTIME) - start) < ticks) {
  }
}

static void icm_write_reg(uint8_t reg, uint8_t value)
{
  bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value);
}

static uint8_t icm_read_reg(uint8_t reg)
{
  return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY);
}

static int16_t be16(const uint8_t *p)
{
  return (int16_t)(((uint16_t)p[0] << 8) | p[1]);
}

int main(void)
{
  uint8_t raw[14];
  long accel_full_scale_mg;
  long gyro_full_scale_mdps;

  printf("\nICM-42686-P/ICM-42688-P PMOD SPI test\n");
  printf("SPI MMIO: 0x%08lx, Genesys2 PMOD JC\n",
         (unsigned long)BSP_PMOD_SPI_BASE);
  printf("pins: CS=JC1 MOSI=JC2 MISO=JC3 SCK=JC4\n");

  bsp_pmod_spi_init();
  bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
  bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
  delay_us(1000u);

  printf("WHO_AM_I reads:");
  uint8_t who_am_i = 0u;
  for (unsigned attempt = 0; attempt < 4u; ++attempt) {
    who_am_i = icm_read_reg(ICM_WHO_AM_I);
    printf(" %02x", who_am_i);
  }
  printf("\n");

  /*
   * Keep DEVICE_CONFIG.SPI_MODE at its reset value. Although the RTL shifts
   * and samples on the falling edge, board propagation makes it interoperate
   * with the sensor's default mode-0 timing. Switching the sensor to mode 1
   * breaks this working timing relationship on the current bitstream.
   */
  const uint8_t device_config = icm_read_reg(ICM_DEVICE_CONFIG);
  printf("DEVICE_CONFIG=0x%02x (expected 0x00)\n", device_config);

  if (who_am_i == ICM42686_WHO_AM_I) {
    accel_full_scale_mg = 32000l;
    gyro_full_scale_mdps = 4000000l;
    printf("WHO_AM_I=0x%02x: ICM-42686-P PASS\n", who_am_i);
  } else if (who_am_i == ICM42688_WHO_AM_I) {
    accel_full_scale_mg = 16000l;
    gyro_full_scale_mdps = 2000000l;
    printf("WHO_AM_I=0x%02x: ICM-42688-P PASS\n", who_am_i);
  } else {
    printf("WHO_AM_I=0x%02x: FAIL (expected 0x44 or 0x47)\n", who_am_i);
    printf("SPI communication failed. DK J4: CS=1 SCK=2 MOSI=3 MISO=4 GND=7.\n");
    printf("Hold the DK SAMG55 in reset so it cannot contend for the sensor SPI bus.\n");
    bsp_uart_wait_tx_idle();
    return 1;
  }

  /* Enable accelerometer and gyroscope in low-noise mode. */
  icm_write_reg(ICM_PWR_MGMT0, 0x0fu);
  delay_us(50000u);

  const uint8_t pwr = icm_read_reg(ICM_PWR_MGMT0);
  const uint8_t gyro_cfg = icm_read_reg(ICM_GYRO_CONFIG0);
  const uint8_t accel_cfg = icm_read_reg(ICM_ACCEL_CONFIG0);
  printf("PWR_MGMT0=0x%02x GYRO_CONFIG0=0x%02x ACCEL_CONFIG0=0x%02x\n",
         pwr, gyro_cfg, accel_cfg);
  printf("units: temperature=mC accel=mg gyro=mdps\n");

  for (unsigned sample = 0; sample < 40u; ++sample) {
    const uint8_t status = icm_read_reg(ICM_INT_STATUS);
    bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ,
                           ICM_SPI_DUMMY,
                           raw,
                           sizeof(raw));

    const int16_t temp = be16(&raw[0]);
    const int16_t ax = be16(&raw[2]);
    const int16_t ay = be16(&raw[4]);
    const int16_t az = be16(&raw[6]);
    const int16_t gx = be16(&raw[8]);
    const int16_t gy = be16(&raw[10]);
    const int16_t gz = be16(&raw[12]);

    const long temp_mc = 25000l + (long)(((int64_t)temp * 100000) / 13248);
    const long ax_mg = (long)(((int64_t)ax * accel_full_scale_mg) / 32768);
    const long ay_mg = (long)(((int64_t)ay * accel_full_scale_mg) / 32768);
    const long az_mg = (long)(((int64_t)az * accel_full_scale_mg) / 32768);
    const long gx_mdps = (long)(((int64_t)gx * gyro_full_scale_mdps) / 32768);
    const long gy_mdps = (long)(((int64_t)gy * gyro_full_scale_mdps) / 32768);
    const long gz_mdps = (long)(((int64_t)gz * gyro_full_scale_mdps) / 32768);

    printf("%02u st=%02x T=%ld A=%ld,%ld,%ld G=%ld,%ld,%ld\n",
           sample, status, temp_mc,
           ax_mg, ay_mg, az_mg, gx_mdps, gy_mdps, gz_mdps);
    delay_us(100000u);
  }

  printf("IMU read complete\n");
  bsp_uart_wait_tx_idle();
  return 0;
}