#include "InertialSensor_ADIS1647x.h"

#define GRAVITY_MSS 9.80665f

#define BACKEND_SAMPLE_RATE 2000

/*
  device registers
 */
#define REG_PROD_ID 0x72
#define PROD_ID_16470 0x4056
#define PROD_ID_16477 0x405d

#define REG_GLOB_CMD 0x68
#define GLOB_CMD_SW_RESET 0x80

#define REG_RANG_MDL 0x5E // 16477 only

#define REG_DATA_CNTR 0x22

/*
  timings
 */
#define T_STALL_US 20U
#define T_RESET_MS 500U

extern unsigned long getRunTimeCounterValue(void);

void delay_microseconds(unsigned long dt)
{
    unsigned long t0 = getRunTimeCounterValue();
    unsigned long t1 = t0 + dt;
    while (getRunTimeCounterValue() < t1)
    {
        ;
    }
}

float radians(float in)
{
    return in * 3.141592654f / 180.0f;
}

int16_t be16toh(int16_t in)
{
    int16_t rslt;

    ((uint8_t *)&rslt)[0] = ((uint8_t *)&in)[1];
    ((uint8_t *)&rslt)[1] = ((uint8_t *)&in)[0];

    return rslt;
}
static void read_sensor(InertialSensor_ADIS1647x_t *imu);
static bool check_product_id(InertialSensor_ADIS1647x_t *imu);

// read a 16 bit register
static uint16_t read_reg16(InertialSensor_ADIS1647x_t *imu, uint8_t regnum);

// write a 16 bit register
static void write_reg16(InertialSensor_ADIS1647x_t *imu, uint8_t regnum, uint16_t value);

bool InertialSensor_ADIS1647x_init(InertialSensor_ADIS1647x_t *imu,
                                   const char *name,
                                   SPI_DEV_t *dev,
                                   GPIO_EXIT_t *drdy)
{
    imu->dev = dev;
    imu->drdy = drdy;
    imu->name = name;
    imu->crc_err = 0;
    imu->seq_err = 0;
    imu->done_first_read = false;
    imu->last_counter = 0;
    imu->temp_count = 0;
    imu->comm_err = 0;
    imu->good_cnt = 0;

    SPI_DEV_begin(imu->dev, -1);

    osDelay(T_RESET_MS);

    if (!check_product_id(imu))
    {
        goto failsafe;
    }
    // perform software reset
    write_reg16(imu, REG_GLOB_CMD, GLOB_CMD_SW_RESET);
    osDelay(T_RESET_MS);

    // re-check after reset
    if (!check_product_id(imu))
    {
        goto failsafe;
    }

    // we leave all config registers at defaults

    SPI_DEV_end(imu->dev);
    imu->success = true;
    return true;

failsafe:
    SPI_DEV_end(imu->dev);
    imu->success = false;
    return false;
}

/*
  check product ID
 */
bool check_product_id(InertialSensor_ADIS1647x_t *imu)
{
    uint16_t prod_id = read_reg16(imu, REG_PROD_ID);
    switch (prod_id)
    {
    case PROD_ID_16470:
        // can do up to 40G
        imu->accel_scale = 1.25 * GRAVITY_MSS * 0.001;
        imu->_clip_limit = 39.5f * GRAVITY_MSS;
        imu->gyro_scale = radians(0.1);
        return true;

    case PROD_ID_16477:
        // can do up to 40G
        imu->accel_scale = 1.25 * GRAVITY_MSS * 0.001;
        imu->_clip_limit = 39.5f * GRAVITY_MSS;
        // RANG_MDL register used for gyro range
        uint16_t rang_mdl = read_reg16(imu, REG_RANG_MDL);
        switch ((rang_mdl >> 2) & 3)
        {
        case 0:
            imu->gyro_scale = radians(1.0 / 160);
            break;
        case 1:
            imu->gyro_scale = radians(1.0 / 40);
            break;
        case 3:
            imu->gyro_scale = radians(1.0 / 10);
            break;
        default:
            return false;
        }
        return true;
    }
    return false;
}

/*
  read a 16 bit register value
 */
uint16_t read_reg16(InertialSensor_ADIS1647x_t *imu, uint8_t regnum)
{
    uint8_t req[2];
    uint8_t reply[2];

    req[0] = regnum;
    req[1] = 0;
    SPI_DEV_select(imu->dev);
    SPI_DEV_transfer(imu->dev, req, sizeof(req), NULL, 0, 1);
    SPI_DEV_unselect(imu->dev);

    delay_microseconds(T_STALL_US);

    SPI_DEV_select(imu->dev);
    SPI_DEV_transfer(imu->dev, NULL, 0, reply, sizeof(reply), 1);
    SPI_DEV_unselect(imu->dev);

    uint16_t ret = (reply[0] << 8U) | reply[1];
    return ret;
}

/*
  write a 16 bit register value
 */
