#include "ICM20948.h"
#include "i2C.h"
#include "MathFunc.h"
#include "Biquad.h"
#include <cmath>

static BiquadFilter FiltAcc[3];
static BiquadFilter FiltGyr[3];

static IMU_Proc IMU_DoneProc;

static long GyroShift[3]{0, 0, 0};

struct IMU_DataBuffer
{
  short ax, ay, az, gx, gy, gz, temp, mx, my, mz;
};

static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax);
static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size);

static unsigned char IMU_Buffer[32];
static I2C_Request IMU_Device = {&IMU_CallbackProc, IMU_Buffer, sizeof(IMU_Buffer), 0 };

void ICM20948::Init()
{
  lpf2p_init(&FiltAcc[0], 500.0f, 25.0f);
  lpf2p_init(&FiltAcc[1], 500.0f, 25.0f);
  lpf2p_init(&FiltAcc[2], 500.0f, 25.0f);
  
  lpf2p_init(&FiltGyr[0], 500.0f, 25.0f);
  lpf2p_init(&FiltGyr[1], 500.0f, 25.0f);
  lpf2p_init(&FiltGyr[2], 500.0f, 25.0f);
  
  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
  
  IMU_SetReg(IMU_Addr, 0x06, 0x80);
  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
  
  IMU_SetReg(IMU_Addr, 0x06, 0x01);
  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
  
  IMU_SetBank(2);
  
  IMU_SetReg(IMU_Addr, 0x01, GYRO_DLPFCFG_73 | GYRO_FS_SEL_2000 | GYRO_FCHOICE_ON); // GYRO_CONFIG_1
  IMU_SetReg(IMU_Addr, 0x14, ACCEL_DLPFCFG_69 | ACCEL_FS_SEL_8 | ACCEL_FCHOICE_ON); // ACCEL_CONFIG
  
  IMU_SetBank(0);
  
  for (int a = 0; a < 100000; a++) { asm volatile("NOP"); }
}

void ICM20948::IMU_SetBank(unsigned char Bank)
{
  unsigned char reg[2];
  reg[0] = 0x7F; reg[1] = Bank << 4;
  I2C1_Write(IMU_Addr, reg, 2);
  I2C1_Stop();
}

void ICM20948::Get(IMU_Data& Data)
{
  struct { short ax; short ay; short az; short gx; short gy; short gz; short temp; short mx; short my; short mz; } data;
  
  I2C1_Write(IMU_Addr, IMU_Read_Reg);
  I2C1_Read(IMU_Addr, &data, sizeof(data));
  I2C1_Stop();
  
  Data.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735;
  
  Data.Acc.X = Rev16(data.ax);
  Data.Acc.Y = Rev16(data.ay);
  Data.Acc.Z = Rev16(data.az);
  
  Data.Gyr.X = Rev16(data.gx);
  Data.Gyr.Y = Rev16(data.gy);
  Data.Gyr.Z = Rev16(data.gz);

  Data.Mag.X = data.mx;
  Data.Mag.Y = data.my;
  Data.Mag.Z = data.mz;
}

void ICM20948::GetAsync(const IMU_Proc& DoneProc)
{
  IMU_DoneProc = DoneProc;
  I2C1_Trans(&IMU_Device, IMU_Addr, &IMU_Read_Reg, 1, sizeof(IMU_DataBuffer));
}


static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size)
{
  IMU_DataBuffer& data = *(IMU_DataBuffer*)Data;
  
  data.ax = Rev16(data.ax);
  data.ay = Rev16(data.ay);
  data.az = Rev16(data.az);

  data.gx = Rev16(data.gx);
  data.gy = Rev16(data.gy);
  data.gz = Rev16(data.gz);
  
  data.ax = biquad_apply(&FiltAcc[0], data.ax);
  data.ay = biquad_apply(&FiltAcc[1], data.ay);
  data.az = biquad_apply(&FiltAcc[2], data.az);

  data.gx = biquad_apply(&FiltGyr[0], data.gx);
  data.gy = biquad_apply(&FiltGyr[1], data.gy);
  data.gz = biquad_apply(&FiltGyr[2], data.gz);
  
  static IMU_DataBuffer test = *(IMU_DataBuffer*)Data;
  
  if (CalibDataIMU.AllowedCalib) IMU_Calibrate(data, 50, 4096, 500);
  
  static IMU_Data result;
  
  static float alpha = 0.01f;
  
  result.RawAcc.X = (result.RawAcc.X * (1.0f - alpha)) + test.ax * alpha;
  result.RawAcc.Y = (result.RawAcc.Y * (1.0f - alpha)) + test.ay * alpha;
  result.RawAcc.Z = (result.RawAcc.Z * (1.0f - alpha)) + test.az * alpha;

  result.RawGyr.X = (result.RawGyr.X * (1.0f - alpha)) + test.gx * alpha;
  result.RawGyr.Y = (result.RawGyr.Y * (1.0f - alpha)) + test.gy * alpha;
  result.RawGyr.Z = (result.RawGyr.Z * (1.0f - alpha)) + test.gz * alpha;
  
  result.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735; // 21+(temperature / 333.87)
  
  result.Acc.X = Normalize(data.ax, 1.0f, CalibDataIMU.Data.Acc.X[0], CalibDataIMU.Data.Acc.X[1]);
  result.Acc.Y = Normalize(data.ay, 1.0f, CalibDataIMU.Data.Acc.Y[0], CalibDataIMU.Data.Acc.Y[1]);
  result.Acc.Z = Normalize(data.az, 1.0f, CalibDataIMU.Data.Acc.Z[0], CalibDataIMU.Data.Acc.Z[1]);

  result.Gyr.X = ((float)(data.gx - GyroShift[0])) / 16.384f;
  result.Gyr.Y = ((float)(data.gy - GyroShift[1])) / 16.384f;
  result.Gyr.Z = ((float)(data.gz - GyroShift[2])) / 16.384f;
  
  IMU_DoneProc(result);
}

static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax)
{
  long len = imu.gx * imu.gx + imu.gy * imu.gy + imu.gz * imu.gz;
  if (len > GyrLim * GyrLim) return;
  
  static float x = imu.gx, y = imu.gy, z = imu.gz;
  
  const float alpha = 0.001f;
  
  x = x * (1.0f - alpha) + alpha * imu.gx;
  y = y * (1.0f - alpha) + alpha * imu.gy;
  z = z * (1.0f - alpha) + alpha * imu.gz;
  
  GyroShift[0] = x;
  GyroShift[1] = y;
  GyroShift[2] = z;
  
  len = imu.ax * imu.ax + imu.ay * imu.ay + imu.az * imu.az;
  if (abs(len - AccZero * AccZero) > (AccMax * AccMax)) return;
  
  len = sqrtf(len);
  
  static float acc = len;
  
  acc = acc * (1.0f - alpha) + alpha * len;
}