/*==========================================================================================
MIT License

Copyright (c) 2026 https://madflight.com

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===========================================================================================*/

// Header only BMI270 SPI IMU sensor class

#pragma once

#include "imu.h"
#include <SPI.h>


//Global array that stores the configuration file of BMI270
static const uint8_t bmi270_maximum_fifo_config_file[] = {
    0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00,
    0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5,
    0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22,
    0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
    0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00,
    0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb,
    0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5,
    0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41,
    0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24,
    0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94,
    0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
    0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20,
    0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08,
    0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0,
    0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84,
    0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61,
    0xf5, 0xeb, 0x2c, 0xe1, 0x6f
};

class ImuGizmoBMI270 : public ImuGizmo {
  public:
    int spi_freq = 10000000; //BMI270: SPI 10MHz max, I2C 1MHz max
    int16_t a[3]; //raw acc data
    int16_t g[3]; //raw gyro data

    // BMI270 registers (not the complete list)
    typedef enum {
        BMI270_REG_CHIP_ID = 0x00,
        BMI270_REG_ERR_REG = 0x02,
        BMI270_REG_STATUS = 0x03,
        BMI270_REG_ACC_DATA_X_LSB = 0x0C,
        BMI270_REG_GYR_DATA_X_LSB = 0x12,
        BMI270_REG_SENSORTIME_0 = 0x18,
        BMI270_REG_SENSORTIME_1 = 0x19,
        BMI270_REG_SENSORTIME_2 = 0x1A,
        BMI270_REG_EVENT = 0x1B,
        BMI270_REG_INT_STATUS_0 = 0x1C,
        BMI270_REG_INT_STATUS_1 = 0x1D,
        BMI270_REG_INTERNAL_STATUS = 0x21,
        BMI270_REG_TEMPERATURE_LSB = 0x22,
        BMI270_REG_TEMPERATURE_MSB = 0x23,
        BMI270_REG_FIFO_LENGTH_LSB = 0x24,
        BMI270_REG_FIFO_LENGTH_MSB = 0x25,
        BMI270_REG_FIFO_DATA = 0x26,
        BMI270_REG_ACC_CONF = 0x40,
        BMI270_REG_ACC_RANGE = 0x41,
        BMI270_REG_GYRO_CONF = 0x42,
        BMI270_REG_GYRO_RANGE = 0x43,
        BMI270_REG_AUX_CONF = 0x44,
        BMI270_REG_FIFO_DOWNS = 0x45,
        BMI270_REG_FIFO_WTM_0 = 0x46,
        BMI270_REG_FIFO_WTM_1 = 0x47,
        BMI270_REG_FIFO_CONFIG_0 = 0x48,
        BMI270_REG_FIFO_CONFIG_1 = 0x49,
        BMI270_REG_SATURATION = 0x4A,
        BMI270_REG_INT1_IO_CTRL = 0x53,
        BMI270_REG_INT2_IO_CTRL = 0x54,
        BMI270_REG_INT_LATCH = 0x55,
        BMI270_REG_INT1_MAP_FEAT = 0x56,
        BMI270_REG_INT2_MAP_FEAT = 0x57,
        BMI270_REG_INT_MAP_DATA = 0x58,
        BMI270_REG_INIT_CTRL = 0x59,
        BMI270_REG_INIT_DATA = 0x5E,
        BMI270_REG_ACC_SELF_TEST = 0x6D,
        BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
        BMI270_REG_PWR_CONF = 0x7C,
        BMI270_REG_PWR_CTRL = 0x7D,
        BMI270_REG_CMD = 0x7E,
    } Register_e;

