#include "InertialSensor_BMI055.h"
#include <stdio.h>

#define GRAVITY_MSS     9.80665f

/*
  device registers, names follow datasheet conventions, with REGA_
  prefix for accel, and REGG_ prefix for gyro
 */
#define REGA_BGW_CHIPID    0x00
#define REGA_ACCD_X_LSB    0x02
#define REGA_ACCD_TEMP     0x08
#define REGA_INT_STATUS_0  0x09
#define REGA_INT_STATUS_1  0x0A
#define REGA_INT_STATUS_2  0x0B
#define REGA_INT_STATUS_3  0x0C
#define REGA_FIFO_STATUS   0x0E
#define REGA_PMU_RANGE     0x0F
#define REGA_PMU_BW        0x10
#define REGA_PMU_LPW       0x11
#define REGA_ACCD_HBW      0x13
#define REGA_BGW_SOFTRESET 0x14
#define REGA_OUT_CTRL      0x20
#define REGA_EST_LATCH     0x21
#define REGA_FIFO_CONFIG_0 0x30
#define REGA_PMU_SELF_TEST 0x32
#define REGA_FIFO_CONFIG_1 0x3E
#define REGA_FIFO_DATA     0x3F

#define REGG_CHIPID        0x00
#define REGA_RATE_X_LSB    0x02
#define REGG_INT_STATUS_0  0x09
#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 int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))

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

/*
  probe and initialise accelerometer
 */
static bool _accel_init(InertialSensor_BMI055_t *imu)
{
    SPI_DEV_begin(imu->dev_accel, 1000);

    uint8_t v;
    if (!_read_registers(imu->dev_accel, REGA_BGW_CHIPID, &v, 1) || v != 0xFA) {
        goto failed;
    }

    if (!_write_register(imu->dev_accel, REGA_BGW_SOFTRESET, 0xB6)) {
        goto failed;
    }
    vTaskDelay(10);
    
    // setup 16g range
    if (!_write_register(imu->dev_accel, REGA_PMU_RANGE, 0x0C)) {
        goto failed;
    }

    // setup filter bandwidth 1kHz
    if (!_write_register(imu->dev_accel, REGA_PMU_BW, 0x0F)) {
        goto failed;
    }

    // disable low-power mode
    if (!_write_register(imu->dev_accel, REGA_PMU_LPW, 0)) {
        goto failed;
    }

    // setup for unfiltered data
    if (!_write_register(imu->dev_accel, REGA_ACCD_HBW, 0x80)) {
        goto failed;
    }

    // setup FIFO for streaming X,Y,Z
    if (!_write_register(imu->dev_accel, REGA_FIFO_CONFIG_1, 0x80)) {
        goto failed;
    }


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

    SPI_DEV_end(imu->dev_accel);
    return true;
    
failed:
    SPI_DEV_end(imu->dev_accel);
    return false;
}

/*
  probe and initialise gyro
 */
static bool _gyro_init(InertialSensor_BMI055_t *imu)
{
    SPI_DEV_begin(imu->dev_gyro, 1000);

    uint8_t v;
    if (!_read_registers(imu->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F) {
        goto failed;
    }

    if (!_write_register(imu->dev_gyro, REGG_BGW_SOFTRESET, 0xB6)) {
        goto failed;
    }
    vTaskDelay(10);

    // setup 2000dps range
    if (!_write_register(imu->dev_gyro, REGG_RANGE, 0x00)) {
        goto failed;
    }

    // setup filter bandwidth 230Hz, no decimation
    if (!_write_register(imu->dev_gyro, REGG_BW, 0x81)) {
        goto failed;
    }

    // disable low-power mode
    if (!_write_register(imu->dev_gyro, REGG_LPM1, 0)) {
        goto failed;
    }

    // setup for filtered data
    if (!_write_register(imu->dev_gyro, REGG_RATE_HBW, 0x00)) {
        goto failed;
    }

    // setup FIFO for streaming X,Y,Z
    if (!_write_register(imu->dev_gyro, REGG_FIFO_CONFIG_1, 0x80)) {
        goto failed;
    }

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

    SPI_DEV_end(imu->dev_gyro);
    return true;
    
failed:
    SPI_DEV_end(imu->dev_gyro);
    return false;
}

bool InertialSensor_BMI055_init(InertialSensor_BMI055_t *imu, const char *name, SPI_DEV_t* dev_accel, SPI_DEV_t* dev_gyro)
{
	  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;
}

/*
  read accel fifo
 */
static void _read_fifo_accel(InertialSensor_BMI055_t *imu)
{
    uint8_t num_frames;

		 if (!_read_registers(imu->dev_accel, REGA_FIFO_STATUS, &num_frames, 1)) {
					++imu->accel_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_accel, REGA_FIFO_DATA, data, num_frames*6)) {
					++imu->accel_error_count;
					return;
			}

			if (imu->temperature_counter++ == 100) {
					imu->temperature_counter = 0;
					int8_t t;
					if (!_read_registers(imu->dev_accel, REGA_ACCD_TEMP, (uint8_t *)&t, 1)) {
							++imu->accel_error_count;
					} else {
							imu->temp_degc = (0.5f * t) + 23.0f;
					}
			}

			// data is 12 bits with 16g range, 7.81mg/LSB
			const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0f;
			for (uint8_t i = 0; i < num_frames; i++) {
					const uint8_t *d = &data[i*6];
					int16_t xyz[3];
					xyz[0] = (int16_t)((uint16_t)((d[0]&0xF0) | (d[1]<<8)));
					xyz[1] = (int16_t)((uint16_t)((d[2]&0xF0) | (d[3]<<8)));
					xyz[2] = (int16_t)((uint16_t)((d[4]&0xF0) | (d[5]<<8)));

					imu->accel[0] = xyz[0] * scale;
					imu->accel[1] = xyz[1] * scale;
					imu->accel[2] = xyz[2] * scale;
			}
}

/*
  read gyro fifo
 */
static void _read_fifo_gyro(InertialSensor_BMI055_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 / 57.3f) / 32767.0f;
			for (uint8_t i = 0; i < num_frames; i++) {
					const uint8_t *d = &data[i*6];
					int16_t xyz[3];
					xyz[0] = (int16_t)((uint16_t)((d[0]) | (d[1]<<8)));
					xyz[1] = (int16_t)((uint16_t)((d[2]) | (d[3]<<8)));
					xyz[2] = (int16_t)((uint16_t)((d[4]) | (d[5]<<8)));

					imu->gyro[0] = xyz[0] * scale;
					imu->gyro[1] = xyz[1] * scale;
					imu->gyro[2] = xyz[2] * scale;
			}
}



void InertialSensor_BMI055_update(InertialSensor_BMI055_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);
		}
	}
}