#include <Wire.h>
#include <Adafruit_MotorShield.h>
//#include "Adafruit_VL53L0X.h"
#include "ArnholdMesh.h"
#include "LedPanel.h"
#include <Arduino.h>
#include <string>
#include <vector>
#include "Camera.h"
#include <PID_v1.h>
#include "utilities.h"

//definition needed for mesh
#define   STATION_SSID     "lemur"
#define   STATION_PASSWORD ""
#define   MQTT_BROKER      ""
#define   MQTT_TOPIC       ""

double Setpointx, Inputx, Outputx;
double Setpointy, Inputy, Outputy;

//Identify all the global constants that will be used by the robot
const int BAUD_RATE = 115200;
const int MAX_SPEED = 255;
const int SEEKING_SPEED = 70;
const double RESOLUTION_W = 320.0;
const double RESOLUTION_H = 240.0;
const int ENDDEMO_TAG = 0;
const uint32_t THISNODE_ID = 88789821;

//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 = 50; //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;

//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

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
Adafruit_DCMotor *motorVertical = AFMS.getMotor(3);
Adafruit_DCMotor *motorLeft = AFMS.getMotor(2);
Adafruit_DCMotor *motorRight = AFMS.getMotor(4);


Camera cam(&interface);
LedPanel panel(NUMPIXELS,PIN_PIXELS);
ArnholdMesh thisNode;

//Specify the links and initial tuning parameters
double Kpx=2, Kix=0.05, Kdx=0.25;
double Kpy=2, Kiy=0.1, Kdy=0.25;
PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);

void setup() {
  Serial.begin(BAUD_RATE);
  translateCodetoThreshold(findcolor);
  Wire.begin();
  AFMS.begin();  // create with the default frequency 1.6KHz
  interface.begin(); //communication between ESP and OpenMV
  panel.beginPanel();

  thisNode.setPanel(&panel);
  thisNode.setStationInfo(STATION_SSID, STATION_PASSWORD);
  thisNode.setMQTTInfo(MQTT_BROKER, MQTT_TOPIC);
  thisNode.amIMQTTBridge(false);
  thisNode.init();
  
  Setpointx = 160.0; 
  Setpointy = 120.0; //the values that the PID will try to reach
  PID_y.SetOutputLimits(-255, 255); //up positive
  PID_x.SetOutputLimits(-255, 255); //left positive
  PID_x.SetMode(AUTOMATIC);
  PID_y.SetMode(AUTOMATIC);
}

void loop() {  
  //
  panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);

//  float r = 0; 
//  float g = 0; 
//  float b = 0;
//  lab2rgb((float)threshold[0], (float)threshold[2], (float)threshold[4], r, g, b);
//  panel.singleLED(DEBUG_THRESHOLD_MIN, (int)r, (int)g, (int)b);
//  lab2rgb((float)threshold[1], (float)threshold[3], (float)threshold[5], r, g, b);
//  panel.singleLED(DEBUG_THRESHOLD_MAX, (int)r, (int)g, (int)b);
//  
  //
  thisNode.update();

  while(pau == 1){
    thisNode.update();
    int zero = 0;
    moveVertical(zero);
    moveHorizontal(zero,zero);
    panel.singleLED(DEBUG_STATE, 10, 10, 0);
    Serial.println("pause");
  }
  
  //Check to see if we want to end the demo
  int xtemp = 0;
  int ytemp = 0;
  int angletemp = 0;