    // BMI270 register configuration values
    typedef enum {
        BMI270_VAL_CMD_SOFTRESET = 0xB6,
        BMI270_VAL_CMD_FIFOFLUSH = 0xB0,
        BMI270_VAL_PWR_CTRL = 0x0E,              // enable gyro, acc and temp sensors
        BMI270_VAL_PWR_CONF = 0x02,              // disable advanced power save, enable FIFO self-wake
        BMI270_VAL_ACC_CONF_BWP = 0x01,          // set acc filter in osr2 mode (only in high performance mode)
        BMI270_VAL_ACC_CONF_HP = 0x01,           // set acc in high performance mode
        BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00,    // set gyro filter in OSR4 mode
        BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01,    // set gyro filter in OSR2 mode
        BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02,    // set gyro filter in normal mode
        BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01,  // set gyro in high performance noise mode
        BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode
        BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1
        BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02,  // enable the FIFO watermark interrupt pin 1
        BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A,  // active high, push-pull, output enabled, input disabled 
        BMI270_VAL_FIFO_CONFIG_0 = 0x00,         // don't stop when full, disable sensortime frame
        BMI270_VAL_FIFO_CONFIG_1 = 0x80,         // only gyro data in FIFO, use headerless mode
        BMI270_VAL_FIFO_DOWNS = 0x00,            // select unfiltered gyro data with no downsampling (6.4KHz samples)
        BMI270_VAL_FIFO_WTM_0 = 0x06,            // set the FIFO watermark level to 1 gyro sample (6 bytes)
        BMI270_VAL_FIFO_WTM_1 = 0x00,            // FIFO watermark MSB
    } ConfigValues_e;

    typedef enum {
        GYRO_ODR_25_HZ = 6,
        GYRO_ODR_50_HZ,
        GYRO_ODR_100_HZ,
        GYRO_ODR_200_HZ,
        GYRO_ODR_400_HZ,
        GYRO_ODR_800_HZ,
        GYRO_ODR_1600_HZ,
        GYRO_ODR_3200_HZ
    } gyroOdr_e;

    typedef enum {
        GYRO_RANGE_2000_DPS = 8, //for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well, or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!)
        GYRO_RANGE_1000_DPS = 1,
        GYRO_RANGE_500_DPS = 2,
        GYRO_RANGE_250_DPS = 3,
        GYRO_RANGE_125_DPS = 4
    } gyroRange_e;

    typedef enum {
        ACCEL_ODR_0_78_HZ = 1,
        ACCEL_ODR_1_56_HZ,
        ACCEL_ODR_3_12_HZ,
        ACCEL_ODR_6_25_HZ,
        ACCEL_ODR_12_5_HZ,
        ACCEL_ODR_25_HZ,
        ACCEL_ODR_50_HZ,
        ACCEL_ODR_100_HZ,
        ACCEL_ODR_200_HZ,
        ACCEL_ODR_400_HZ,
        ACCEL_ODR_800_HZ,
        ACCEL_ODR_1600_HZ
    } accelOdr_e;

    typedef enum {
        ACCEL_RANGE_2_G = 0,
        ACCEL_RANGE_4_G,
        ACCEL_RANGE_8_G,
        ACCEL_RANGE_16_G
    } accelRange_e;

    float acc_multiplier;
    float gyro_multiplier;

  private:
    ImuGizmoBMI270() {} //private constructor

  public:
    const char* name() override {return "BMI270";}
    ~ImuGizmoBMI270() {}

    static ImuGizmo* create(ImuConfig *config, ImuState *state) {
        if(!config || !state) return nullptr;

        //check config
        if(config->uses_i2c || !config->spi_bus || config->spi_cs < 0) {
          //Serial.println("IMU: ERROR check config - BMI270 SPI sensor with invalid imu_bus_type, imu_spi_bus and/or pin_imu_cs");
          return nullptr;
        }

        //create gizmo
        auto gizmo = new ImuGizmoBMI270();
        if(!gizmo->sensor_init(config->spi_bus, config->spi_cs, IMU_GYRO_DPS, IMU_ACCEL_G, config->sample_rate_requested)) {
          delete gizmo;
          return nullptr;
        }

        //return config
        config->sample_rate = gizmo->_rate_hz;
        return gizmo;
    }