void write_reg16(InertialSensor_ADIS1647x_t *imu, uint8_t regnum, uint16_t value)
{
    uint8_t req[2];

    req[0] = (regnum | 0x80);
    req[1] = value & 0xFF;

    SPI_DEV_select(imu->dev);
    SPI_DEV_transfer(imu->dev, req, sizeof(req), NULL, 0, 1);
    SPI_DEV_unselect(imu->dev);

    delay_microseconds(T_STALL_US);

    req[0] = ((regnum + 1) | 0x80);
    req[1] = (value >> 8) & 0xFF;

    SPI_DEV_select(imu->dev);
    SPI_DEV_transfer(imu->dev, req, sizeof(req), NULL, 0, 1);
    SPI_DEV_unselect(imu->dev);

    delay_microseconds(T_STALL_US);
}

typedef struct
{
    uint8_t cmd[2];
    uint16_t diag_stat;
    int16_t gx;
    int16_t gy;
    int16_t gz;
    int16_t ax;
    int16_t ay;
    int16_t az;
    int16_t temp;
    uint16_t counter;
    uint8_t pad;
    uint8_t checksum;
} adis_data;

/*
  loop to read the sensor
 */
void read_sensor(InertialSensor_ADIS1647x_t *imu)
{
    adis_data data_in;
    adis_data data;
    do
    {
        data_in.cmd[0] = REG_GLOB_CMD;
        for (int i=1; i<sizeof(data_in);++i)
        {
          ((uint8_t *)&data_in)[i] = 0;
        }
        SPI_DEV_begin(imu->dev, -1);
        SPI_DEV_select(imu->dev);
		// if (HAL_OK != HAL_SPI_TransmitReceive(imu->dev->bus->hspi,(uint8_t *)&data_in, (uint8_t *)&data, sizeof(data), 1))
		// {
        //      SPI_DEV_unselect(imu->dev);
        //      SPI_DEV_end(imu->dev);
        //      imu->comm_err++;
        //      return;
		// }
        if (!SPI_DEV_transfer(imu->dev, (uint8_t *)&data_in, sizeof(data), (uint8_t *)&data, sizeof(data), 1))
        {
            SPI_DEV_unselect(imu->dev);
            SPI_DEV_end(imu->dev);
            imu->comm_err++;
            return;
        }
        SPI_DEV_unselect(imu->dev);
        SPI_DEV_end(imu->dev);
    } while (be16toh(data.counter) == imu->last_counter);

    /*
      check the 8 bit checksum of the packet
     */
    uint8_t sum = 0;
    const uint8_t *b = (const uint8_t *)&data.diag_stat;
    for (uint8_t i = 0; i < offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++)
    {
        sum += b[i];
    }
    if (sum != data.checksum)
    {
        // corrupt data
        imu->crc_err++;
        return;
    }

    /*
      check if we have lost a sample
     */
    uint16_t counter = be16toh(data.counter);
    if (imu->done_first_read)
    {
        if ((imu->last_counter + 2) != counter)
        {
          if ((imu->last_counter) == counter)
          {
              return;
          }
          else
          {
            imu->seq_err++;
          }
        }
    }
    imu->done_first_read = true;
    imu->last_counter = counter;

    imu->accel[1] = (float)be16toh(data.ax);
    imu->accel[0] = (float)be16toh(data.ay);
    imu->accel[2] = -(float)be16toh(data.az);
    imu->gyro[1] = (float)be16toh(data.gx);
    imu->gyro[0] = (float)be16toh(data.gy);
    imu->gyro[2] = -(float)be16toh(data.gz);

    imu->accel[0] *= imu->accel_scale;
    imu->accel[1] *= imu->accel_scale;
    imu->accel[2] *= imu->accel_scale;
    imu->gyro[0] *= imu->gyro_scale;
    imu->gyro[1] *= imu->gyro_scale;
    imu->gyro[2] *= imu->gyro_scale;
    imu->good_cnt++;
    imu->seq++;

    /*
      publish average temperature at 20Hz
     */
    imu->temp_sum += (float)(be16toh(data.temp)) * 0.1f;
    imu->temp_count++;

    if (imu->temp_count == 100)
    {
        imu->temp = imu->temp_sum / imu->temp_count;
        imu->temp_sum = 0;
        imu->temp_count = 0;
    }
}

/*
  sensor read loop
 */
void InertialSensor_ADIS1647x_loop(InertialSensor_ADIS1647x_t *imu)
{
    while (true)
    {
        //GPIO_EXIT_take(imu->drdy, 1);
        osDelay(1);
        read_sensor(imu);
    }
}

void InertialSensor_ADIS1647x_update(InertialSensor_ADIS1647x_t *imu)
{
    read_sensor(imu);
}


void InertialSensor_ADIS1647x_monitor(InertialSensor_ADIS1647x_t *imu)
{
    if (imu->success)
    {
        imu->crc_pps = imu->crc_err;
        imu->seq_pps = imu->seq_err;
        imu->comm_pps = imu->comm_err;
        imu->good_pps = imu->good_cnt;

        imu->crc_err = 0;
        imu->seq_err = 0;
        imu->comm_err = 0;
        imu->good_cnt = 0;
    }
}