#include <math.h>
#include <stdio.h>
#include <string.h>

#include "stm32g4xx.h"

#include "gpio.h"
#include "led.h"
#include "ori.h"
#include "bar.h"
#include "eep.h"
#include "tim.h"
#include "pwm.h"
#include "tele.h"
#include "med.h"
#include "tick.h"
#include "ibus.h"
#include "iwdg.h"
#include "pid.h"
#include "uart.h"
#include "gps.h"
#include "mag.h"
#include "tof.h"
#include "flow.h"

#include "imu.h"

#include "mot.h"
#include "move.h"

#include "prot.h"


static const float PI = 3.14159265359f;
static const float TO_RAD = PI/180.0f;

#define TELE_TIMER 10 // ms
#define LED_TIMER 100 // ms
#define JOY_TIMER 200 // ms

#define LOG_TIMER 5 // ms

#define PWM_FREQ 100 // Hz

#define MAIN_UPDATE_FREQ 100 // Hz
#define MAIN_UPDATE_PRIORITY 2

PID_Data PID_X = { -1000, 1000, {-500, 500, 2.5}, {-50, 50, 0.0}, {-500, 500, 0.010} };
PID_Data PID_Y = { -1000, 1000, {-500, 500, 2.5}, {-50, 50, 0.0}, {-500, 500, 0.010} };
PID_Data PID_Z = { -1000, 1000, {-500, 500, 3}, {-50, 50, 0.0}, {-500, 500, 0.02}  };

short Joy_throt = 0; // Gass
short Joy_pitch = 0; // Y
short Joy_roll = 0;  // X
short Joy_yaw = 0;   // Z

short Main_PWM[4]{ 0, 0, 0, 0 };

float Main_dXYZ[3]={0, 0, 0};
bool MainValid=false;

IBUS_Data Joypad;
bool JoypadOff;

IMU_Data DataIMU;
BAR_Data DataBAR;
MAG_Data DataMAG;

bool imu_ready=false, bar_ready=false, mag_ready=false;

