// FrenoPIDSim02

#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 = 100, Ki = 0.009, Kd = 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 = 200; // 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(" Force1: ");
    Serial.print(input, 1);
    Serial.print(" PIDOut: ");
    Serial.print(output / 30, 1); // Mantener la división por 30 para la escala
    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;

  //----Esta parte debe omitirse si se quiere simular la realimentación------
  // 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;
  //----Hasta aquí debe omitirse si se quiere simular la realimentación------

  // Esperar el inicio del disparador
  if (digitalRead(START_PIN) == LOW) {
    isRunning = true;
    Serial.println("Sistema iniciado");
    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)
    Serial.println("Sistema detenido");
    delay(200); // Debounce
  }

  // Simulación del sistema (para pruebas sin hardware)
  // Comienzo de la simulación
  if (isRunning) {
    float angle = output; // El ángulo del servo (output del PID)
    float force = 2.5 * (1 + cos(radians(angle))); // Fuerza de frenado (0 a 5 kg)
    input = force; // Usar la fuerza simulada como entrada del PID

    // 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
    }


    // Añadir mensajes de depuración
    /*
    Serial.print(" Angle2: ");
    Serial.println(angle / 30); // Mantener la división por 30 para la escala
    Serial.print(" CalcForce2: ");
    Serial.println(force);
    Serial.print(" PIDOut2: ");
    Serial.println(output / 30); // Mantener la división por 30 para la escala
    */
    // Mostrar en el monitor serie
    
    Serial.print("Setpoint: ");
    Serial.print(setpoint, 1);
    Serial.print(" Force1: ");
    Serial.print(input, 1);
    Serial.print(" PIDOut: ");
    Serial.print(output / 30, 1); // Mantener la división por 30 para la escala
    Serial.println();
  }
  // Fin de la simulación

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