#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

//Define the aggressive and conservative Tuning Parameters
double aggKp = 10, aggKi = 0.2, aggKd = 1;         // Aggressive values
double consKp = 1, consKi = 0.05, consKd = 0.25;  // Conservative values

//Specify the links and initial tuning parameters
PID ballPID(&pidInput, &pidOutput, &pidSetpoint, aggKp, aggKi, aggKd, 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;

    double gap = abs(pidSetpoint - pidInput);  //distance away from setpoint
    if (gap < 3) {                      //we're close to setpoint, use conservative tuning parameters
      ballPID.SetTunings(consKp, consKi, consKd);
    } else {
      //we're far from setpoint, use aggressive tuning parameters
      ballPID.SetTunings(aggKp, aggKi, aggKd);
    }

    // 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;
}