#include <esp_now.h>
#include <WiFi.h>
#include <SparkFun_VL53L1X.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "LedPanel.h"
#include <Arduino.h>
#include <string>
#include <vector>
#include "Camera.h"
#include <PID_v1.h>
#include "utilities.h"

// ============================== All variables ============================================
//-------------------- lidar part -------------------- 
SFEVL53L1X distanceSensor;
int budgetIndex = 4;
int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
int LED = LED_BUILTIN;
// ---------------------------------------------------
// -----------------------const variables----------------------------
//Identify all the global constants that will be used by the robot
const int BAUD_RATE = 115200;
const int MAX_SPEED = 255;
const double RESOLUTION_W = 320.0;
const double RESOLUTION_H = 240.0;
const int ENDDEMO_TAG = 0;
const uint32_t THISNODE_ID = 88789821;
double Setpointx, Inputx, Outputx;
double Setpointy, Inputy, Outputy;


// -------------------------------------------------------------------
// ----------------------- variables ----------------------------
uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
String success;

String sentDebugM = "";
int count = 0;
int count_var = 0;
int print_count = 0;
int change_count =0;
//----------------------------------------------------------
String strData = "";
double valData = 0.0;
String queData = "";
double Kpx=2, Kix=0.1, Kdx=0.25;
double Kpy=1, Kiy=0.1, Kdy=0.25;
int8_t g1 = 0,g2=1,g3=2;
int8_t goal_id[3] = {g1, g2, g3};
// int8_t goal[3] = {0,1,2};
// ------------- color part ------------------------- 
      // LAB: L[min] - L[max], A[min] - A[max], B[min] - B[max]
      //green old = (30, 100, -68, -13, 30, 127)
      // green_new = (30, 100, -49, -22, 31, 127)
      //(30,100,-68,2,6,127) - detect the yellow wall as well
      // red (30, 100, 127, 41, 127, 13)      
int8_t Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
int base_speed = 70;
int seeking_speed = 70;
int lidar_thres = 300; // mm 
int gbx = 0, gby = 0,gbd = 0;
double gbc = 0;
int debug_ball_cap = 0;
// -------------------------------------------------------------------

//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
char newPTCL = '1';
int pau = 0;
int displayTracking = 0;
// int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
// int BASE_SPEED = 70; //125;

//The debug LEDs that we will be using. Description at:

const int DEBUG_STATE = 31;
const int DEBUG_KP = 30;
const int DEBUG_KI = 29;
const int DEBUG_KD = 28;
const int DEBUG_BASESPEED = 27;
const int DEBUG_THRESHOLD_MIN = 26;
const int DEBUG_THRESHOLD_MAX = 25;
const int DEBUG_VERTICALSPEED = 17;
const int DEBUG_RSPEED = 16;
const int DEBUG_LSPEED = 24;

// =============================== All the setup ===========================================
//Create the interface that will be used by the camera
openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
Camera cam(&interface);
LedPanel panel(NUMPIXELS,PIN_PIXELS);
// ========================== Motor part ====================================
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// changed
Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1); 
Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);
Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
// changed
// ========================== PID part ====================================
// Setup PID
PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);

//-------------------------------------------------------------------------------------
typedef struct struct_message {
  String StrD;
  double ValD;
  String DebugM;
  String QueM;
  String VarM;
} struct_message;

struct_message receivedData;
//-------------------------------------------------------------------------------------
// Callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  
  if (status ==0){
    success = "Delivery Success :)";
  }
  else{
    success = "Delivery Fail :(";
  }
}


// Callback when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&receivedData, incomingData, sizeof(receivedData));
  Serial.print("\ndata:");
  Serial.println(receivedData.StrD);
  Serial.println(receivedData.ValD);
  Serial.println(receivedData.QueM);
  
  if (receivedData.QueM != "?"){
    valData = receivedData.ValD;
  }
  
  strData = receivedData.StrD;
  queData = receivedData.QueM;
  
  count = 0;
  count_var = 0;
  print_count=0;
  change_count = 0;
  
  Serial.print("queData:");
  Serial.println(queData);
}
//======================================================================================