  private:
    bool sensor_init(SPIClass *spi, const uint8_t csPin, int gyro_scale_dps, int acc_scale_g, int rate_hz) {
      _spi = spi;
      _csPin = csPin;

      set_gyro_scale_dps(gyro_scale_dps);
      set_acc_scale_g(acc_scale_g);
      set_rate(rate_hz);

      pinMode(_csPin, OUTPUT);
      digitalWrite(_csPin, HIGH);
      delay(10);

      // Toggle the CS to switch the device into SPI mode.
      // Device switches initializes as I2C and switches to SPI on a low to high CS transition
      digitalWrite(_csPin, LOW);
      delay(1);
      digitalWrite(_csPin, HIGH);
      delay(10);

      if(who_am_i() != 0x24) return false;

      // Perform a soft reset to set all configuration to default
      // Delay 100ms before continuing configuration
      write_reg(BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET);
      delay(100);

      // Toggle the CS to switch the device into SPI mode.
      // Device switches initializes as I2C and switches to SPI on a low to high CS transition
      digitalWrite(_csPin, LOW);
      delay(1);
      digitalWrite(_csPin, HIGH);
      delay(10);

      bmi270UploadConfig();

      // Configure the accelerometer
      write_reg(BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | accelOdr);

      // Configure the accelerometer full-scale range
      write_reg(BMI270_REG_ACC_RANGE, accelRange);

      // Configure the gyro
      uint8_t GyroOsrMode = BMI270_VAL_GYRO_CONF_BWP_OSR4;
      //uint8_t GyroOsrMode = BMI270_VAL_GYRO_CONF_BWP_OSR2;
      //uint8_t GyroOsrMode = BMI270_VAL_GYRO_CONF_BWP_NORM;        
      write_reg(BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (GyroOsrMode << 4) | gyroOdr);

      // Configure the gyro full-range scale
      write_reg(BMI270_REG_GYRO_RANGE, gyroRange);

      // Interrupt driven by data ready
      write_reg(BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1);

      // Configure the behavior of the INT1 pin
      write_reg(BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE);

      // Configure the device for  performance mode
      write_reg(BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF);

      // Enable the gyro, accelerometer and temperature sensor - disable aux interface
      write_reg(BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL);

      return true;
  }

    int who_am_i() {
      return read_reg(0x00);
    }

    void readSensor(void) {
        uint8_t data[12];
        spi_read(0x0C, data, 12);
        a[0] = data[1]<<8 | data[0];
        a[1] = data[3]<<8 | data[2];
        a[2] = data[5]<<8 | data[4];
        g[0] = data[7]<<8 | data[6];
        g[1] = data[9]<<8 | data[8];
        g[2] = data[11]<<8 | data[10];
    }

  public:
    //Get sensor data in NED frame
    //x=North (forward), y=East (right), z=Down 
    //acc: gravitation force is positive in axis direction (i.e. at rest az = 1)
    //gyro: direction of positive rotation by right hand rule, i.e. positive is: yaw right, roll right, pitch up
    void getMotion6NED(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) override
    {
      readSensor();
      //BM270 sensor orientation for gyro is NWU
      //BM270 sensor orientation for acc  is SED (i.e. at rest az = 1)
      *ax = -a[0] * acc_multiplier;  //North = -South
      *ay = a[1] * acc_multiplier;   //East  = East
      *az = a[2] * acc_multiplier;   //Down  = Down
      *gx = g[0] * gyro_multiplier;  //North = North
      *gy = -g[1] * gyro_multiplier; //East  = -West
      *gz = -g[2] * gyro_multiplier; //Down  = -Up
    }

    //set maximum rate such that actual rate <= requested rate
    void set_rate(int rate_hz)
    {
      if(rate_hz >= 3200) {
        accelOdr = ACCEL_ODR_1600_HZ;
        gyroOdr = GYRO_ODR_3200_HZ;
        _rate_hz = 1600;
      }if(rate_hz >= 1600) {
        accelOdr = ACCEL_ODR_1600_HZ;
        gyroOdr = GYRO_ODR_1600_HZ;
        _rate_hz = 1600;
      }else if(rate_hz >= 800) {
        accelOdr = ACCEL_ODR_800_HZ;
        gyroOdr = GYRO_ODR_800_HZ;
        _rate_hz =  800;
      }else if(rate_hz >= 800) {
        accelOdr = ACCEL_ODR_400_HZ;
        gyroOdr = GYRO_ODR_400_HZ;
        _rate_hz =  400;
      }else if(rate_hz >= 200) {
        accelOdr = ACCEL_ODR_200_HZ;
        gyroOdr = GYRO_ODR_200_HZ;
        _rate_hz =  200;
      }else{
        accelOdr = ACCEL_ODR_100_HZ;
        gyroOdr = GYRO_ODR_100_HZ;
        _rate_hz =  100;       
      }
    }

