#include "config.h"
#include "data.hpp"

int flight_mode   = 0; 
int dep_altitude  = 0; 
int sep_altitude = 0; 
int prev_flight_mode = 0; 
bool initialised  = false;
bool flight_started = false;
bool deployed     = false; 
bool wing_opened  = false; 
bool spiral = false;
bool separation = false;
unsigned long spiral_time = 0; 
unsigned long init_time = 0; 

float setPoint_waypoint = 0; 
float error_waypoint    = 0; 
float cmd_to_waypoint   = 0;


//------------------- 0 -------------------//

void flight_init() { 
  
  if ((gps.satellites.value()>=6 and gps_ok and gps.hdop.value()<150 and millis()>5000) or NO_INIT) {

    EasyBuzzer.beep(3000,100,50,10,500,1);

    if ((NAV_WAYPOINT == true) and (NAV_HOME == true)) {
      waypoint[last_waypoint_number].latitude = gps.location.lat();
      waypoint[last_waypoint_number].longitude = gps.location.lng();
      waypoint[last_waypoint_number].radius = HOME_WAYPOINT_THRESHOLD; 
      current_waypoint = waypoint[waypoint_number]; 
    }

    else if ((NAV_WAYPOINT == false) and (NAV_HOME == true)) {
      current_waypoint.latitude = gps.location.lat();
      current_waypoint.longitude = gps.location.lng();
      current_waypoint.radius = HOME_WAYPOINT_THRESHOLD; 
    }

    else if ((NAV_WAYPOINT == true) and (NAV_HOME == false)) {
      current_waypoint = waypoint[waypoint_number]; 
    }

    b_vs.reset(); 
    b_al.reset(); 
    g_vs.reset();
    
    baroset(gps.altitude.meters(), 1); 
    get_baro(1);
    merged_alt = gps.altitude.meters(); 

    cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);

    ground_altitude = gps.altitude.meters();
    ground_altitude = constrain(ground_altitude, 0, 2000); 
    
    dep_altitude = (DEP_ALT+ground_altitude);

    if (ALT_MODE) {
      sep_altitude = (SEP_ALT+ground_altitude); 
    }
    else {
      sep_altitude = SEP_ALT; 
    }
  
    setcam(1, 60, 60); 
    newfile();

    if (DROP == true) { flight_mode = 1;}
    else { 
      myPID.SetMode(MANUAL); 
      flight_mode = 7; 
    }

    init_time = millis(); 
    initialised = true;      
 }

 if (millis()-sat_buzz>5000) { 
    sat_buzz = millis(); 
    EasyBuzzer.beep(2000,50,25,gps.satellites.value(),100,1); 
  } 
  
}

//------------------- 1 -------------------//

void ready_steady() { 

 if (millis()-init_time>=3000) { 

  if (is_ascent(VUP, 0))    {
    flight_mode = 2; 
    EasyBuzzer.beep(1000,100,50,2,500,1); 
    setcam(1, 20, 600); 
  }
   
  if (is_descent(v_down(VDOWN), 1)) {  
    flight_mode = 3; 
    EasyBuzzer.beep(3000,100,50,3,500,1); 
    setcam(1, 60, 600); 
    init_time = millis();
  } 
  
  if ((atof(fix_type.value()) < 2) and (NO_INIT == false) and (flight_started == false))  { 
    flight_mode = 0; 
  }

  if (I_WANT_TO_FLY) {
    flight_mode = 5; 
  }   
 }
} 

//------------------- 2 -------------------//

void flight_ascent() { 
  
  if (is_descent(0, 0)) {
    flight_mode = 1;
  }
    
  if ((gps.altitude.meters()-ground_altitude)>10) {
    flight_started = true; 
  }
  
  if (gps.altitude.meters()>sep_altitude and gps_ok) {
    separation = true;
  }
  
}

//------------------- 3 -------------------//

void flight_descent() { 
  
  if (is_ascent(0, 0)) {
    flight_mode = 1; 
  } 

  if ((DEP_MODE and (millis()-init_time>DESCENT_TIMER)) or ((!DEP_MODE and merged_alt < dep_altitude))) { 
    flight_mode = 4;
    EasyBuzzer.beep(3000,100,50,5,500,1); 
    deployed = true; 
    init_time = millis(); 
    setcam(1, 60, 600); 
  }  
 }   

//------------------- 4 -------------------//

void flight_gliding() {

  if ((millis()-init_time) >= OPENING_TIMER) {
    wing_opened = true; 
    
    if (failsafe == true) {
      flight_mode = 5; 
      setcam(1, 20, 600); 
      myPID.SetMode(AUTOMATIC);
    } 
    
    else {
      flight_mode = 6; 
      myPID.SetMode(MANUAL);
    }
  } 
}

