

#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"



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

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

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

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


// ==============================main loop====================================

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

void loop() {  
  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
  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, 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);
    moveVertical(Outputy);
    moveHorizontal(Outputx, BASE_SPEED);
  }else { // seeking
    Serial.println("seeking...");
    moveVertical(0);
    moveHorizontal(SEEKING_SPEED, 0);
    }
  
  /*
  if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
    if (displayTracking > 0){
      displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
    }
    panel.singleLED(DEBUG_STATE, 0, 10, 0);
    Inputx = x/1.00;
    Inputy = y/1.00;
    PID_x.Compute();
    PID_y.Compute();

    //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);
  }
  */
}
// ==============================main loop====================================



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


//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
      // LAB: L[min] - L[max], A[min] - A[max], B[min] - B[max]
      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);
  }
}
