#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <PID_v1.h>
#include "MassaTermicaSimulation.h"

// Dichiarazione delle variabili globali
double Setpoint = 130.0; // Temperatura di riferimento
double Input, Output; // Variabili per il PID
double Kp = 5.0, Ki = 0.25, Kd = 0.1; // Costanti del PID
double outMin = 0, outMax = 180; // Limiti dell'uscita del PID

unsigned long lastTime, SampleTime = 1000; // Variabili per il campionamento temporale

Servo heaterServo; // Oggetto servo per controllare il servo motore
LiquidCrystal_I2C lcd(0x27, 20, 4); // Oggetto LCD per la visualizzazione
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); // Oggetto PID

void setup() {
  // Inizializzazione del display LCD e del servo
  lcd.init();
  lcd.backlight();
  heaterServo.attach(9);

  // Inizializzazione della simulazione della massa termica
  initMassaTermicaSimulation();

  // Impostazione del PID in modalità automatica e dei limiti dell'uscita
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(outMin, outMax);
}

void loop() {
  unsigned long now = millis();

  // Verifica se è il momento di eseguire un nuovo ciclo del PID
  if (now - lastTime >= SampleTime) {
    // Lettura della temperatura attuale
    Input = getMassaTermica();

    // Calcolo dell'errore rispetto alla temperatura di riferimento
    double error = Setpoint - Input;

    // Esecuzione del PID 

      pid.Compute(); // Calcolo dell'uscita del PID

      // Mappatura dell'uscita del PID nell'intervallo del servo motore
      Output = map(Output, outMin, outMax, 0, 180);

      // Controllo del servo motore e aggiornamento della simulazione della massa termica
      heaterServo.write(Output);
      updateMassaTermica(Output);


    // Aggiornamento del display LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Temp: ");
    lcd.print(getMassaTermica());
    lcd.print(" C");

    lcd.setCursor(0, 1);
    lcd.print("Posizione: ");
    lcd.print(Output);
    lcd.print((char)223);

    
    // Memorizza il tempo dell'ultimo ciclo del PID
    lastTime = now;
  }
}