#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 = 0.1, Ki = 300, Kd = 0.0; // 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 = 10; // Intervalo del proceso de impresión (10 ms)

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

    // Buffer para almacenar los valores convertidos a cadenas
    char bufferSetpoint[10];
    char bufferInput[10];

    // Convertir los valores flotantes a cadenas con 3 decimales
    dtostrf(setpoint, 6, 3, bufferSetpoint);
    dtostrf(input, 6, 3, bufferInput);

    // Mostrar setpoint en la pantalla
    lcd.setCursor(0, 0);
    lcd.print("F:");
    lcd.setCursor(0, 1); // Ajustar la posición según sea necesario
    lcd.print(bufferSetpoint);

    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(bufferInput);
    } 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(bufferSetpoint);
    Serial.print(" Force: ");
    Serial.print(bufferInput);
    Serial.print(" Output: ");
    Serial.print(output / 30, 3); // Ajusta la precisión del output si es necesario
    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;
/* //Inicio de lectura de celda de carga física (deshabilitar para simularla sin tener que usar hardware)
  // 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;
*/ //Fin de lectura de celda de carga física

//SIMULACION DE FEEDBACK
// SIMULACION DE FEEDBACK
  // Ajustamos el output para que el coseno tenga un rango adecuado
  // Mapeamos el output (-180 0) a un rango más adecuado para la función coseno
  float scaledOutput = map(output, 0, 180, -180, 0);
  float force = 2.5 * (1 + cos(radians(scaledOutput))); // 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
}