//  if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
//    int zero = 0;
//    moveVertical(zero);
//    moveHorizontal(zero,zero);
//    panel.singleLED(DEBUG_STATE, 0, 0, 10);
//    while(1){
//      Serial.println("end of demo");
//    }
//  }

  //if the demo is still ongoing, check to see if there is a desired-color blob
  int x = 0;
  int y = 0;
  panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
  //if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
  int id = -1;
  int tx = 0;
  int ty = 0;
  int tz = 0;
  int rx = 0;
  int ry = 0;
  int rz = 0;
  int8_t goal[3] = {0,1,2};
  if(cam.exe_goalfinder(goal[0], goal[1], goal[2], id, x, y, tz, rx, ry, rz)){
    if (displayTracking > 0){
      displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
    }
    panel.singleLED(DEBUG_STATE, 0, 10, 0);
    Serial.println("blob detected");
    Serial.print("x value: ");
    Serial.println(x);
    Serial.print("y value: ");
    Serial.println(y);
    Inputx = x/1.00;
    Inputy = y/1.00;
    PID_x.Compute();
    PID_y.Compute();

    Serial.println(Outputy);
    Serial.println(Outputx);

    //actuate the vertical motor
    moveVertical(Outputy);
    moveHorizontal(Outputx, BASE_SPEED);
    
  } else { //seeking algorithm
    //panel.resetPanel();
    panel.singleLED(DEBUG_STATE, 10, 10, 10);
    Serial.println("seeking...");
    int zero = 0;
    moveVertical(zero);
    moveHorizontal(SEEKING_SPEED, zero);
  }
}

//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);
      motorVertical->setSpeed(abs((int) vel));
      motorVertical->run(BACKWARD);
    } else if(vel < 0) { //down
      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
      motorVertical->setSpeed(abs((int) Outputy));
      motorVertical->run(FORWARD);
    } else {
      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
      motorVertical->setSpeed(0);
    }
}

void moveHorizontal(int vel_hori,int base_speed){
  int lspeed = -1*vel_hori + base_speed;
  int rspeed = vel_hori + base_speed;

  if (rspeed > 0){
    motorLeft->run(BACKWARD);
  } else {
    motorLeft->run(FORWARD);
  }

  if (lspeed > 0){
    motorRight->run(BACKWARD);
  } else {
    motorRight->run(FORWARD);
  }
  displaySpeed(lspeed, rspeed);
  motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed)));
  motorRight->setSpeed(min(MAX_SPEED, abs(lspeed)));
}

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


//Interpret a message to change what color the camera is detecting
void setColor(String msg) {
  if (msg.length() < 3){
    int val = msg.toInt();
    translateCodetoThreshold(val);
    Serial.println("code detected");
  } else {
    Serial.println("new threshold detected");
    setColorThreshold(msg, threshold, 6);
  }
  for (int j = 0; j < 6; j++){
    Serial.print(threshold[j]);
    Serial.print(" ");
  }
  Serial.println("");
}

//in case we don't want to maually set each LAB threshold, we can send over an int to
//use preset threshold values instead
void translateCodetoThreshold(int code){
  switch(code){
    case 1: //green old = (30, 100, -68, -13, 30, 127)
      //(30,100,-68,2,6,127) - detect the yellow wall as well
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = -93;
      threshold[3] = -5;
      threshold[4] = 13;
      threshold[5] = 127;
    break;
    case 2: //blue
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = -108;
      threshold[3] = -9;
      threshold[4] = 0;
      threshold[5] = -42;
    break;
    case 5: //red (30, 100, 127, 41, 127, 13)
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = 127;
      threshold[3] = 41;
      threshold[4] = 127;
      threshold[5] = 13;
  }
}


//threshold array must have at least six elements. This function helps 
//translating a message with threshold values to ints. Example msg that would
//be received by this function are "1 2 3 4 5 6" or "-100 70 9 -9 -50 128". 
//NOTE: - the threshold value should be between -128 to 128
//      - the message should not include the command character used by the mesh  
//      - suggested command character: 'C'[olor]
void setColorThreshold(String msg, int8_t thres[], int arraySize){
  int len = msg.length();
  int temp = 0;
  int startpoint = 0;
  for (int i = 0; i < len; i++){
    if (msg[i] == ' '){
      thres[temp] = msg.substring(startpoint,i).toInt();
      startpoint = i + 1;
      temp++;
    }
    if (temp == 5){
      thres[temp] = msg.substring(startpoint).toInt();
      break; 
    }
  }
}

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



