#ifndef PID_H
#define PID_H

#include <string>
#include "Debug.h"
#include <PID_v1.h>
#include "Constants.h"
#include "GroundComm.h"

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

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

// Function Declarations
void Debug_SetPIDConstants(double &p_constant, double &i_constant, double &d_constant, double new_p, double new_i, double new_d);

void PID_Setup(){
  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 PID_GoalFound(int InputxVal, int InputyVal){
  Inputx = InputxVal;
  Inputy = InputyVal;
  PID_x.Compute();
  PID_y.Compute();
}

void PID_BallDetected(int InputxVal, int InputyVal){
  Inputx = InputxVal;
  Inputy = InputyVal;
  PID_x.Compute();
  PID_y.Compute();
}

//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
  Debug_SetPIDConstants(p_constant, i_constant, d_constant, new_p, new_i, new_d);
}

#endif