void DoneProcIMU(IMU_Data& Data)
{
  DataIMU=Data;
  imu_ready=true;
  if(bar_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}

void DoneProcBAR(bool Ready, BAR_Data& Data)
{
  if(!Ready) { BAR_GetAsunc(DoneProcBAR); return; }
  DataBAR=Data;
  bar_ready=true;
  if(imu_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}

void DoneProcMAG(MAG_Data& Data)
{
  DataMAG=Data;
  mag_ready=true;
  if(bar_ready && imu_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}

short test_pow[3];

Point DataGPS{0,0,0};

short test_p=0, test_r=0;

void Main_Update()
{
  if(!bar_ready) BAR_GetAsunc(DoneProcBAR);
  if(!imu_ready) IMU_GetAsunc(DoneProcIMU);
  //if(!mag_ready) MAG_GetAsunc(DoneProcMAG);

  //if(!imu_ready || !bar_ready || !mag_ready) return;
  if(!imu_ready || !bar_ready) return;
  
  
  
  PingIWDG();
  
  imu_ready=false;
  bar_ready=false;
  mag_ready=false;
  
  DataIMU.Mag={DataMAG.X, DataMAG.Y, DataMAG.Z};
  
  ORI_Get(DataORI, MAIN_UPDATE_FREQ, DataIMU, DataBAR);
  
  static Vec3 flow;
  
  static long timer=0;
  if(timer>=4)
  {
    float range=TOF_GetRange();
    short dx, dy;
    unsigned char qality;
    bool move=FLOW_GetMotion(&dx, &dy, &qality);
    flow=Motion({DataORI.SinX, DataORI.SinY, DataORI.CosZ}, move, dx, dy, range, 0);
    timer=0;
    
    if(range<0.15f) 
    {
      Main_StatusMode=STATUS_MODE::on_ground;
      Main_SysMode=SYS_MODE::hold;
    }
  }
  else timer++;
  
  short yaw=(DataORI.Yaw-Joy_yaw)+180;
  if(yaw<0) yaw+=360;
  if(yaw>360) yaw-=360;
  static short pov_z;
  pov_z = (short)(PID_Update(180, yaw, PID_Z, DataORI.Gyr.Z));
  
  static float height;
  static Point zero;
  
  
  float altitude;
  Point coord;
  bool valid;
  bool ready;
  
  ready=GPS_GetCoordinates(DataGPS, valid);
  valid=1;
  
  /*MainValid=(last && valid);
  
  if(MainValid) 
  {
    if(Joypad.SWB>1500) { zero=coord; height=altitude; ready=true; }
    GPS_LocalDistance(zero, coord, Main_dXYZ[0], Main_dXYZ[1]);
    Main_dXYZ[2]=height-altitude;
  }*/
  
  //GPS_Navigation(MainValid, DataORI, Main_dXYZ);
  
  //if(Joypad.SWA<1500 ) return;
  
  static float heigh=0, heigh_g=0, heigh_z=0;
  
  if(Joypad.SWA<1500)
  {
    PWM_SetAll(900);
    Base_BeginXYZ=DataGPS;
    return;
  }
  
  if(Joy_throt<1050)
  {
    PWM_SetAll(900);
    return;
  }
  
  static bool autopilot=false;
  
  static short throt;
  throt = Joy_throt-1000;
  
  static short pitch=0, roll=0;
  
  
  
  if(Joypad.X>1550 || Joypad.X<1450 || Joypad.Y>1550 || Joypad.Y<1450) autopilot=false;
  
  static float mx=0, my=0;
  
  if(Joypad.SWD>1500)
  {
    MainFlyMode=MAIN_FLY_MODE::STOP;
    
    /*if(Joypad.SWB>1500)
    {
      MainFlyMode=MAIN_FLY_MODE::STOP;
    }
    else
    {
      //PointGPS=DataGPS;
      //PointGPS.Altitude+=3;
    }*/
    
    if(MainFlyMode==MAIN_FLY_MODE::WAIT)
    {
      if(autopilot==true) { PWM_SetAll(900); return; }
    }
    
    if(MainFlyMode==MAIN_FLY_MODE::START)
    {
      if(autopilot==true) { PWM_SetAll(1100); return; }
    }
    
    if(MainFlyMode==MAIN_FLY_MODE::STOP)
    {
      Main_StatusMode=STATUS_MODE::landing;
      Main_SysMode=SYS_MODE::land;
      
      if(autopilot==true)
      {
        //throt=500;
        
        float shift=0.5f+flow.z;
        
        float fx=-flow.x*shift;
        float fy=-flow.y*shift;
        
        mx+=fx;
        my+=fy;
        
        pitch=my*0.005f;
        roll=mx*0.005f;
        
        pitch+=fy*0.4f;
        roll+=fx*0.4f;
        
        if(pitch>+20) pitch=+20;
        if(pitch<-20) pitch=-20;
        
        if(roll>+20) roll=+20;
        if(roll<-20) roll=-20;
        
        Joy_pitch=pitch;
        Joy_roll=roll;
        
        static long add_power=0;
        add_power=(heigh-flow.z*1000.0f)*0.1f;
        
        if(add_power>+500) add_power=+500;
        if(add_power<-500) add_power=-500;
        
        throt+=add_power;
        
        static int count=0;
        static float last_h=0, delta_h=0;
        if(count>=9)
        {
          delta_h=(last_h-flow.z*1000.0f);
          
          last_h=flow.z*1000.0f;
          
          count=0;
        }
        else count++;
        
        if(delta_h>+200) delta_h=+200;
        if(delta_h<-200) delta_h=-200;
        
        throt+=delta_h*1.0f;
        
        if(throt>800) throt=800;
        if(throt<100) throt=100;
        

        if(Joypad.SWB>1500)
        {
          if(flow.z>0.3f) 
            heigh-=3.0f;
          else 
            heigh-=6.0f;
        }
        if(heigh<-1000) 
        {
          heigh=-1000;
          MainFlyMode=MAIN_FLY_MODE::START;
        }
      }
    }
    else 
    {
      heigh=flow.z*1000.0f;
      mx=0; my=0;
    }
    
    if(MainFlyMode==MAIN_FLY_MODE::FLY)
    {

      Main_StatusMode=STATUS_MODE::fly;
      Main_SysMode=SYS_MODE::auto_mode;
      
      if((autopilot==true) && (ready && valid))
      {
        //throt=500;
        
        static float last_a=0, delta_a=0;
        long power=(PointGPS.Altitude-DataGPS.Altitude)*100.0f;
        
        
        
        static int count=0;
        
        if(count>1)
        {
          delta_a=(DataGPS.Altitude-last_a);
          
          last_a=DataGPS.Altitude;
          
          count=0;
        }
        else count++;
        
        power+=delta_a*50.0f;
        
        if(power>+500) power=+500;
        if(power<-500) power=-500;
        
        //throt+=power;
        
        //if(throt<100) throt=100;
      }
      
      if((autopilot==true) && (ready && valid))
      {
        static float delta_x=0, delta_y=0;
        static float ldx=0, ldy=0;
        
        float dx, dy;
        
        GPS_LocalDistance(PointGPS, DataGPS, dx, dy);
        
        dx=dx;
        dy=-dy;
        
        
        pitch=dy*10.0f;
        roll=dx*10.0f;
        
        if(pitch>+20) pitch=+20;
        if(pitch<-20) pitch=-20;
        
        if(roll>+20) roll=+20;
        if(roll<-20) roll=-20;
        
        static int count=0;
        
        if(count>1)
        {
          delta_x=(delta_x+(dx-ldx))/2.0f;
          delta_y=(delta_y+(dy-ldy))/2.0f;
          
          ldx=dx;
          ldy=dy;
          
          count=0;
        }
        else count++;
        
        //pitch+=delta_y*10.0f;
        //roll+=delta_x*10.0f;
        
        if(pitch>+20) pitch=+20;
        if(pitch<-20) pitch=-20;
        
        if(roll>+20) roll=+20;
        if(roll<-20) roll=-20;
        
        test_p=pitch;
        test_r=roll;
        
        //Vec3 v = RotateToZ({(float)pitch, (float)roll, 0}, false);
        //pitch=v.x; roll=v.y;
        
        Joy_pitch=pitch;
        Joy_roll=roll;
      }
      
      if(autopilot==true)
      {
        Joy_pitch=test_p;
        Joy_roll=test_r;
      }
    }
  }
  else
  {
    mx=0; my=0;
    heigh=flow.z*1000.0f;
    MainFlyMode=MAIN_FLY_MODE::WAIT;
    autopilot=true;
  }
  
  short pov_x = (short)(PID_Update(Joy_pitch, DataORI.Pitch, PID_X, -DataORI.Gyr.X));
  short pov_y = (short)(PID_Update(Joy_roll,  DataORI.Roll,  PID_Y, -DataORI.Gyr.Y));
  
  //throt+=pov_h;
  
  //float stream=DataORI.Tilt;
  //if(stream>25) stream=25;
  //throt*=1.0f/cosf(stream*TO_RAD);
  if(throt>1000) throt=1000;
  
  QUAD_Set3(throt, pov_x, pov_y, pov_z);
}
//------------------------------------------------------------------------------

int main()
{
  //InitIWDG(1000);
  
  GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT); // POWER ON 3V3
  
  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  DWT->CYCCNT = 0;
  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
  
  //asm("BKPT 0");
  
  PWM_Init(PWM_FREQ); // 100Hz
  PWM_SetAll(900);
  
  LED_Init();
  ORI_Init();
  BAR_Init();
  EEP_Init();
  TIM7_Init(MAIN_UPDATE_PRIORITY);
  //TELE_Init();
  TICK_Init();
  IBUS_Init(115200); // 115200
  GPS_Init();
  //MAG_Init();
  TOF_Init();
  LPUART1_Init(57600);
  
  FLOW_Init();
  
  
  //EEP_Write(0, "Hello", 6);
  char data[32];
  EEP_Read(2, data, 32);

  //LED_Red_Set(true);

  //InitIWDG(300);
  TIM7_Update(MAIN_UPDATE_FREQ*2, Main_Update); 
  
  while(true)
  {
    
    GPS_Update();
    TOF_Update2();
    
    PROT_Update();
    
    const unsigned long tick=TICK_GetCount();
    
    static unsigned long joy_tick=tick;
    if(IBUS_Update(Joypad, JoypadOff))
    {
      joy_tick=tick;
      if(Joypad.SWA>1500)
      {
        Joy_pitch = (1500 - Joypad.Y)/16;
        Joy_roll = (Joypad.X - 1500)/16;
        Joy_yaw = (Joypad.W - 1500)/16;
        
        if (Joypad.Z < 1050) Joy_throt = 1000;
        else if (Joypad.Z >= 1050 && Joypad.Z <= 2000)
        {
          Joy_throt = Joypad.Z;
        }
      }
      else
      {
        PWM_SetAll(900);
      }
    }
    
    if((tick-joy_tick>JOY_TIMER) || JoypadOff)
    {
      PWM_SetAll(900);
    }
    
    static unsigned long led_tick=0;
    static bool led=false;
    if(tick-led_tick>LED_TIMER)
    {
      led_tick=tick;
      LED_Set(led=!led);
      
    }
    
    //short info[]={DataORI.Acc.X, DataORI.Acc.Y, DataORI.Acc.Z, DataORI.Acc.Sum, DataORI.Gyr.X, DataORI.Gyr.Y, DataORI.Gyr.Z, (short)DataORI.Pitch, (short)DataORI.Roll, (short)DataORI.Yaw};
    //TELE_Update(info, sizeof(info), TELE_TIMER);
    
    static int Fixed=0;
    
    static unsigned long log_tick=0;
    if(tick-log_tick>TELE_TIMER)
    {
      log_tick=tick;
      
      float info[]={DataORI.Bar.Acc, DataORI.Speed.Z, DataORI.Bar.Speed};
      UART2_Send("Z", 1);
      UART2_Send(info, sizeof(info));
      UART2_Send("V", 1);
      
      static char buffer[128];
      //int len=sprintf(buffer, "Z:%d,%d,%d,%d,%d,%d,%d#\n", Fixed, (int)DataORI.Pitch, (int)DataORI.Roll, (int)DataORI.Yaw, (int)(DataORI.Iner.X*1000), (int)(DataORI.Iner.Y*1000), (int)(DataORI.Iner.Z*1000)); 

      //if(len>0) LPUART1_Send(buffer, len);
      
      //int len=sprintf(buffer, "G:0,%4.3f,%4.8f,%4.8f#\n", DataGPS.Altitude, DataGPS.Coord.Latitude, DataGPS.Coord.Longitude);
      
      //int len=sprintf(buffer, "G:0,%d,%d\n", test_p, test_r);
      
      //if(len>0) LPUART1_Send(buffer, len);
      
      Fixed++;
    }
  }
}
//------------------------------------------------------------------------------