#include "InertialSensor_BMI088.h"
#include <stdio.h>
#include <string.h>

#define GRAVITY_MSS 9.80665f

/*
  device registers, names follow datasheet conventions, with REGA_
  prefix for accel, and REGG_ prefix for gyro
 */
#define REGA_CHIPID 0x00
#define REGA_ERR_REG 0x02
#define REGA_STATUS 0x03
#define REGA_X_LSB 0x12
#define REGA_INT_STATUS_1 0x1D
#define REGA_TEMP_LSB 0x22
#define REGA_TEMP_MSB 0x23
#define REGA_CONF 0x40
#define REGA_RANGE 0x41
#define REGA_PWR_CONF 0x7C
#define REGA_PWR_CTRL 0x7D
#define REGA_SOFTRESET 0x7E
#define REGA_FIFO_CONFIG0 0x48
#define REGA_FIFO_CONFIG1 0x49
#define REGA_FIFO_DOWNS 0x45
#define REGA_FIFO_DATA 0x26
#define REGA_FIFO_LEN0 0x24
#define REGA_FIFO_LEN1 0x25

#define REGG_CHIPID 0x00
#define REGA_RATE_X_LSB 0x02
#define REGG_INT_STATUS_1 0x0A
#define REGG_INT_STATUS_2 0x0B
#define REGG_INT_STATUS_3 0x0C
#define REGG_FIFO_STATUS 0x0E
#define REGG_RANGE 0x0F
#define REGG_BW 0x10
#define REGG_LPM1 0x11
#define REGG_RATE_HBW 0x13
#define REGG_BGW_SOFTRESET 0x14
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA 0x3F

#define ACCEL_BACKEND_SAMPLE_RATE 1600
#define GYRO_BACKEND_SAMPLE_RATE 2000

static bool read_registers(SPI_DEV_t *dev, uint8_t reg, uint8_t *buf,
                           uint32_t size)
{
    SPI_DEV_select(dev);
    bool rslt = SPI_DEV_read_registers(dev, reg, buf, size);
    SPI_DEV_unselect(dev);
    return rslt;
}

bool write_register(SPI_DEV_t *dev, uint8_t reg, uint8_t val)
{
    SPI_DEV_select(dev);
    bool rslt = SPI_DEV_write_register(dev, reg, val);
    SPI_DEV_unselect(dev);
    return rslt;
}

/*
  read from accelerometer registers, special SPI handling needed
*/
static bool read_accel_registers(InertialSensor_BMI088_t *imu, uint8_t reg, uint8_t *data, uint8_t len)
{
    // for SPI we need to discard the first returned byte. See
    // datasheet for explanation
    uint8_t b[len + 2];
    SPI_DEV_select(imu->dev_accel);
    b[0] = reg | 0x80;
    memset(&b[1], 0, len + 1);
    if (!SPI_DEV_transfer(imu->dev_accel, b, len + 2, b, len + 2, 2))
    {
        SPI_DEV_unselect(imu->dev_accel);
        return false;
    }
    memcpy(data, &b[2], len);
    SPI_DEV_unselect(imu->dev_accel);
    return true;
}

/*
  write to accel registers with retries. The SPI sensor may take
  several tries to correctly write a register
*/
static bool write_accel_register(InertialSensor_BMI088_t *imu, uint8_t reg, uint8_t v)
{
    for (uint8_t i = 0; i < 8; i++)
    {
        write_register(imu->dev_accel, reg, v);
        uint8_t v2 = 0;
        if (read_accel_registers(imu, reg, &v2, 1) && v2 == v)
        {
            return true;
        }
    }
    return false;
}

static const struct
{
    uint8_t reg;
    uint8_t value;
} accel_config[] = {
    {REGA_CONF, 0xAC},
    // setup 24g range
    {REGA_RANGE, 0x03},
    // disable low-power mode
    {REGA_PWR_CONF, 0},
    {REGA_PWR_CTRL, 0x04},
    // setup FIFO for streaming X,Y,Z
    {REGA_FIFO_CONFIG0, 0x00},
    {REGA_FIFO_CONFIG1, 0x50},
};

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
#endif

static bool setup_accel_config(InertialSensor_BMI088_t *imu)
{
    if (imu->done_accel_config)
    {
        return true;
    }
    imu->accel_config_count++;
    for (uint8_t i = 0; i < ARRAY_SIZE(accel_config); i++)
    {
        uint8_t v;
        if (!read_accel_registers(imu, accel_config[i].reg, &v, 1))
        {
            return false;
        }
        if (v == accel_config[i].value)
        {
            continue;
        }
        if (!write_accel_register(imu, accel_config[i].reg, accel_config[i].value))
        {
            return false;
        }
    }
    imu->done_accel_config = true;
    printf("BMI088: accel config OK (%u tries)\n", (unsigned)imu->accel_config_count);
    return true;
}