void setup() {
  Serial.begin(BAUD_RATE);
  AFMS.begin();  // create with the default frequency 1.6KHz
  interface.begin(); //communication between ESP and OpenMV
  panel.beginPanel();
  // -------------- PID part --------------------
  //the values that the PID will try to reach
  Setpointx = 400.0; 
  Setpointy = 300.0; 
  PID_y.SetOutputLimits(-255, 255); //up positive
  PID_x.SetOutputLimits(-255, 255); //left positive
  PID_x.SetMode(AUTOMATIC);
  PID_y.SetMode(AUTOMATIC);
  // -------------- lidar part --------------------
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
  if (distanceSensor.begin() == 0)
    Serial.println("Sensor online!");
  distanceSensor.startRanging();
  distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
  digitalWrite(LED, LOW);

  // ----------------esp now -----------------------
  Wire.begin();
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);
  
  // Register peer
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  
  // Add peer        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }
  
  // Register for a callback function that will be called when data is received
  esp_now_register_recv_cb(OnDataRecv);

  
}

// ==============================main loop====================================
void loop() {
  if (change_count==0){
    if (queData != "?"){
      ChangeVariables();
    }else if(queData == "?"){
      QueryVariables();
    }
    
    change_count +=1;
  }
  
  panel.singleLED(DEBUG_BASESPEED, base_speed, base_speed, base_speed);
  //if the demo is still ongoing, check to see if there is a desired-color blob
  panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
  
  //========== lidar part =========
  int dist = 0;
  int ball_cap = 0;
  dist = distanceSensor.getDistance();
  Serial.println(dist);
  
  if ((dist < lidar_thres) && (dist > 0)){ // if we capture the ball 
    ball_cap = 1; 
    Serial.println("found ball");
     receivedData.DebugM = String(dist);
     // send_message_once(); 
  }else{
    ball_cap = 2;
    Serial.println("no ball");
    receivedData.DebugM = String(dist);
     // send_message_once(); 
  }
  // ========== goal finder =========
  int id = -1;
  int tx = 0; int ty = 0; int tz = 0;
  int rx = 0; int ry = 0; int rz = 0;
  int x = 0;
  int y = 0; 
  
  if (ball_cap == 1 || debug_ball_cap == 1){ // if we catch the green ball
    // we go find the goal
  if(cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz)){
    panel.singleLED(DEBUG_STATE, 0, 10, 0);
    Inputx = tx/1.00;
    Inputy = ty/1.00;
    PID_x.Compute();
    PID_y.Compute();
    /*
    Serial.print("x: ");
    Serial.println(Outputx);
    Serial.print("y: ");
    Serial.println(Outputy);
    */
    // Serial.println("moving toward goal");
     receivedData.DebugM = "det g mv2g";
     send_message_once(); 
      
    moveVertical(Outputy);
    moveHorizontal(Outputx, base_speed);
  }else { // seeking
      seeking(); 
      receivedData.DebugM = "seeking2";
      send_message_once(); 
      // Serial.print("catched the ball and seeking");
    }
  }else { // we keep searching for the green ball
  // if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
   if (ML_detect_gb(gbx,gby)){
    // Serial.print("catching the ball");
    receivedData.DebugM = "cating b";
      send_message_once(); 
    // panel.singleLED(DEBUG_STATE, 0, 10, 0);
    Inputx = gbx/1.00;
    Inputy = gby/1.00;
    PID_x.Compute();
    PID_y.Compute();
    //actuate the vertical motor
    moveVertical(Outputy);
    moveHorizontal(Outputx, base_speed);
  } else { //seeking algorithm
    seeking();
    receivedData.DebugM = "seeking1";
    send_message_once(); 
    // Serial.println("seeking the ball ");
  }
         }

   print_allvariables();      
}
// ============================== ^ main loop ^====================================


// ============================== other functions  ====================================
bool ML_detect_gb(int x,int y){
  if (x == 0 && y == 0){
    return false;
  } else {
    return true;
  }
}

