#ifndef GROUNDCOMM_H
#define GROUNDCOMM_H

#include <string>
#include <WiFi.h>
#include <Wire.h>
#include <Arduino.h>
#include <esp_now.h>

// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98};
String success;

// Define variables to store BME280 readings to be sent
String sentDebugM = "";
int count = 0;
int count_var = 0;
int print_count = 0;
int change_count = 0;

// Define variables to store incoming readings
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;
int g1 = 0, g2 = 1 , g3 = 2;
int goal_id[3] = {g1, g2, g3};
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;

double ballDetectTime = 0.0;

// =====================================================================================
//Structure the sending data
//Must match the receiver structure
typedef struct struct_message {
  String StrD;
  double ValD;
  String DebugM;
  String QueM;
  String VarM;
} struct_message;

// Function Declarations
void ChangeVariables();
void QueryVariables();
void send_message();
void send_message_var_once();
double getDoubleVal(String checkData, String Ans, double val, double ori_val);
int getIntVal(String checkData,String Ans,double val,int ori_val);

// Create a struct_message to hold incoming sensor readings
// struct_message incomingReadings;
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 GroundComm_Setup(){
  // Set device as a Wi-Fi Station
  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 peers
  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);
}

void GroundComm_LoopStart(){
  if (change_count == 0){
    if (queData != "?"){
      ChangeVariables();
    }
  else if(queData == "?"){
      QueryVariables();
    }
    change_count +=1;
  }
}

bool GroundComm_BallDetect(int &x, int& y){
  Serial.println("In GroundComm_BallDetect");
  x = gbx;
  y = gby;
  Serial.println(x);
  Serial.println(y);
  if (x == -1)
    return false;
  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----
  gbx = getDoubleVal(strData,"gbx",valData,gbx);
  gby = getDoubleVal(strData,"gby",valData,gby);
  if (strData == "gbx" || strData == "gby")
    ballDetectTime = millis();
}


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("gbx:");
    Serial.print(gbx);
    Serial.print("|");
    Serial.print("gby:");
    Serial.println(gby);
    
    Serial.println("---------------------------\n");
    print_count +=1 ;
  }
}

#endif