  private:
    void set_acc_scale_g(int scale_in_g)
    {
      if(scale_in_g <= 2) {
        accelRange = ACCEL_RANGE_2_G;
        acc_multiplier = 2.0 / 32768.0;
      }else if(scale_in_g <= 4) { 
        accelRange = ACCEL_RANGE_4_G;
        acc_multiplier = 4.0 / 32768.0;
      }else if(scale_in_g <= 8) { 
        accelRange = ACCEL_RANGE_8_G;
        acc_multiplier = 8.0 / 32768.0;
      }else{ 
        accelRange = ACCEL_RANGE_16_G;
        acc_multiplier = 16.0 / 32768.0;
      }
    }

    void set_gyro_scale_dps(int scale_in_dps)
    {
      if(scale_in_dps <= 125) {
        gyroRange = GYRO_RANGE_125_DPS;
        gyro_multiplier = 125.0 / 32768.0;
      }else if(scale_in_dps <= 250) {
        gyroRange = GYRO_RANGE_250_DPS;
        gyro_multiplier = 250.0 / 32768.0;
      }else if(scale_in_dps <= 500) { 
        gyroRange = GYRO_RANGE_500_DPS;
        gyro_multiplier = 500.0 / 32768.0;
      }else if(scale_in_dps <= 1000) { 
        gyroRange = GYRO_RANGE_1000_DPS;
        gyro_multiplier = 1000.0 / 32768.0;
      }else{ 
        gyroRange = GYRO_RANGE_2000_DPS;
        gyro_multiplier = 2000.0 / 32768.0;
      }
    }

  private:
    SPIClass * _spi;
    uint8_t _csPin;
    int _rate_hz;
    accelRange_e accelRange = ACCEL_RANGE_16_G;
    accelOdr_e accelOdr = ACCEL_ODR_100_HZ;
    gyroRange_e gyroRange = GYRO_RANGE_2000_DPS;
    gyroOdr_e gyroOdr = GYRO_ODR_100_HZ;

    void bmi270UploadConfig()
    {
        write_reg(BMI270_REG_PWR_CONF, 0);
        write_reg(BMI270_REG_INIT_CTRL, 0);

        // Transfer the config file
        spi_write(BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file));

        delay(10);
        write_reg(BMI270_REG_INIT_CTRL, 1);
    }

    int8_t spi_read(const uint8_t reg, uint8_t * data, const uint32_t count) {
        _spi->beginTransaction(SPISettings(spi_freq, MSBFIRST, SPI_MODE3));
        digitalWrite(_csPin, LOW);
        _spi->transfer(0x80 | reg);
        _spi->transfer(0); //dummy byte --- BMI270 specific ---
        for (uint32_t k=0; k<count; k++) {
            data[k] = _spi->transfer(0);
        }
        digitalWrite(_csPin, HIGH);
        _spi->endTransaction();
        return 0;
    }

    int8_t spi_write(uint8_t reg, const uint8_t *data, uint32_t count) {
        _spi->beginTransaction(SPISettings(spi_freq, MSBFIRST, SPI_MODE3));
        digitalWrite(_csPin, LOW);
        _spi->transfer(reg);
        for (uint32_t k=0; k<count; k++) {
            _spi->transfer(data[k]);
        }
        digitalWrite(_csPin, HIGH);
        _spi->endTransaction();
        return 0;
    }


    void write_reg(uint8_t reg, uint8_t data) {
      spi_write(reg, &data, 1);
      delay(1);
    }

    uint8_t read_reg(uint8_t reg) {
      uint8_t data;
      spi_read(reg, &data, 1);
      delay(1);
      return data;
    }

};