//int goal_id[3] = {0, 1, 2};
//////goal seeking algorithm
//if (lidar detect green ball){
//  int id = -1; int tx = 0; int ty = 0; int tz = 0;
//  int rx = 0; int ry = 0; int rz = 0; 
//  if(cam.exe_goalfinder(goal_id[0], goal_id[1], goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz)){
//    //apply the same PID control when seeking green ball
//    //if we are at a specific position near the goal (slightly above), move straight forward for 5 second
//  } else {
//    //seeking mechanism to find the goal
//    //move up until we reach the desired altitude
//    //if already at the desired altitude, rotate around  
//  } 
//} else { //no ball in basket
//  //do whatever we have been doing before to find a green balloon
//}




////MESH COMMUNICATION UTILITY FUNCTIONS////
void interpretDashInstructions(String& msg){
  // Deserialize different dash topics as they are received
  String result;
  std::list<uint32_t> nodesToSend;
  Serial.println(msg);
  // If dash topic is Command
  if(thisNode.dashDeserialize(msg, "Command", result, nodesToSend)){
    // Interpret what command on the dash means
    char numChar = result[0];
    String commandContent = result.substring(1);
    if(numChar == 'G'){
      String topo = thisNode.mesh.subConnectionJson();
      thisNode.sendJsonToDash("Debug", topo);
      return;
    }
    else {
      thisNode.sendOverAllFreqs(nodesToSend, result);
    }
  }
  // If the dash topic is MasterJoystick
  else if(thisNode.dashDeserialize(msg, "MasterJoystick", result, nodesToSend)){
     // Interpret what a dash MasterJoystick message means
     String joystickcontrol = "J";
     joystickcontrol += result;
     thisNode.sendOverAllFreqs(nodesToSend, result);
     return;
  }
}

void vehicleInstruction(char instructionType, String instrData, uint32_t from){
  // Do something with the data receieved over the network
  // Based on what the instructionType is, do something different with the instrData
  // instructionType CAN NOT BE:
  // 'B', 'O', 'g', 'm', 'u', 'R', 'D'
  //  I am using those within ArnholdMesh for testing purposes
  switch(instructionType){
    case 'C':
      setColor(instrData);
      Serial.print("color: ");
      Serial.println(instrData);
      break;
    case 'T':
      setPIDConstants(instrData, Kpx, Kix, Kdx); 
      Serial.print("new PID constants: ");
      Serial.print(Kpx);
      Serial.print(" ");
      Serial.print(Kix);
      Serial.print(" ");
      Serial.print(Kdx);
      Serial.println("");
      break;
    case 'P': //pause the program
      pau = instrData.toInt();
      Serial.print("paused: ");
      Serial.println(pau);
      break;
    case 'S': //change the base speed
      int new_base_speed = instrData.toInt();
      if (new_base_speed >= 0 && new_base_speed <= 255){
        BASE_SPEED = new_base_speed;
        Serial.print("base speed: ");
        Serial.println(BASE_SPEED);
      } else {
        Serial.println("invalid base speed");
      }
      break;
  }
}

// Callback function needed for painlessMesh
void receivedCallback(uint32_t from, String &msg) {
  Serial.printf("startHere: Received from %u msg=%s\n", from, msg.c_str());
  thisNode.lastMsgReceieved = millis();
  thisNode.colorsL[2] = 5;
  thisNode.panel->topLeftQuadrant(thisNode.colorsL[0], thisNode.colorsL[1], thisNode.colorsL[2]);
  thisNode.interpretInstruction(from, msg);
}


// Callback function needed for MQTT
void mqttCallback(char* topic, uint8_t* payload, unsigned int length) {
  char* cleanPayload = (char*)malloc(length+1);
  memcpy(cleanPayload, payload, length);
  cleanPayload[length] = '\0';
  String msg = String(cleanPayload); 
  free(cleanPayload);
  Serial.println(msg);
  interpretDashInstructions(msg);
}

void newConnectionCallback(uint32_t nodeId) {
  Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
  digitalWrite(LED_BUILTIN, HIGH);
  if(thisNode.bridgeId == 0){
      String askpayload = "g";
      thisNode.sendOverAllFreqs(nodeId, askpayload);
  }
}
