#include <ESP32Servo.h>

// Pin definitions for the components
const int potenPin      = 33;  // Analog pin for the potentiometer (accelerator)
const int servoPin      = 25;  // PWM pin for the servo motor (speedometer)
const int btnPin        = 26;  // Digital pin for the button (brake)
const int ledPin        = 27;  // Digital pin for the LED (brake light)
const int trigPin       = 14;  // Digital pin for the ultrasonic sensor's trigger
const int echoPin       = 12;  // Digital pin for the ultrasonic sensor's echo

// Create a Servo object
Servo servo;

// Global variables to manage the system state
int currentSpeed = 0;           // The current simulated speed
int lastSpeed = -1;             // Stores the last printed speed to avoid duplicates
int brakingStartSpeed = 0;      // The speed at which braking began
int preBrakeSpeed = 0;          // Stores the speed just before braking

bool isBraking = false;         // Flag to indicate if the system is currently braking
unsigned long brakeStartTime = 0; // Stores the timestamp when braking started

// Variables for smoother operation
float smoothedPotenValue = 0.0; // Filtered potentiometer value
const float smoothingFactor = 0.1; // Smoothing factor for the EMA filter
unsigned long lastUpdateTime = 0;  // Stores the last time the speed was updated
const unsigned long updateInterval = 50; // Interval for speed updates (in ms)

// Function prototypes
void updateSpeedometer(int speed);
float readUltrasonicDistance();

void setup() {
  // Initialize Serial communication for debugging
  Serial.begin(115200);
  Serial.println("MiniExam: Speed Control System Initializing...");

  // Configure pin modes
  pinMode(potenPin, INPUT);
  pinMode(btnPin, INPUT_PULLUP); // Use internal pull-up resistor for the button
  pinMode(ledPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Attach the servo motor to the specified pin
  servo.attach(servoPin);

  // Set initial state
  servo.write(0);
  digitalWrite(ledPin, LOW);
}

void loop() {
  // Check if the system is currently in the braking state
  if (isBraking) {
    unsigned long elapsedTime = millis() - brakeStartTime;

    // Check if the 5-second braking period is still active
    if (elapsedTime < 5000) {
      // Use a smoother "ease-out" curve for deceleration
      // The `progress` variable goes from 0.0 to 1.0 over 5 seconds
      float progress = (float)elapsedTime / 5000.0;
      float easedProgress = 1.0 - (1.0 - progress) * (1.0 - progress);
      currentSpeed = (int)(brakingStartSpeed * (1.0 - easedProgress));
      
      // Check for speed change before updating
      if (currentSpeed != lastSpeed) {
        updateSpeedometer(currentSpeed);
        lastSpeed = currentSpeed;
      }
    } else {
      // Braking period is over, now decide what to do
      isBraking = false; // Stop the braking state
      digitalWrite(ledPin, LOW); // Turn off the brake light
      Serial.println("Braking complete. Resuming normal operation.");

      // Read current conditions to see if brake is still needed
      if (readUltrasonicDistance() > 100 && digitalRead(btnPin) == HIGH) {
        // If the brake is no longer needed, return to the speed before braking
        currentSpeed = preBrakeSpeed;
        // Check for speed change before updating
        if (currentSpeed != lastSpeed) {
          updateSpeedometer(currentSpeed);
          lastSpeed = currentSpeed; // Update lastSpeed to prevent redundant output
        }
      } else {
        // If the brake condition (obstacle or button) is still active, stay at 0 speed
        currentSpeed = 0;
        // Check for speed change before updating
        if (currentSpeed != lastSpeed) {
          updateSpeedometer(currentSpeed);
          lastSpeed = currentSpeed;
        }
      }
    }
    // Return to the top of the loop to prevent other code from running during braking
    return;
  }

  // Read the distance from the ultrasonic sensor
  float distance = readUltrasonicDistance();
  // Check if the button is pressed (LOW due to INPUT_PULLUP)
  bool brakeButtonPressed = (digitalRead(btnPin) == LOW);

  // Check for braking conditions (obstacle within 100cm OR button pressed)
  if (distance <= 100 || brakeButtonPressed) {
    if (!isBraking) { // Trigger braking only if it's not already active
      isBraking = true;
      brakeStartTime = millis(); // Record the start time of the brake
      digitalWrite(ledPin, HIGH); // Turn on the brake light LED
      brakingStartSpeed = currentSpeed; // Store the current speed for deceleration
      preBrakeSpeed = currentSpeed; // Store the speed to return to later
      Serial.printf("BRAKE TRIGGERED! Obstacle at %.2f cm or Button Pressed.\n", distance);
    }
    // Return to the top of the loop
    return;
  }

  // Normal operation (no braking)
  unsigned long currentTime = millis();
  if (currentTime - lastUpdateTime >= updateInterval) {
    lastUpdateTime = currentTime;

    // Read raw potentiometer value
    int potenValue = analogRead(potenPin);

    // Use an Exponential Moving Average (EMA) filter for a smoother response
    if (smoothedPotenValue == 0.0) {
      // Initialize the filter on the first read
      smoothedPotenValue = (float)potenValue;
    } else {
      smoothedPotenValue = (potenValue * smoothingFactor) + (smoothedPotenValue * (1.0 - smoothingFactor));
    }

    // Map the smoothed value to the speed range
    currentSpeed = map((int)smoothedPotenValue, 0, 4095, 0, 200);

    // Only update the speedometer and print if the speed has changed
    if (currentSpeed != lastSpeed) {
      updateSpeedometer(currentSpeed);
      lastSpeed = currentSpeed;
    }
  }
}

/**
 * @brief Updates the servo's angle to represent the speed.
 * @param speed The current speed in km/hr.
 */
void updateSpeedometer(int speed) {
  // Map the speed value (0-200) to the servo's angle (0-180)
  int servoAngle = map(speed, 0, 200, 0, 180);
  servo.write(servoAngle);
  // Print the speed to the Serial Monitor
  Serial.printf("Speed: %d km/hr.\n", speed);
}

/**
 * @brief Reads the distance from the ultrasonic sensor.
 * @return The distance in centimeters.
 */
float readUltrasonicDistance() {
  // Clear the trig pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Set the trig pin HIGH for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echo pin and calculate the distance
  long duration = pulseIn(echoPin, HIGH);
  return duration / 58.2;
}