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

#define PI            3.14159265359f

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

Vector3 QuatToEuler(const Quaternion& q)
{
  Vector3 e;
  
  e.X = atan2f(2*(q.W*q.X + q.Y*q.Z), 1 - 2*(q.X*q.X + q.Y*q.Y));

  e.Y = asinf(2*(q.W*q.Y - q.Z*q.X));

  e.Z = atan2f(2*(q.W*q.Z + q.X*q.Y), 1 - 2*(q.Y*q.Y + q.Z*q.Z));

  e.X *= 180.0f / PI;
  e.Y *= 180.0f / PI;
  e.Z *= 180.0f / PI;

  return e;
}

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

Vector3 Euler;

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);
  
  Euler = QuatToEuler(MainIRS.OriQuat);
  
  /*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);*/
}

static bool control_update_flag = 0;

int main()
{
  SystemClock_Config(); // 170MHz
  IMU_InitPower();
  
  TICK_Init();
  
  TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
  
  IMUObj = TryFindIMU(MainDrone.IMUInit);
  
  TIM6_Enable();
  
  attitude_t attitude;
  rc_channels rx_chs_raw;
  rc_channels rx_chs_normalized;
  control_channels_t ctrl_chs;
  
  attitude_init(&attitude);
  receiver_init();
  motors_init();
  
  while (true)
  {
    receiver_update(&rx_chs_raw);
	  rx_chs_normalized = normalize_channels(rx_chs_raw);
    
    if (control_update_flag)
		{
      control_update_flag = 0;
      
      Vector3 Gyro = Vector3(DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z);
      
      attitude_controller_update(
        &ctrl_chs,
        &rx_chs_normalized,
        &MainIRS.OriQuat,
        &Gyro
      );
		}
		
		if (rx_chs_normalized.rc_armed)
	  {
      CalibDataIMU.AllowedCalib = false;
      
      motors_set_throttle_mix(
        rx_chs_normalized.rc_throttle,
        &ctrl_chs,
        rx_chs_normalized.rc_armed
      );
	  }
	  else
		{
			motors_turn_off();
			CalibDataIMU.AllowedCalib = true;
		}
  }
}