#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "U8glib.h"
 
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE); // I2C / TWI 
//U8X8_SSD1306_128X64_NONAME_HW_I2C display(U8X8_PIN_NONE); 
const int WIDTH=128;
const int HEIGHT=64;
const int LENGTH=WIDTH;
 
// Define the analog input pin and the servo pin
const int analogInputPin = A0;
const int servoPinR = 9;
const int servoPinL = 8;
const int LEDPIN = 2;

// Define the minimum and maximum values for the servo
const int servoMin = 0;
const int servoMax = 180;

// Define the antiwindup variables
const int integralMin = -5;
const int integralMax = 5;


// Define the analog input pins
const int kpInputPin = A1;
const int kiInputPin = A2;
const int kdInputPin = A3;

// Define max values of Kp, Ki and Kd
const float maxKp = 2.0;
const float maxKi = 0.5;
const float maxKd = 0.2;



// Define the PID variables
float Kp = 1.0;
float Ki = 1.0;
float Kd = 1.0;
float analogInput = 0.0;



// Define the PID variables for the LCD display
float Kp_display = 0;
float Ki_display = 0;
float Kd_display = 0;

// Define the PID variables for mapping
float Kp_input = 0;
float Ki_input = 0;
float Kd_input = 0;

// Define order 
float order = 0.0;

// Define the PID variables
double error = 0.0;
double lastError = 0.0;
double integral = 0.0;
double derivative = 0.0;
double output = 0.0;
// variables externas del controlador
double Input, Output, Setpoint;
 
// variables internas del controlador
unsigned long currentTime, previousTime;
double elapsedTime;
bool dirR; 

// Define the servo object
Servo servoL;
Servo servoR;
// Define the LCD object
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Prepare plot in I2C screen
int x;
int y[LENGTH];
int z[LENGTH];
 
void clearY(){
  for(int i=0; i<LENGTH; i++){
    y[i] = -1;
    z[i] = -1;
  }
}
 
void drawY(){
  u8g.drawPixel(0, y[0]);
  for(int i=1; i<LENGTH; i++){
    if(y[i]!=-1){
      //u8g.drawPixel(i, y[i]);
      u8g.drawLine(i-1, y[i-1], i, y[i]);
      u8g.drawLine(i-1, z[i-1], i, z[i]);
    }else{
      break;
    }
  }
}

void setup() {
  // Initialize the serial port
  Serial.begin(9600);
  pinMode(LEDPIN, OUTPUT);
  // Initialize the LCD display
  lcd.init();
  lcd.backlight();

  // Attach the servo to the pin
  servoL.attach(servoPinL);
  servoR.attach(servoPinR);

  // Set the initial position of the servo to the center
  servoL.write(90);
  servoR.write(90);
  // Prepare Screen
  x = 0;
  clearY();
}


void loop() {
  // Read the analog inputs and map them to the desired range
  Kp_input = analogRead(A1);
  Kp = maxKp * Kp_input / 1023 ;
  //Kp_input = analogRead(kpInputPin) ;
  Ki_input = analogRead(A2);
  Kd_input = analogRead(A3);
  Ki =  maxKi * (Ki_input / 1023);
  Kd = maxKd * Kd_input / 1023;

  // Update the values for the LCD display
  Kp_display = Kp;
  Ki_display = Ki;
  Kd_display = Kd;
  // clear screen trick just for simulation
  //lcd.setCursor(0,0);
  //lcd.print("                    ");
  // Update the LCD display with the current values of Kp, Ki, and Kd
  lcd.setCursor(0, 0);
  lcd.print("Kp: ");
  lcd.print(Kp_display);
  lcd.setCursor(10, 0);
  lcd.print("Ki: ");
  lcd.print(Ki_display);
  lcd.setCursor(0, 1);
  lcd.print("Kd: ");
  lcd.print(Kd_display);
  // Read the analog input and map it to the range of the servo
  analogInput = analogRead(A0);
  float target = servoMin + (analogInput / 1023) * (servoMax - servoMin);
  // Check which servo to use, left or right
  if (target >= 90) {
    dirR = true;
  }
  else{
    dirR = false;
  }
  lcd.setCursor(10, 1);
  lcd.print("Sp: ");
  lcd.print(target);
  float targetL = min(90,target);
  float targetR = max(90, target);
  // Calculate the error and update the integral and derivative
  currentTime = millis();                               // obtener el tiempo actual
  elapsedTime = (double)(currentTime - previousTime)/1000;     // calcular el tiempo transcurrido, en segundos
  if (dirR == true) {
    error = target - servoR.read();
  }
  else{
    error = target -servoL.read();
  }
  integral += error * elapsedTime* Ki;
  if (integral < integralMin) {
    integral = integralMin;
    digitalWrite(LEDPIN, HIGH);
  }
  else if (integral > integralMax) {
    integral = integralMax;
    digitalWrite(LEDPIN, HIGH);
  }
  else {
    digitalWrite(LEDPIN, LOW);
  }
  derivative = (error - lastError) / elapsedTime;
  lastError = error;
  previousTime = currentTime;  
  // Calculate the output using PID control NOTE! Ki used on integral term estimation above for windup control
  output = Kp * error + integral + Kd * derivative;
  lcd.setCursor(0, 2);
  lcd.print("Lv: ");
  if (dirR == true) {
    lcd.print(servoR.read());
  }
  else {
    lcd.print(servoL.read());
  }
  lcd.print(" ");
  // Write the output to the servo
  if (dirR == true) {
    order = servoR.read() + output;
    servoR.write(order);
    servoL.write(90);
   }
  else{
    order = servoL.read() + output;
    servoL.write(order);
    servoR.write(90);
   }
  lcd.setCursor(0, 3);
  lcd.print("goal: ");
  lcd.print(order);
  //lcd.clear();
  y[x] = map(order, 0, 180, HEIGHT-1, 0);
  z[x] = map(target,0, 180, HEIGHT -1, 0);
 
  u8g.firstPage();  
  do {
    drawY();
  } while( u8g.nextPage() );
  
  //delay(10);
 
  x++;
  if(x >= WIDTH){
    x = 0;
    clearY();
  }

  // plotear TC
  lcd.setCursor(8, 2);
  lcd.print("  ");
  lcd.setCursor(10, 2);
  lcd.print("Tc: ");
  lcd.print(elapsedTime);

  delay(1);
/*
  // Check for serial input
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    int semicolon1 = input.indexOf(';');
    int semicolon2 = input.indexOf(';', semicolon1 + 1);
    if (semicolon1 != -1 && semicolon2 != -1) {
      Kp = input.substring(0, semicolon1).toDouble();
      Ki = input.substring(semicolon1 + 1, semicolon2).toDouble();
      Kd = input.substring(semicolon2 + 1).toDouble();
      Serial.print("Kp=");
      Serial.print(Kp);
      Serial.print(";Ki=");
      Serial.print(Ki);
      Serial.print(";Kd=");
      Serial.println(Kd);
    }
  }
  */
}