#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 "sbus.h"
#include "iwdg.h"
#include "pid.h"
#include "uart.h"
#include "gps.h"

#include "laser.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, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
PID_Data PID_Y = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
PID_Data PID_Z = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.03}  };

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;
SBUS_Data Joypad;
bool JoypadOff;

IMU_Data DataIMU;
BAR_Data DataBAR;
MAG_Data DataMAG = {0,0,0};


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



SBUS_Data rc;
unsigned char fs;

short PWM_NONE = 0;


int  m= 0;
unsigned short dist_mm;
      uint16_t dist ;
        float meters;

    short dx, dy;
    unsigned char qality;
 

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



short test_pow[3];

Point DataGPS{0,0,0};

short test_p=0, test_r=0;

Vec3 test_iner={0,0,0};

float range = 0;

 void Main_Update()
{
 IMU_Get(DataIMU);
      
   DataIMU.Bar=BAR_GetData(&DataIMU.Tmp);
  //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)
  {
     if (isRangeReady())
    {
     range = getRange();
    }

    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>JOY_MID) { 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<JOY_MID ) return;
  
  static float heigh=0, heigh_g=0, heigh_z=0;
  
  if(Joypad.SWA<JOY_MID)
  {
    PWM_SetAll(PWM_NONE);
    Base_BeginXYZ=DataGPS;
    return;
  }
  
  if(Joy_throt<JOY_MIN+50)
  {
    PWM_SetAll(PWM_NONE);
    return;
  }
  
  static bool autopilot=false;
  
  static short throt;
  throt = (Joy_throt-JOY_MIN)/JOY_VAL;
  
  static short pitch=0, roll=0;
  
  
  
  if(Joypad.X>JOY_MID+50 || Joypad.X<JOY_MID-50 || Joypad.Y>JOY_MID+50 || Joypad.Y<JOY_MID-50) autopilot=false;
  
  static float mx=0, my=0;
  
  if(Joypad.SWD>JOY_MID)
  {
    MainFlyMode=MAIN_FLY_MODE::STOP;
    
    /*if(Joypad.SWB>JOY_MID)
    {
      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;
        
        test_iner.x=mx/1000;
        test_iner.y=my/1000;
        test_iner.z=flow.z;
        
        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>JOY_MID)
        {
          if(flow.z>0.3f) 
            heigh-=3.0f;
          else 
            heigh-=10.0f;
        }
        if(heigh<-5000) 
        {
          heigh=-5000;
          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_Set(throt, pov_x, pov_y, pov_z); //!!!!!!!!!!!!!!!!
}
//------------------------------------------------------------------------------
 
 float poww=0;
 bool isGoZero = false;
 
 
int main()
{
  //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); //???
   PWM_SetAll(PWM_NONE);//???
   
   LED_Init();
   
   
   ORI_Init();
   BAR_Init();
   
 //  EEP_Init();
   TIM7_Init(MAIN_UPDATE_PRIORITY);
  TICK_Init();
  SBUS_Init(); 
  Laser_Init_v2(); 
  UART2_Init(57600);//LPUART1_Init
  
  FLOW_Init();
  //EEP_Write(0, "Hello", 6);
  //char data[32];
  //EEP_Read(2, data, 32);
  
    TIM7_Update(MAIN_UPDATE_FREQ*2, Main_Update); 

   //174 1811
  while(true)
  {
   // GPS_Update();
   
    /*
     if(SBUS_Update(Joypad, JoypadOff))
     {
         poww = (Joypad.Z-174)*700/(1811-174);
        PWM_SetAll(PWM_NONE);
     }
    */
    
    
    PROT_Update();
    
    const unsigned long tick=TICK_GetCount();
    
    static unsigned long joy_tick=tick;
    
    


  //--------------------------------------------------
    
    if(SBUS_Update(Joypad, JoypadOff))
    {
      joy_tick=tick;
      if(Joypad.SWA>JOY_MID)
      {
        if (!isGoZero)
        {
          GoZero();
          isGoZero = true;
        }
        Joy_pitch = (JOY_MID_PITCH - Joypad.Y)/16;
        Joy_roll = (Joypad.X - JOY_MID_ROLL)/16;
        Joy_yaw = (Joypad.W - JOY_MID_YAW)/16;
        
        if (Joypad.Z < JOY_MIN+50) Joy_throt = JOY_MIN;
        else if (Joypad.Z >= JOY_MIN+50 && Joypad.Z <= JOY_MAX)
        {
          Joy_throt = Joypad.Z;
        }
      }
      else
      {
        PWM_SetAll(PWM_NONE);
             isGoZero = false;
      }
    }
   
    //--------------------------------------------------
    if((tick-joy_tick>JOY_TIMER) || JoypadOff)
    {
      PWM_SetAll(PWM_NONE);
    }
    
    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++;
    }
  }
}
//------------------------------------------------------------------------------