#include <ESP32Servo.h>

// --- Pin Configuration ---
const int potenPin = 12; 
const int servoPin = 14;
const int btnPin   = 27; 
const int ledPin   = 26;
const int trigPin  = 25;
const int echoPin  = 33;

// --- Global Variables ---
Servo servo;
int currentSpeed = 0;
int lastSpeed = -1;
int targetSpeed = 0;   // เก็บค่าความเร็วที่คันเร่งต้องการ
int startBrakeSpeed = 0;

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) {
      // คำนวณความเร็วที่ลดลงแบบ linear ภายใน 5 วิ
      int brakingSpeed = startBrakeSpeed - (startBrakeSpeed * elapsedTime / 5000);
      if (brakingSpeed < 0) brakingSpeed = 0;
      updateSpeedometer(brakingSpeed);
    } else {
      isBraking = false;
      digitalWrite(ledPin, LOW);
      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();
      startBrakeSpeed = currentSpeed; // จำค่าก่อนเบรค
      digitalWrite(ledPin, HIGH);
      Serial.printf("BRAKE TRIGGERED! Obstacle at %.2f cm or Button Pressed.\n", distance);
    }
    return;
  }

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

  // --- แสดงผลเมื่อความเร็วเปลี่ยน ---
  if (targetSpeed != lastSpeed) {
    updateSpeedometer(targetSpeed);
    lastSpeed = targetSpeed;
  }

  delay(50);
}

// --- Update Servo + Serial ---
void updateSpeedometer(int speed) {
  currentSpeed = speed;
  int servoAngle = map(speed, 0, 200, 0, 180);
  servo.write(servoAngle);

  Serial.printf("Speed: %d km/hr.\n", speed);
}

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