void QueryVariables(){
  if (strData == "kpx"){
    receivedData.VarM = String(Kpx);
  }else if(strData == "kix"){
    receivedData.VarM = String(Kix);
  }else if(strData == "kdx"){
    receivedData.VarM = String(Kdx);
  }else if(strData == "kpy"){
    receivedData.VarM = String(Kpy);
  }else if(strData == "kiy"){
    receivedData.VarM = String(Kiy);
  }else if(strData == "kdy"){
    receivedData.VarM = String(Kdy);
  }else if(strData == "tha"){
    receivedData.VarM = String(Lmin);
  }else if(strData == "thb"){
    receivedData.VarM = String(Lmax);
  }else if(strData == "thc"){
    receivedData.VarM = String(Amin);
  }else if(strData == "thd"){
    receivedData.VarM = String(Amax);
  }else if(strData == "the"){
    receivedData.VarM = String(Bmin);
  }else if(strData == "thf"){
    receivedData.VarM = String(Bmin);
  }else if(strData == "bsp"){
    receivedData.VarM = String(base_speed);
  }else if(strData == "ssp"){
    receivedData.VarM = String(seeking_speed);
  }else if(strData == "lth"){
    receivedData.VarM = String(lidar_thres);
  }

  send_message_var_once();
  // queData = "";
  receivedData.VarM = "";
}


void send_message_var_once(){
  if(count_var==0){
    send_message(); 
    count_var+=1;
  }
}


void send_message_once(){
  if(count==0){
    send_message(); 
    count+=1;
    receivedData.DebugM = "";
  }
}


void send_message(){
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &receivedData, sizeof(receivedData));
  // esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));
  //-------------------------------------------------------------------------------------
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }
  //-------------------------------------------------------------------------------------
  delay(100);
}


void ChangeVariables(){ 
  // all the variables in this function can be remotely changed
  
  //-------------------PID-----------------
  Kpx = getDoubleVal(strData,"kpx",valData,Kpx);
  Kix = getDoubleVal(strData,"kix",valData,Kix);
  Kdx = getDoubleVal(strData,"kdx",valData,Kdx);
  Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
  Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
  Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
  
  //-------------------Goal id-----------------
  g1 = getIntVal(strData,"gda",valData,goal_id[0]);
  g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
  g3 = getIntVal(strData,"gdc",valData,goal_id[2]);
  goal_id[0] = g1;
  goal_id[1] = g2;
  goal_id[2] = g3;
  
  //-------------------Color threshold-----------------
  Lmin = getIntVal(strData,"tha",valData,threshold[0]);
  Lmax = getIntVal(strData,"thb",valData,threshold[1]);
  Amin = getIntVal(strData,"thc",valData,threshold[2]);
  Amax = getIntVal(strData,"thd",valData,threshold[3]);
  Bmin = getIntVal(strData,"the",valData,threshold[4]);
  Bmax = getIntVal(strData,"thf",valData,threshold[5]);
  threshold[0] = Lmin;
  threshold[1] = Lmax;
  threshold[2] = Amin;
  threshold[3] = Amax;
  threshold[4] = Bmin;
  threshold[5] = Bmax;
  
  //-------base_speed,seeking_speed,lidar_thres-----------------
  base_speed = abs(getDoubleVal(strData,"bsp",valData,base_speed));
  seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
  lidar_thres = getDoubleVal(strData,"lth",valData,lidar_thres);

  //------ green ball x, y----
  gbc = getDoubleVal(strData,"gbc",valData,gbc);
  gbx = gbc /1000000;
  gby = fmod(gbc,1000000)/1000;//(gbc.toInt() % 1000000)/1000;
  gbd = fmod(gbc,1000); // gbc.toInt() % 1000;

  // 
  Setpointx = getDoubleVal(strData,"spx",valData,Setpointx);
  Setpointy = getDoubleVal(strData,"spy",valData,Setpointy);

  debug_ball_cap = getDoubleVal(strData,"dbc",valData,debug_ball_cap);
  
  // gbx = getDoubleVal(strData,"gbx",valData,gbx);
  // gby = getDoubleVal(strData,"gby",valData,gby);
  
}

