#include <AFMotor.h>
#include <SoftwareSerial.h>

SoftwareSerial bluetoothSerial(9, 10);  // RX, TX

// Motors
AF_DCMotor motor1(1); 
AF_DCMotor motor2(2); 
AF_DCMotor motor3(3); 
AF_DCMotor motor4(4);

// Pins
#define HORN A5
#define BRAKELIGHT A1
#define HAZARDS A0
#define HEADLIGHTS A3
#define TRIG 13
#define ECHO 12

long duration;
int distance;

bool hazardOn = false;
unsigned long lastBlink = 0;
int blinkInterval = 500; // blink speed

void setup() {
  Serial.begin(9600);
  bluetoothSerial.begin(9600);

  pinMode(HORN, OUTPUT);
  pinMode(BRAKELIGHT, OUTPUT);
  pinMode(HAZARDS, OUTPUT);
  pinMode(HEADLIGHTS, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  stopMotors();
}

void loop() {
  // === Horn Auto-Beep when object close ===
  distance = getDistance();
  if (distance > 0 && distance <= 10) {
    digitalWrite(HORN, HIGH);
  } else {
    digitalWrite(HORN, LOW);
  }

  // === Hazard blinking ===
  if (hazardOn) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastBlink >= blinkInterval) {
      lastBlink = currentMillis;
      digitalWrite(HAZARDS, !digitalRead(HAZARDS));
    }
  }

  // === Bluetooth commands ===
  if (bluetoothSerial.available()) {
    char command = bluetoothSerial.read();
    Serial.println(command);

    stopMotors(); // reset motors before new command

    switch (command) {
      // ==== Movement (swapped) ====
      case 'F': left(); break;     
      case 'B': right(); break;    
      case 'L': forward(); break;  
      case 'R': back(); break;     

      // Brake light
      case 'U': digitalWrite(BRAKELIGHT, HIGH); break; // ON
      case 'u': digitalWrite(BRAKELIGHT, LOW);  break; // OFF

      // Hazards
      case 'X': hazardOn = true; break;
      case 'x': hazardOn = false; digitalWrite(HAZARDS, LOW); break;

      // Headlights
      case 'W': digitalWrite(HEADLIGHTS, HIGH); break; // ON
      case 'w': digitalWrite(HEADLIGHTS, LOW);  break; // OFF

      // Emergency stop
      case 'S': stopMotors(); break;
    }
  }
}

// === Distance ===
int getDistance() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  duration = pulseIn(ECHO, HIGH, 20000); 
  if (duration == 0) return -1;
  return duration / 58.2;
}

// === Movement ===
void forward() {
  setMotorSpeed(200);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

void back() {
  setMotorSpeed(200);
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}

void left() {
  setMotorSpeed(200);
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

void right() {
  setMotorSpeed(200);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
}

void stopMotors() {
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}

void setMotorSpeed(int spd) {
  motor1.setSpeed(spd);
  motor2.setSpeed(spd);
  motor3.setSpeed(spd);
  motor4.setSpeed(spd);
}