#include "stm32g4xx.h"
#include "Tick.h"
#include "Tim.h"
#include "IIMU.h"
#include "ICM20948.h"
#include "IRS.h"
#include "Vector.h"
#include "Autopilot.h"

extern "C" void SystemClock_Config();

constexpr long TIM_PRIORITY = 2;

IMU_Data DataIMU;

IRS MainIRS;

IIMU* IMUObj;

float GetCurrentTime()
{
  return ((float)TICK_GetCount()) / 1000.0f;
}

void DoneProcIMU(IMU_Data& Data)
{
  DataIMU = Data;
  // MagReady=true;
}

void TimerUpdateSensor()
{
  //IMUObj->GetAsync(DoneProcIMU);
  if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
  //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
}

void TimerUpdateMain()
{
  float time = GetCurrentTime();
  
  Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
  Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
  
  // Vector3 mag;
  // if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
  // else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
  
  MainIRS.UpdateGyro(gyr);
  MainIRS.UpdateAccel(acc);
  /*if(MagReady)
  {
    MagReady=false;
    MainIRS.UpdateMagnet(mag);
  }
  
  MainGPS.UpdateAverage(time);
  MainBar.UpdateAverage();
  MainOF.UpdateAverage(IRS::Period);
  MainLen.UpdateAverage();
  
  if(DataFLOW.Update)
  {
    DataFLOW.Update=false;
    UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
  }
  
  if(DataTOF.Update)
  {
    DataTOF.Update=false;
    UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
  }
  
  UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
  
  if(DataGPS.Update)
  {
    DataGPS.Update=false;
    Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
    UpdateGPS(gps, time, DataGPS.Valid);
  }*/
  
  Vector3 pos;
  MainIRS.RestoreAllShift(pos);
  /*MainVel.PointBegin-=pos;
  
  AutoControl(gyr);
  
  ReadStateINS();
  
  if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
}

int main()
{
  SystemClock_Config(); // 170MHz
  IMU_InitPower();
  
  TICK_Init();
  
  TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
  
  IMUObj = TryFindIMU(MainDrone.IMUInit);
  
  TIM6_Enable();
  
  while (true)
  {
    
  }
}