#include <ESP32Servo.h>

// --- 1. กำหนด Pin ของอุปกรณ์ ---
const int potenPin      = 14; // Slide Potentiometer (Analog)
const int servoPin      = 12; // Servo (PWM)
const int btnPin        = 27; // Button
const int ledPin        = 26; // Brake LED
const int trigPin       = 33;  // Ultrasonic Trig
const int echoPin       = 25;  // Ultrasonic Echo

// --- Global Variables ---
Servo servo;
int currentSpeed = 0;
int lastSpeed = -1;

bool isBraking = false;
unsigned long brakeStartTime = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("MiniExam: Speed Control System Initializing...");

  pinMode(potenPin, INPUT);
  pinMode(btnPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servo.attach(servoPin);
  servo.write(0);
  digitalWrite(ledPin, HIGH);
}

void loop() {
  // --- เบรค ---
  if (isBraking) {
    unsigned long elapsedTime = millis() - brakeStartTime;

    if (elapsedTime < 5000) {
      int brakingSpeed = map(5000 - elapsedTime, 0, 5000, 0, currentSpeed);
      updateSpeedometer(brakingSpeed);
    } else {
      isBraking = false;
      digitalWrite(ledPin, HIGH);
      Serial.println("Braking complete. Resuming normal operation.");
    }
    return;
  }

  // --- ตรวจสอบเงื่อนไขการเบรค ---
  float distance = readUltrasonicDistance();
  bool brakeButtonPressed = (digitalRead(btnPin) == LOW);

  if (distance <= 100 || brakeButtonPressed) {
    if (!isBraking) {
      isBraking = true;
      brakeStartTime = millis();
      digitalWrite(ledPin, HIGH);
      Serial.printf("BRAKE TRIGGERED! Obstacle at %.2f cm or Button Pressed.\n", distance);
    }
    return;
  }

  // --- ควบคุมความเร็วปกติ (คันเร่ง) ---
  int potenValue = analogRead(potenPin);
  currentSpeed = map(potenValue, 0, 4095, 0, 200);

  if (currentSpeed != lastSpeed) {
    updateSpeedometer(currentSpeed);
    lastSpeed = currentSpeed;
  }

  delay(50);
}

void updateSpeedometer(int speed) {
  int servoAngle = map(speed, 0, 200, 0, 180);
  servo.write(servoAngle);
  Serial.printf("Speed: %d km/hr.\n", speed);
}

float readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration / 58.2;
}