/*
  probe and initialise accelerometer
 */
static bool accel_init(InertialSensor_BMI088_t *imu)
{
    uint8_t v;

    // dummy ready on accel ChipID to init accel (see section 3 of datasheet)
    read_accel_registers(imu, REGA_CHIPID, &v, 1);

    if (!read_accel_registers(imu, REGA_CHIPID, &v, 1) || v != 0x1E)
    {
        return false;
    }

    if (!setup_accel_config(imu))
    {
        printf("BMI088: delaying accel config\n");
    }

    printf("BMI088: found accel\n");

    return true;
}

/*
  probe and initialise gyro
 */
static bool gyro_init(InertialSensor_BMI088_t *imu)
{
    uint8_t v;
    if (!read_registers(imu->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F)
    {
        return false;
    }

    /* Soft-reset gyro
        Return value of 'write_register()' is not checked.
        This commands has the tendency to fail upon soft-reset.
    */
    write_register(imu->dev_gyro, REGG_BGW_SOFTRESET, 0xB6);
    osDelay(30);

    //dev_gyro->setup_checked_registers(5, 20);

    // setup 2000dps range
    if (!write_register(imu->dev_gyro, REGG_RANGE, 0x00))
    {
        return false;
    }

    // setup filter bandwidth 230Hz, no decimation
    if (!write_register(imu->dev_gyro, REGG_BW, 0x81))
    {
        return false;
    }

    // disable low-power mode
    if (!write_register(imu->dev_gyro, REGG_LPM1, 0))
    {
        return false;
    }

    // setup for filtered data
    if (!write_register(imu->dev_gyro, REGG_RATE_HBW, 0x00))
    {
        return false;
    }

    // setup FIFO for streaming X,Y,Z
    if (!write_register(imu->dev_gyro, REGG_FIFO_CONFIG_1, 0x80))
    {
        return false;
    }

    printf("BMI088: found gyro\n");

    return true;
}

/*
  read accel fifo
 */
static void read_fifo_accel(InertialSensor_BMI088_t *imu)
{
    if (!setup_accel_config(imu))
    {
        return;
    }
    uint8_t len[2];
    if (!read_accel_registers(imu, REGA_FIFO_LEN0, len, 2))
    {
        imu->accel_error_count++;
        return;
    }
    uint16_t fifo_length = len[0] + (len[1] << 8);
    if (fifo_length & 0x8000)
    {
        // empty
        return;
    }

    // don't read more than 8 frames at a time
    if (fifo_length > 8 * 7)
    {
        fifo_length = 8 * 7;
    }
    if (fifo_length == 0)
    {
        return;
    }

    uint8_t data[fifo_length];
    if (!read_accel_registers(imu, REGA_FIFO_DATA, data, fifo_length))
    {
        imu->accel_error_count++;
        return;
    }
    // assume configured for 24g range
    const float scale = (1.0 / 32768.0) * GRAVITY_MSS * 24.0;
    const uint8_t *p = &data[0];
    int32_t xyz[3];
    int32_t tmp;
    int32_t nn;
    xyz[0] = 0;
    xyz[1] = 0;
    xyz[2] = 0;
    nn = 0;
    while (fifo_length >= 7)
    {
        /*
          the fifo frames are variable length, with the frame type in the first byte
         */
        uint8_t frame_len = 2;
        switch (p[0] & 0xFC)
        {
        case 0x84:
        {
            // accel frame
            frame_len = 7;
            const uint8_t *d = p + 1;
            {
                tmp = (int32_t)(int16_t)((uint16_t)(d[0] | (d[1] << 8)));
                xyz[0] += tmp;
                tmp = (int32_t)(int16_t)((uint16_t)(d[2] | (d[3] << 8)));
                xyz[1] += tmp;
                tmp = (int32_t)(int16_t)((uint16_t)(d[4] | (d[5] << 8)));
                xyz[2] += tmp;
                nn++;
            };
            imu->accel_count++;
            imu->accel_seq++;

            break;
        }
        case 0x40:
            // skip frame
            frame_len = 2;
            break;
        case 0x44:
            // sensortime frame
            frame_len = 4;
            break;
        case 0x48:
            // fifo config frame
            frame_len = 2;
            break;
        case 0x50:
            // sample drop frame
            frame_len = 2;
            break;
        }
        p += frame_len;
        fifo_length -= frame_len;
    }
    if (nn > 0)
    {
        float nscale;
        nscale = scale/nn;
        imu->accel[0] = xyz[1] * nscale;
        imu->accel[1] = xyz[0] * nscale;
        imu->accel[2] = -xyz[2] * nscale;
    }


    if (imu->temperature_counter++ == 100)
    {
        imu->temperature_counter = 0;
        uint8_t tbuf[2];
        if (!read_accel_registers(imu, REGA_TEMP_LSB, tbuf, 2))
        {
            imu->accel_error_count++;
        }
        else
        {
            uint16_t temp_uint11 = (tbuf[0] << 3) | (tbuf[1] >> 5);
            int16_t temp_int11 = temp_uint11 > 1023 ? temp_uint11 - 2048 : temp_uint11;
            imu->temp_degc = temp_int11 * 0.125f + 23;
        }
    }
}

/*
  read gyro fifo
 */
static void read_fifo_gyro(InertialSensor_BMI088_t *imu)
{
    uint8_t num_frames;
    if (!read_registers(imu->dev_gyro, REGG_FIFO_STATUS, &num_frames, 1))
    {
        imu->gyro_error_count++;
        return;
    }
    num_frames &= 0x7F;

    // don't read more than 8 frames at a time
    if (num_frames > 8)
    {
        num_frames = 8;
    }
    if (num_frames == 0)
    {
        return;
    }
    uint8_t data[6 * num_frames];
    if (!read_registers(imu->dev_gyro, REGG_FIFO_DATA, data, num_frames * 6))
    {
        imu->gyro_error_count++;
        return;
    }

    // data is 16 bits with 2000dps range
    const float scale = (2000.0f / 180.0f * M_PI) / 32767.0f;
    imu->gyro_count += num_frames;
    imu->gyro_seq++;
    int32_t xyz[3];
    int32_t tmp;
    xyz[0] = 0;
    xyz[1] = 0;
    xyz[2] = 0;
    for (uint8_t i = 0; i < num_frames; i++)
    {
        const uint8_t *d = &data[i * 6];
        {
            tmp = (int32_t)(int16_t)((uint16_t)(d[0] | d[1] << 8));
            xyz[0] += tmp;
            tmp = (int32_t)(int16_t)((uint16_t)(d[2] | d[3] << 8));
            xyz[1] += tmp;
            tmp = (int32_t)(int16_t)((uint16_t)(d[4] | d[5] << 8));
            xyz[2] += tmp;
        }
    }
    float nscale = scale/num_frames;
    imu->gyro[0] = xyz[1] * nscale;
    imu->gyro[1] = xyz[0] * nscale;
    imu->gyro[2] = -xyz[2] * nscale;
}

bool InertialSensor_BMI088_init(InertialSensor_BMI088_t *imu, const char *name, SPI_DEV_t *dev_accel, SPI_DEV_t *dev_gyro)
{
    imu->done_accel_config = false;
    imu->accel_config_count = 0;

    imu->accel_error_count = 0;
    imu->gyro_error_count = 0;
    imu->temperature_counter = 0;

    imu->name = name;
    imu->dev_accel = dev_accel;
    imu->dev_gyro = dev_gyro;

    SPI_DEV_set_read_flag(dev_accel, 0x80);
    SPI_DEV_set_read_flag(dev_gyro, 0x80);

    imu->success = (accel_init(imu) && gyro_init(imu));
    return imu->success;
}

void InertialSensor_BMI088_update(InertialSensor_BMI088_t *imu)
{
    if (imu->success)
    {
        if (SPI_DEV_begin(imu->dev_accel, 0))
        {
            read_fifo_accel(imu);
            read_fifo_gyro(imu);
            SPI_DEV_end(imu->dev_accel);
        }
    }
}

void InertialSensor_BMI088_loop(InertialSensor_BMI088_t *imu)
{
    while (true)
    {
        osDelay(2);
        if (SPI_DEV_begin(imu->dev_accel, 0))
        {
            read_fifo_accel(imu);
            read_fifo_gyro(imu);
            SPI_DEV_end(imu->dev_accel);
        }
    }
}

void InertialSensor_BMI088_monitor(InertialSensor_BMI088_t *imu)
{
    imu->accel_pps = imu->accel_count;
    imu->gyro_pps = imu->gyro_count;

    imu->accel_count = 0;
    imu->gyro_count = 0;
}