#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 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_WHO_AM_I_VALUE 0x44u

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_select();
  delay_us(10u);
  (void)bsp_pmod_spi_xfer8(reg & 0x7fu);
  delay_us(10u);
  (void)bsp_pmod_spi_xfer8(value);
  delay_us(10u);
  bsp_pmod_spi_deselect();
  delay_us(10u);
}

static void icm_read_regs(uint8_t reg, uint8_t *data, unsigned len)
{
  bsp_pmod_spi_select();
  delay_us(10u);
  (void)bsp_pmod_spi_xfer8(reg | 0x80u);
  delay_us(10u);
  for (unsigned i = 0; i < len; ++i) {
    data[i] = bsp_pmod_spi_xfer8(0xffu);
    delay_us(10u);
  }
  bsp_pmod_spi_deselect();
  delay_us(10u);
}

static uint8_t icm_read_reg(uint8_t reg)
{
  uint8_t value;
  icm_read_regs(reg, &value, 1u);
  return value;
}

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];

  printf("\nICM-42686-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);
  delay_us(1000u);

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

  /*
   * The FPGA master is CPOL=0/CPHA=1. ICM-42686-P powers up accepting
   * modes 0/3, so first switch DEVICE_CONFIG.SPI_MODE to modes 1/2.
   * A mode-1-master/mode-0-slave write is valid because the slave samples
   * MOSI on the rising edge, before the master changes it on the falling edge.
   */
  icm_write_reg(ICM_DEVICE_CONFIG, 0x10u);
  delay_us(100u);

  const uint8_t device_config = icm_read_reg(ICM_DEVICE_CONFIG);
  const uint8_t who_am_i = icm_read_reg(ICM_WHO_AM_I);
  printf("DEVICE_CONFIG=0x%02x (expected 0x10)\n", device_config);
  printf("WHO_AM_I=0x%02x (expected 0x%02x): %s\n",
         who_am_i, ICM_WHO_AM_I_VALUE,
         who_am_i == ICM_WHO_AM_I_VALUE ? "PASS" : "FAIL");

  if (who_am_i != ICM_WHO_AM_I_VALUE) {
    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 (default +/-32g, +/-4000dps)\n");

  for (unsigned sample = 0; sample < 40u; ++sample) {
    const uint8_t status = icm_read_reg(ICM_INT_STATUS);
    icm_read_regs(ICM_TEMP_DATA1, 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 * 32000) / 32768);
    const long ay_mg = (long)(((int64_t)ay * 32000) / 32768);
    const long az_mg = (long)(((int64_t)az * 32000) / 32768);
    const long gx_mdps = (long)(((int64_t)gx * 4000000) / 32768);
    const long gy_mdps = (long)(((int64_t)gy * 4000000) / 32768);
    const long gz_mdps = (long)(((int64_t)gz * 4000000) / 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("ICM-42686-P read complete\n");
  bsp_uart_wait_tx_idle();
  return 0;
}