#include <PID_v2.h>
#include <LiquidCrystal_I2C.h>
#include <Encoder.h>
#include <TimerOne.h>

#define ENCODER_PINA 2
#define ENCODER_PINB 3
#define ENCODER_BUTTON 4

LiquidCrystal_I2C lcd(0x27, 20, 4);  // I2C display address and number of columns and rows
double Setpoint, Input, Output;
double Kp = 2, Ki = 5, Kd = 1;  // PID parameter values
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

Encoder encoder(ENCODER_PINA, ENCODER_PINB);
int16_t last_encoder_value = 0;

unsigned long last_lcd_update_time = 0;         // Variable to store the last display update time
const unsigned long lcd_update_interval = 500;  // Interval between display updates, in milliseconds

int pid_mode = 1;      // Initialize PID mode as automatic (1)
int current_page = 0;  // Initialize current page as 1

void timerIsr() {
  encoder.read();
}

void setup() {
  pinMode(ENCODER_PINA, INPUT);
  pinMode(ENCODER_PINB, INPUT);
  pinMode(ENCODER_BUTTON, INPUT);
  lcd.init();       // Initialize the display
  lcd.backlight();  // Turn on the display backlight
  lcd.clear();
  Serial.begin(115200);

  Setpoint = 200;  // Define the setpoint

  myPID.SetMode(pid_mode);   // Set the PID operation mode to automatic or manual
  myPID.SetSampleTime(100);  // Set the sampling time in milliseconds

  Timer1.initialize(1000);           // Initialize the timer with a 1ms period
  Timer1.attachInterrupt(timerIsr);  // Attach the encoder service function to the timer
}

void loop() {
  Input = analogRead(A0);  // Read the value from the analog input

  // Read the encoder value and adjust the PID parameter values or change the PID mode accordingly
  int16_t encoder_value = (encoder.read() / 4);
  


  Serial.println(encoder_value);
  if (encoder_value != last_encoder_value) {
    if (current_page == 1) {
      if (encoder_value > last_encoder_value) {
        Setpoint += 1;
      } else {
        Setpoint -= 1;
      }
    } else if (current_page == 2) {
      if (encoder_value > last_encoder_value) {
        Kp += 0.1;
      } else {
        Kp -= 0.1;
      }
    } else if (current_page == 3) {
      if (encoder_value > last_encoder_value) {
        Ki += 0.1;
      } else {
        Ki -= 0.1;
      }
    } else if (current_page == 4) {
      if (encoder_value > last_encoder_value) {
        Kd += 0.1;
      } else {
        Kd -= 0.1;
      }
    } else if (current_page == 5) {
      pid_mode = !pid_mode;     // Toggle between automatic and manual modes (0 or 1)
      myPID.SetMode(pid_mode);  // Set the new mode in the PID
    }
    last_encoder_value = encoder_value;
  }

  myPID.SetTunings(Kp, Ki, Kd);  // Set the PID parameter values

  myPID.Compute();  // Compute the PID output

  analogWrite(9, Output);  // Write the analog output value to pin 9

  // Update the display only at the defined interval
  if (millis() - last_lcd_update_time >= lcd_update_interval) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Setpoint: ");
    lcd.print(Setpoint);
    lcd.setCursor(0, 1);
    lcd.print("Input: ");
    lcd.print(Input);
    lcd.setCursor(0, 2);
    lcd.print("Output: ");
    lcd.print(Output);
    lcd.setCursor(0, 3);
    lcd.print("Mode: ");
    if (pid_mode == 1) {
      lcd.print("Auto");
    } else {
      lcd.print("Manual");
    }
    last_lcd_update_time = millis();
  }

  // If the encoder button is pressed, change to the next page
  if (digitalRead(ENCODER_BUTTON) == LOW) {
    current_page += 1;
    if (current_page > 5) {
      current_page = 1;
    }
    delay(250);  // Debounce the button
  }

  // Update the display according to the current page
  switch (current_page) {
    case 1:
      lcd.setCursor(0, 0);
      lcd.print("Setpoint: ");
      lcd.print(Setpoint);
      break;
    case 2:
      lcd.setCursor(0, 0);
      lcd.print("Kp: ");
      lcd.print(Kp);
      break;
    case 3:
      lcd.setCursor(0, 0);
      lcd.print("Ki: ");
      lcd.print(Ki);
      break;
    case 4:
      lcd.setCursor(0, 0);
      lcd.print("Kd: ");
      lcd.print(Kd);
      break;
    case 5:
      lcd.setCursor(0, 0);
      lcd.print("Mode: ");
      if (pid_mode == 1) {
        lcd.print("Auto");
      } else {
        lcd.print("Manual");
      }
      break;
  }
}