double getDoubleVal(String checkData,String Ans,double val,double ori_val){
  if (checkData == Ans){
    strData = "";
    valData = 0.0;
    return val;
  }else {
    return ori_val;
  }
}

int getIntVal(String checkData,String Ans,double val,int ori_val){
  if (checkData == Ans){
    strData = "";
    valData = 0.0;
    
    return (int8_t)val;
  }else {
    return ori_val;
  }
}

void print_allvariables(){
  if (print_count<=1){
    Serial.println("---------------");
    Serial.print("base speed:");
    Serial.print(base_speed);
    Serial.print("|");
    Serial.print("seeking speed:");
    Serial.print(seeking_speed);
    Serial.print("|");
    Serial.print("lidar thres:");
    Serial.println(lidar_thres);
    
    Serial.print("threshold:");
    Serial.print(threshold[0]);
    Serial.print("|");
    Serial.print(threshold[1]);
    Serial.print("|");
    Serial.print(threshold[2]);
    Serial.print("|");
    Serial.print(threshold[3]);
    Serial.print("|");
    Serial.print(threshold[4]); 
    Serial.print("|");   
    Serial.println(threshold[5]);  
    
    Serial.print("gid:");
    Serial.print(goal_id[0]);
    Serial.print(goal_id[1]);
    Serial.println(goal_id[2]);
    
    Serial.print("Kdx:");
    Serial.print(Kdx);
    Serial.print("|"); 
    Serial.print("Kix:");
    Serial.print(Kix);
    Serial.print("|");
    Serial.print("Kpx:");
    Serial.print(Kpx);
    Serial.print("|");
    Serial.print("Kdy:");
    Serial.print(Kdy);
    Serial.print("|");
    Serial.print("Kiy:");
    Serial.print(Kiy);
    Serial.print("|");
    Serial.print("Kpy:");
    Serial.println(Kpy);
    Serial.print("|");

    Serial.print("gbc:");
    Serial.print(gbc);
    Serial.print("|");
    Serial.print("gbx:");
    Serial.print(gbx);
    Serial.print("|");
    Serial.print("gby:");
    Serial.print(gby);
    Serial.print("gbd:");
    Serial.println(gbd);
    
    Serial.println("---------------------------\n");
    print_count +=1 ;
  }
}

void seeking(){
    // Serial.println("seeking...");
    moveVertical(0);
    moveHorizontal(seeking_speed, 0);
}

//vel value should be between -255 to 255 with positive values moving the blimp
//upward.
void moveVertical(int vel){
    if (vel > 0) { //up
      panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0);
      // changed 
      motorVertical_L->setSpeed(abs((int) vel));
      motorVertical_R->setSpeed(abs((int) vel)); 
      motorVertical_L->run(BACKWARD); 
      motorVertical_R->run(FORWARD);
      // changed
    } else if(vel < 0) { //down
      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
      // changed
      motorVertical_L->setSpeed(abs((int) vel));  
      motorVertical_R->setSpeed(abs((int) vel)); 
            motorVertical_L->run(FORWARD);
      motorVertical_R->run(BACKWARD);


      // changed
    } else {
      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
      //changed
      motorVertical_L->setSpeed(0);
      motorVertical_R->setSpeed(0);
      //changed 
    }
}

//LATER WE NEED TO EDIT THIS TO ACCOUNT FOR THE NEW MOTOR
void moveHorizontal(int vel_hori,int base_speed){
  int lspeed = -1*vel_hori + base_speed;
  int rspeed = vel_hori + base_speed;

  if (lspeed > 0){  
    motorLeft->run(BACKWARD); // make it move forward 
  } else {
    motorLeft->run(FORWARD); 
  }
  if (rspeed > 0){
    motorRight-> run(FORWARD); 
  } else {
    motorRight-> run(BACKWARD); // make it move backward
  }
  displaySpeed(lspeed, rspeed);
  motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )));
  motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)));
}

void displaySpeed(int lspeed, int rspeed){
  //Serial.println("display speed");
  if (lspeed < 0){
    panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed));
  } else {
    panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0);
  }

  if (rspeed < 0){
    panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed));
  } else {
    panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0);
  }
}

