#include <LiquidCrystal_I2C.h>
#include <HX711.h>
#include <Servo.h>
#include "PID_v1.h"

// Definir constantes para los pines de conexión
#define DATA_OUT 4
#define SCK_OUT 3

#define SERVO_PIN 10
#define SETPOINT_PIN A0
#define START_PIN 11
#define STOP_PIN 12

// Iniciar LCD I2C con la dirección y tamaño de la pantalla
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Iniciar báscula HX711 con el factor de calibración
HX711 scale;
float calibrationFactor = 40;  // Cambiar este valor según la calibración de tu celda de carga

// Variables del PID
double setpoint, input, output;
double Kp = 2.5, Ki = 300, Kd = 0.05; // PI

// Iniciar Servo y PID
Servo srv;
PID pidcs(&input, &output, &setpoint, Kp, Ki, Kd, P_ON_E, DIRECT);

// Variables auxiliares
float sudut = 0.0;
boolean isRunning = false;

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

  // Configurar los pines
  pinMode(START_PIN, INPUT_PULLUP);
  pinMode(STOP_PIN, INPUT_PULLUP);

  // Configurar LCD
  lcd.begin(16, 2);
  lcd.init();
  lcd.backlight();

  // Configurar Servo
  srv.attach(SERVO_PIN);
  srv.write(180);

  // Configurar HX711
  scale.begin(DATA_OUT, SCK_OUT);
  scale.set_scale(calibrationFactor);

  // Mensaje inicial en la pantalla
  lcd.setCursor(0, 0);
  lcd.print("Selec. Tension");
  delay(2000);
  lcd.clear();

  // Configurar el PID
  pidcs.SetMode(AUTOMATIC);
  pidcs.SetOutputLimits(0, 180); // Limitar la salida del PID al rango del servo
}

void display() {
  static uint32_t last = 0;
  const int interval = 25; // Intervalo del proceso de impresión (200 ms)

  if (millis() - last > interval) {
    last += interval;

    // Mostrar setpoint en la pantalla
    lcd.setCursor(0, 0);
    lcd.print("F:");
    lcd.setCursor(0, 1);
    lcd.print(setpoint, 1);

    if (isRunning) {
      // Mostrar el valor sensado por la celda de carga cuando está en ejecución
      lcd.setCursor(4, 0);
      lcd.print("    FR:     ");
      lcd.setCursor(8, 1);
      lcd.print(input, 1);
    } else {
      // Mostrar "Press Start" cuando no está en ejecución
      lcd.setCursor(4, 0);
      lcd.print("Press Start  "); // Añadimos espacios para limpiar cualquier residuo
    }

    // Mostrar en el monitor serie
    Serial.print("Setpoint: ");
    Serial.print(setpoint, 1);
    Serial.print(" Force: ");
    Serial.print(input, 1);
    Serial.print(" Output: ");
    Serial.print(output/30, 1);
    Serial.println();
  }
}

void loop() {
  // Leer el valor del setpoint del potenciómetro (0 a 5 kg)
  setpoint = map(analogRead(SETPOINT_PIN), 0, 1023, 0, 5000) / 1000.0;
/*
  // Leer y acondicionar el valor de la celda de carga a 0-5 kg
  float raw_weight = scale.get_units();
  input = map(raw_weight, 0, calibrationFactor, 0, 5000) / 1000.0;
*/
//SIMULACION DE FEEDBACK

 //float angle = output; // El ángulo del servo (output del PID)
  // float force = 2.5 * (1 + cos(radians(output))); // Fuerza de frenado (0 a 5 kg)
  float force = output / 36.0; // Usé relación lineal en vez de afectarla por el coseno del ángulo del servo porque el programa falla con coseno
 // float force = 2.5 * (1 + cos(output)); // Fuerza de frenado (0 a 5 kg)
 input = force; // Usar la fuerza simulada como entrada del PID
//FIN DE SIMULACION DE FEEDBACK

  // Esperar el inicio del disparador
  if (digitalRead(START_PIN) == LOW) {
    isRunning = true;
    delay(200); // Debounce
  }

  // Detener el proceso al presionar el botón Stop
  if (digitalRead(STOP_PIN) == LOW) {
    isRunning = false;
    srv.write(180); // Mover el servo a una posición segura (por ejemplo, completamente abierta)
    delay(200); // Debounce
  }

  if (isRunning) {
    // Calcular la salida del PID y controlar el servo
    if (pidcs.Compute()) {
      srv.write(output);
      sudut = output; // Mantener el valor de salida del PID para el ángulo del servo
    }
  }

  display(); // Actualizar la pantalla LCD y el monitor serie
}