#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Dichiarazioni PID
double Setpoint = 150.0; // Temperatura desiderata in gradi Celsius
double Kp = 1.5;        // Costante proporzionale  2.0; 
double Ki = 0.6;        // Costante integrale  0.1
double Kd = 1.0;        // Costante derivativa  1.0

// Variabili PID
double Input, Output;
double ITerm, lastInput;
unsigned long lastTime;
double SampleTime = 1000; // Intervallo di campionamento in millisecondi
double outMin = 0;        // Limite inferiore dell'uscita
double outMax = 180;      // Limite superiore dell'uscita

Servo servo;

// Inizializzazione dell'LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  // Inizializzazione dell'LCD I2C
  lcd.init();
  lcd.backlight();

  // Inizializzazione del servo
  servo.attach(9);

  // Inizializzazione del PID
  Input = analogRead(A0);
  lastInput = Input;
  lastTime = millis();
}

void loop() {
  unsigned long now = millis();
  unsigned long timeChange = (now - lastTime);

  if (timeChange >= SampleTime) {
    // Calcolo del PID
    double error = Setpoint - Input;
    ITerm += (Ki * error);
    if (ITerm > outMax) ITerm = outMax;
    else if (ITerm < outMin) ITerm = outMin;
    double dInput = (Input - lastInput);

    // Calcolo dell'uscita PID
    Output = Kp * error + ITerm - Kd * dInput;
    if (Output > outMax) Output = outMax;
    else if (Output < outMin) Output = outMin;

    // Scrittura dell'uscita sul servo
    servo.write(Output);

    // Memorizzazione dei valori per il prossimo ciclo
    lastInput = Input;
    lastTime = now;
  }

  // Lettura della temperatura
  Input = analogRead(A0);
  // Conversione della lettura del sensore in gradi Celsius
  Input = (Input * 5.0) / 1024.0;
  Input = (Input - 0.5) * 100.0;

  // Visualizza le temperature sull'LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Temp: ");
  lcd.print(Input);
  lcd.print(" C");

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

    // Aggiorna la temperatura ogni 2 secondi
  delay(2000);
}