#include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library
#include <Servo.h>
#include <LiquidCrystal.h>

// Definir los pines
const int pinPotentiometer = A0; // Pin del potenciómetro
const int pinServo = 9; // Pin del servomotor
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2; // Pines para el LCD

// Parámetros del PID
double kp = 1;
double ki = 5;
double kd = 0.1;

// Variables para el control PID
double setpoint = 90; // Angulo deseado del servo
double input, output;

// Definir objetos del PID, Servo y LCD
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
Servo myServo;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

void setup() {
  // Inicializar el puerto serie para el LCD
  lcd.begin(16, 2);

  // Inicializar el puerto serie para debug
  Serial.begin(9600);

  // Inicializar el potenciómetro y el servo
  pinMode(pinPotentiometer, INPUT);
  myServo.attach(pinServo);

  // Configurar el control PID
  myPID.SetMode(AUTOMATIC);

  // Mostrar mensaje de inicio en el LCD
  lcd.print("Posicion: ");
}

void loop() {
  // Leer el valor del potenciómetro y la posición actual del servo
  int potValue = analogRead(pinPotentiometer);
  input = map(potValue, 0, 1023, 0, 180);
  double currentPos = myServo.read();

  // Aplicar control PID
  myPID.Compute();

  // Actualizar la posición del servo
  myServo.write(output);

  // Mostrar información en el LCD
  lcd.setCursor(10, 0);
  lcd.print(currentPos);
  lcd.setCursor(0, 1);
  lcd.print("Deseado: ");
  lcd.print(setpoint);
  lcd.setCursor(10, 1);
  lcd.print("Actual: ");
  lcd.print(currentPos);

  // Mostrar información de PID en el puerto serie para debug
  Serial.print("Input: ");
  Serial.print(input);
  Serial.print(", Output: ");
  Serial.print(output);
  Serial.print(", Setpoint: ");
  Serial.println(setpoint);

  delay(100);
}