#include "IIMU.h"
#include "I2C.h"
#include "ICM20948.h"

IMU_Info IMUInfo;

IMU_Calib_Data CalibDataIMU;

void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value)
{
  unsigned char reg[2];
  reg[0] = Reg; reg[1] = Value;
  I2C1_Write(Addr, reg, 2);
  I2C1_Stop();
}

unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg)
{
  I2C1_Write(Addr, Reg);
  I2C1_Read(Addr, &Reg, 1);
  I2C1_Stop();
  return Reg;
}

IIMU* TryFindIMU(bool& find)
{
  I2C1_Init();
  IIMU* Imu = nullptr;
  for (int i = 0; i < sizeof(IMUInfo.Addrs); ++i)
  {
    find = I2C1_CheckDeviceWhoAmI(IMUInfo.Addrs[i], IMUInfo.WhoIAMReg[i], IMUInfo.ExpectedIDs[i]);
    if (find && i == 0)
    {
      static ICM20948 imuICM20948;
      Imu = (IIMU*)&imuICM20948;
      break;
    }
    for (int a = 0; a < 10000000; ++a) { asm volatile("NOP"); }
  }
  if (find) Imu->Init();
  return Imu;
}