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

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;
int ball_capture = 0;

//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 *motor_VL = AFMS.getMotor(2); 
Adafruit_DCMotor *motor_VR = AFMS.getMotor(1);
Adafruit_DCMotor *motor_HL = AFMS.getMotor(3);
Adafruit_DCMotor *motor_HR = AFMS.getMotor(4);

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

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

  if (!baro.begin()) {
    Serial.println("Barometer not found");
    while(1);
  }
  baro.setSeaPressure(1016);
}

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

  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;
  int x = 0;
  int y = 0;
  panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
  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};
  float altitude = baro.getAltitude();

  
  // Have we captured the ball?
  // ball_capture = lidar_output()
  
  if(ball_capture){
	  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.print("Outputx: ");
      Serial.println(Outputx);
      Serial.print("Outputy: ");
      Serial.println(Outputy);
      
	    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);
    }
  }
  else {
	  ball_detect = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y);
    if(ball_detect) {
      // move towards ball
      
    }
    else {
      // seek the ball
      // add seeking algorithm - turn around for 360 degrees?
    }
  }
}

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

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

  if (lspeed > 0){
    motor_HL->run(FORWARD);
  } else {
    motor_HL->run(BACKWARD);
  }

  if (rspeed > 0){
    motor_HR->run(FORWARD);
  } else {
    motor_HR->run(BACKWARD);
  }
  displaySpeed(lspeed, rspeed);
  motor_HL->setSpeed(min(MAX_SPEED, abs(rspeed)));
  motor_HR->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);
  }
}