//------------------- 5 -------------------//

void flight_gliding_auto() { 

  if (!gps_ok) {
    flight_mode = 11;  
    myPID.SetMode(MANUAL);
  }

  if (is_descent(v_down(-5), 1)) {
    myPID.SetMode(MANUAL);
    spiral = true; 
    spiral_time = millis();
  }

  if (millis()-spiral_time>SPIRAL_RECOVER) {
    spiral = false;
    myPID.SetMode(AUTOMATIC);
  }
  
  if (failsafe == false) {
    flight_mode = 6;
    myPID.SetMode(MANUAL);
  }  

  if (I_WANT_TO_FLY == true) { 
    flight_mode = 5;
  } 

  if (new_cog) {
    new_cog = false;
    if (DEBUG) { Serial.println("New direction and command computed"); }  
    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
  }
  
}

//------------------- 6 -------------------//

void flight_gliding_manual() { 
  if (failsafe == true) {
    flight_mode = 5;
    myPID.SetMode(AUTOMATIC);
  }  
}

//------------------- 7 -------------------//

void on_ground() {  
  if (gps.speed.mps()>=LAUNCH_SPEED) {
    flight_mode = 8; 
  }
}

//------------------- 8 -------------------//

void motorised_man() {

  if (channels[6] > 1000) { 
    flight_mode = 9; 
    myPID.SetMode(AUTOMATIC); 
  } 
  
  if (failsafe == true) { 
    flight_mode = 10; 
    myPID.SetMode(AUTOMATIC); 
  }
}

//------------------- 9 -------------------//

void motorised_auto() { 
  
  if (new_cog) {
    new_cog = false;
    if (DEBUG) { Serial.println("New direction and command computed"); }  
    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
  }
  
  if (channels[6] < 1000) { 
    flight_mode = 8; 
  }
}

//------------------- 10 -------------------//

void motorised_failSafe() { 
  
  if (new_cog) {
    new_cog = false;
    if (DEBUG) { Serial.println("New direction and command computed"); }  
    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
  }
   
  if (failsafe == false) { 
    flight_mode = 8; 
    myPID.SetMode(MANUAL); 
  }
  

  /* DANGEROUS: WOULD SET MOTOR TO MAX!!!
  if (!gps_ok) {
    flight_mode = 11;  
    myPID.SetMode(MANUAL);
  }
  */

  if (is_descent(v_down(-5), 1)) {
    myPID.SetMode(MANUAL);
    spiral = true; 
    spiral_time = millis();
  }

  if (millis()-spiral_time>SPIRAL_RECOVER) {
    spiral = false;
    myPID.SetMode(AUTOMATIC);
  }
  
}

//------------------- 11 -------------------//

void flight_gliding_no_gps() {
  if (gps_ok) {
    flight_mode = 5; 
    myPID.SetMode(AUTOMATIC);
  }
}

//------------------- STATE MACHINE -------------------//

void cmpt_flight_state() {

  if (flight_mode!=prev_flight_mode and prev_flight_mode !=0) {
      cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
      save_data(); 
      send_data();
      prev_flight_mode = flight_mode; 
  }
  
  switch(flight_mode) { 
    case 0: 
      flight_init(); 
      setled(255, 0, 0, 25, 2000);
    break;

    case 1: 
      ready_steady(); 
      setled(0, 255, 0, 25, 500);
    break;

    case 2: 
      flight_ascent(); 
      setled(0, 0, 255, 25, 2000); 
    break;

    case 3: 
      flight_descent();
      setled(255, 128, 0, 25, 1000);  
    break;

    case 4: 
      flight_gliding(); 
    break;

    case 5: 
      flight_gliding_auto(); 
      setled(255, 255, 0, 25, 1000); 
    break;

    case 6: 
      flight_gliding_manual(); 
      setled(0, 255, 255, 25, 1000);
    break;

    case 7: 
      on_ground(); 
      setled(128, 0, 255, 25, 1000); 
    break;

    case 8: 
      motorised_man(); 
      setled(0, 0, 255, 25, 500); 
    break;

    case 9: 
      motorised_auto(); 
      setled(0, 255, 255, 25, 500);
    break; 

    case 10: 
      motorised_failSafe(); 
      setled(0, 255, 255, 25, 500);
    break; 

    case 11: 
      flight_gliding_no_gps(); 
      setled(255, 0, 0, 25, 200);
    break; 
  }
}