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

// Define PID constants
double Kp = 1.0;
double Ki = 0.5;
double Kd = 0.2;

// Define input, output, and setpoint variables
double input, output, setpoint;

// Create PID object
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// Create LCD object
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Setup function
void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize I2C display
  lcd.init();
  lcd.backlight();

  // Set up PID object
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(0, 255); // Set output limits for PWM
  pid.SetSampleTime(10); // Set sample time to 10 ms

  // Set initial setpoint and output values
  setpoint = 50;
  output = 0;
}

// Loop function
void loop() {
  // Read input value
  input = analogRead(A0);

  // Compute PID output
  pid.Compute();

  // Send output to actuator
  analogWrite(9, output);

  // Print debug information to serial monitor
  Serial.print("Input: ");
  Serial.print(input);
  Serial.print(", Output: ");
  Serial.println(output);

  // Update display
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Input: ");
  lcd.print(input);
  lcd.setCursor(0, 1);
  lcd.print("Output: ");
  lcd.print(output);

  // Wait for a short period before repeating loop
  delay(10);
}