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

// LCD Configuration
LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C LCD at address 0x27

// Pin Definitions
const int trigPinBall = 7; // Ultrasonic sensor Trig
const int echoPinBall = 6; // Ultrasonic sensor Echo
const int fanPin = 9;      // PWM output for blower control
const int potPin = A0;     // Potentiometer for setting target position
const int buttonPin = 2;   // Push button to confirm target position
const int buzzerPin = 3;   // Buzzer for feedback

// Variables
float ballPosition = 0.0;      // Measured position of the ball
float targetPosition = 0.0;    // Live target position from potentiometer
float confirmedPosition = -1;  // Confirmed target position after pressing button
bool buttonPressed = false;    // Button state
bool positionConfirmed = false; // Position confirmation flag
unsigned long previousLCDMillis = 0; // Timer for LCD updates
const unsigned long lcdUpdateInterval = 100; // LCD update interval in ms

// PID Variables
double pidInput, pidOutput, pidSetpoint; // PID input, output, and target
double kp = 20.0, ki = 1.0, kd = 10.0;   // PID constants
PID ballPID(&pidInput, &pidOutput, &pidSetpoint, kp, ki, kd, REVERSE);

void setup() {
  // Pin Modes
  pinMode(trigPinBall, OUTPUT);
  pinMode(echoPinBall, INPUT);
  pinMode(fanPin, OUTPUT);
  pinMode(potPin, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);

  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Short beep on startup
  tone(buzzerPin, 1000, 200);

  // Initial LCD Message
  lcd.setCursor(0, 0);
  lcd.print("Init: Set Target");
  delay(1000);
  lcd.clear();

  // Initialize PID Controller
  ballPID.SetMode(AUTOMATIC);
  ballPID.SetOutputLimits(20, 255); // Fan speed range
  ballPID.SetSampleTime(50); // PID sample time in ms

  Serial.begin(9600); // Debugging output
}

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

  // Read button state
  int buttonState = digitalRead(buttonPin);

  if (!positionConfirmed) {
    // Read potentiometer value for live target position
    int potValue = analogRead(potPin);
    targetPosition = map(potValue, 0, 1023, 2, 16); // Map to 2-16 cm range

    // Measure ball position
    ballPosition = measureDistance();

    // Update LCD every 100ms
    if (currentMillis - previousLCDMillis >= lcdUpdateInterval) {
      previousLCDMillis = currentMillis;
      displayLiveValues(targetPosition, ballPosition);
    }

    // Confirm target position on button press
    if (buttonState == LOW && !buttonPressed) {
      buttonPressed = true;
      confirmedPosition = targetPosition;
      positionConfirmed = true;

      tone(buzzerPin, 1000, 1000); // Confirmation beep
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Confirmed:");
      lcd.print(confirmedPosition);
      lcd.print(" cm");
      delay(1000);
      lcd.clear();
    }
    if (buttonState == HIGH) buttonPressed = false;
  } else {
    // Measure ball position
    ballPosition = measureDistance();

    // Set PID input and setpoint
    pidInput = ballPosition;
    pidSetpoint = confirmedPosition;

    // Compute PID output
    ballPID.Compute();
    analogWrite(fanPin, (int)pidOutput); // Adjust fan speed

    // Update LCD with target and ball position
    if (currentMillis - previousLCDMillis >= lcdUpdateInterval) {
      previousLCDMillis = currentMillis;
      displayLiveValues(confirmedPosition, ballPosition);
    }

    // Debugging
    Serial.print("Ball Pos: ");
    Serial.print(ballPosition);
    Serial.print(" | Target: ");
    Serial.print(confirmedPosition);
    Serial.print(" | Fan Speed: ");
    Serial.println((int)pidOutput);
  }
}

void displayLiveValues(float target, float ball) {
  // Display target position and current ball position
  lcd.setCursor(0, 0);
  lcd.print("Target: ");
  lcd.print(target, 0);
  lcd.print(" cm   ");

  lcd.setCursor(0, 1);
  lcd.print("Ball: ");
  lcd.print(ball, 1);
  lcd.print(" cm   ");
}

float measureDistance() {
  // Measure distance using the ultrasonic sensor
  digitalWrite(trigPinBall, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinBall, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinBall, LOW);

  long duration = pulseIn(echoPinBall, HIGH, 30000); // Timeout after 30ms
  float distance = duration * 0.034 / 2.0; // Convert to cm

  if (distance < 2 || distance > 30) return 0; // Return 0 for out-of-range
  return distance;
}