#include "PID.h"
#include <string>
#include <vector>
#include <Wire.h>
#include "Debug.h"
#include "LIDAR.h"
#include <Arduino.h>
#include "Constants.h"
#include "utilities.h"
#include "CameraDefn.h"
#include "GroundComm.h"
#include "Propulsion.h"
#include "ColorDetect.h"
//#include "Localization.h"
// #include "Adafruit_VL53L0X.h"
using namespace std;

// Identify all the variables that will be used by the robot
int     findcolor       = 1; //based on the same code from openMV. Determine the color you want to find
int     pauseState      = 0;
int     displayTracking = 0;
int     ballCapture     = 0;
int     ballDetect      = 0;
int     goalFind        = 0;
float   altitude        = 0;
double *ballProps;

// Function Declarations

void setup() {
  Serial.begin(BAUD_RATE);
  Wire.begin();
  AFMS.begin();  // create with the default frequency 1.6KHz
  
  GroundComm_Setup();
  CameraDefn_Setup();
  ColorDetect_Setup(findcolor);
  Debug_Setup();
  PID_Setup();
  LIDAR_Setup();
  #ifdef LOCALIZATION_H
    Localization_Setup();
  #endif
  #ifdef MESH_H
    Mesh_Setup();
  #endif
}

void loop(){
  /* ---------- MAIN LOOP ---------- */
  Debug_LoopStart();
  #ifdef MESH_H
    Mesh_LoopStart();
  #endif
  GroundComm_LoopStart();
  
  /* --------- PAUSE STATE --------- */
  while(pauseState == 1){
    #ifdef MESH_H
      Mesh_PauseState();
    #endif
    Debug_PauseState();
    Propulsion_PauseState();
  }
  /* ------- END PAUSE STATE ------- */
  
  /* -------- INITIALIZATION ------- */
  int id = -1;
  int  x = 0,  y = 0;
  int tx = 0, ty = 0, tz = 0;
  int rx = 0, ry = 0, rz = 0;
  int8_t goal[3] = {0,1,2};
  
  ballCapture = LIDAR_BallCapture();
  goalFind    = CameraDefn_GoalFind(goal, id, tx, ty, tz, rx, ry, rz);
  if (ML_BALL_DETECT)
    ballDetect  = GroundComm_BallDetect(x, y);
  else
    ballDetect  = CameraDefn_BallDetect(x, y);
  /* ----- END INITIALIZATION ------ */
  
  /* -------- STANDBY STATE -------- */
  Debug_StandbyState();
  /* ------ END STANDBY STATE ------ */

  /* --------- INTERRUPTS ---------- */
  #ifdef LOCALIZATION_H
    altitudeBarrier();
  #endif
  
  if(insideWindyArea()){
    // leave windy area
    // flip horizontal inputs
  }
  /* -------- END INTERRUPTS ------- */

  /* ---------- MAIN LOGIC --------- */
  if(ballCapture){ // Ball captured
	  if(goalFind){ // Ball captured and Goal found
      Serial.println("  Ball captured and Goal found");
      Debug_GoalFound();
      PID_GoalFound(tx/1.00, ty/1.00);
      #ifdef LOCALIZATION_H
        Localization_GoalFound();
	    #endif
	    Propulsion_GoalFound();
    }
    else { // Ball captured but Goal not found
      Serial.println("  Ball captured but Goal not found");
      Debug_GoalSeeking();
      Propulsion_GoalSeeking();
    }
  }
  else { // Ball not captured
	  if(ballDetect) { // Ball not captured but detected
      Serial.println("  Ball not captured but detected");
      Serial.print(x);
      Serial.println(y);
      Debug_BallDetected();
      #ifdef LOCALIZATION_H
        Localization_BallDetected();
      #endif
      PID_BallDetected(x/1.00, y/1.00);
      Propulsion_BallDetected();
    }
    else { // Ball not captured and not detected
      Serial.println("  Ball not captured and not detected");
      Propulsion_BallSeeking();
    }
  }
  /* ------- END MAIN LOGIC -------- */
  /* -------- END MAIN LOOP -------- */
}

#ifdef LOCALIZATION_H
void altitudeBarrier(){
  float altitude = altitudeCalc();
  if(altitude < lowHeightBarrier){
    move_V((lowHeightBarrier - altitude)*255/lowHeightBarrier);
  }
  else if(altitude > ceilingHeight - highHeightBarrier){
    move_V(((ceilingHeight - highHeightBarrier) - altitude)*255/highHeightBarrier);
  }	
}
#endif

float insideWindyArea(){
  
  return 0;
}