//When using the camera to detect objects such as colored blobs or april tags. This function is
//useful when only a single object is the target. The approximate position will be marked with an
//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be 
//called assuming that you have created the LED panel object, which in this code is named "panel".
//If the LED panel is called something else, just edit the code here
void displayTrackedObject(int cx, int cy, int w_res, int h_res){
  int lednum = 0;
  int vertshift = 0;
  panel.resetPanel();
  lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
  vertshift = cy/(h_res/4);
  lednum = lednum + 8*vertshift;
  panel.singleLED(lednum, 10, 10 , 10);
}

//This function translate a string of message into the constants for a PID controller. Ignoring the 
//first command character, user can input any one, two, or all three parameter to be changed by identifying
//the new parameter with the capital letter of that specific parameter, each separated with a space. 
//Some of the valid msg examples are:
//  - "P0.02"
//  - "D1.23"
//  - "P0.14 D9.5"
//  - "P0.1 I0.1 D0.1"
//
//NOTE:
//  - the parameter doesn't have to be in order. You can do DI or IP in whichever order as long as it follows a valid double value
//  - while the function works if you passed a negative double, the PID controller will not and will produce error.
//  - suggested command character: 'T'[uning]
void setPIDConstants(String msg, double &p_constant, double &i_constant, double &d_constant){
  double new_p = Kpx;
  double new_i = Kix;
  double new_d = Kdx;
  
  int len = msg.length();
  int startpoint = 0;
  int endpoint = 0;
  for (int i = 0; i < len; i++){
    if (msg[i] == 'P'){
      startpoint = i + 1;
      for (int j = i + 1; j < len; j++){
        if (msg[j] == ' '){
          endpoint = j;
          break;
        } else {
          endpoint = len;
        }
      }
      if (endpoint > startpoint){ //check to see if it is a valid value
        //Serial.println(msg.substring(startpoint, endpoint));
        new_p = msg.substring(startpoint, endpoint).toDouble();
      }
    }

    if (msg[i] == 'I'){
      startpoint = i + 1;
      for (int j = i + 1; j < len; j++){
        if (msg[j] == ' '){
          endpoint = j;
          break;
        } else {
          endpoint = len;
        }
      }
      if (endpoint > startpoint){ //check to see if it is a valid value
        //Serial.println(msg.substring(startpoint, endpoint));
        //i_constant = msg.substring(startpoint, endpoint).toDouble();
        new_i = msg.substring(startpoint, endpoint).toDouble();
      }
    }

    if (msg[i] == 'D'){
      startpoint = i + 1;
      for (int j = i + 1; j <= len; j++){
        if (msg[j] == ' ' || msg[j] == '\0'){
          endpoint = j;
          break;
        } else {
          endpoint = len;
        }
      }
      if (endpoint > startpoint){ //check to see if it is a valid value
        //Serial.println(msg.substring(startpoint, endpoint));
        //d_constant = msg.substring(startpoint, endpoint).toDouble();
        new_d = msg.substring(startpoint, endpoint).toDouble();
      }
    }
  }

  //DEBUGGING SECTION
  unsigned long t = millis();
  debugPIDConstants(DEBUG_KP, p_constant, new_p);
  debugPIDConstants(DEBUG_KI, i_constant, new_i);
  debugPIDConstants(DEBUG_KD, d_constant, new_d);

  p_constant = new_p;
  i_constant = new_i;
  d_constant = new_d;
  
  while(millis() < t+2000); //debugging LED will light up for 2 seconds
  panel.singleLED(DEBUG_KP, 0, 0, 0);
  panel.singleLED(DEBUG_KI, 0, 0, 0);
  panel.singleLED(DEBUG_KD, 0, 0, 0);
}

void debugPIDConstants(int lednum, double oldval, double newval){
  int r, g, b;
  if (newval > oldval){
    panel.singleLED(lednum, 0, 10, 0);
  } else if (newval < oldval){
    panel.singleLED(lednum, 10, 0, 0);
  } else { //equal
    panel.singleLED(lednum, 10, 10, 10);
  }
}
