#include <ESP32Servo.h>
#include <PID_v1_bc.h>

const int pin1 = 19;
const int pin2 = 18;
const int vert = 13;
const int hori = 12;
const int sel = 14;

Servo servo1;
Servo servo2;

int value1, value2, current_value1, current_value2;
double integral1 = 0, integral2 = 0;
double previous_error1 = 0, previous_error2 = 0;
long previous_T =0;

double PIDcontroller (double &integral, int value, int current_value, double &previous_error) {
  double kp = 5, ki = 3, kd = 0.5;
  double setpoint, processvariable, error, derivative, output;
  long current_T = micros();
  long delta = current_T - previous_T;
  previous_T = current_T;
  setpoint = value;
  processvariable = current_value;
  error = setpoint - processvariable;
  integral += error + error*delta;
  derivative = (error - previous_error)/delta;

  output = (kp * error) + (ki * integral) + (kd * derivative);
  previous_error = error; 

  return output;
}

void setup() {
  Serial.begin(115200);

  servo1.attach(pin1);
  servo1.write(0);
  
  servo2.attach(pin2);
  servo2.write(0);

  value1 = 0;
  value2 = 0;
}

void loop() {
  int valuey = analogRead(vert);
  int valuex = analogRead(hori);

  if (valuey > 2048) {
    value1 += 1; 
  } else if (valuey < 2048) {
    value1 -= 1; 
  }

  if (valuex > 2048) {
    value2 += 1; 
  } else if (valuex < 2048) {
    value2 -= 1; 
  }

  value1 = constrain(value1, 0, 180);
  value2 = constrain(value2, 0, 180);

  current_value1 = servo1.read();
  current_value2 = servo2.read();

  if(current_value1 == -1){
    current_value1 = 0;
  }

  if(current_value2 == -1){
    current_value2 = 0;
  }

  double output1 = PIDcontroller(integral1, value1, current_value1, previous_error1);
  double output2 = PIDcontroller(integral2, value2, current_value2, previous_error2);
  
  // output1 = constrain(output1, 0, 180); 
  // output2 = constrain(output2, 0, 180); 
  
  servo1.write(output1);
  servo2.write(output2);

  Serial.print("Setpoint1: ");
  Serial.print(value1);
  Serial.print(" Current Value1: ");
  Serial.print(current_value1);
  Serial.print(" Output1: ");
  Serial.print(output1);
  
  Serial.print(" :: Setpoint2: ");
  Serial.print(value2);
  Serial.print(" Current Value2: ");
  Serial.print(current_value2);
  Serial.print(" Output2: ");
  Serial.println(output2);

  delay(10);
}