#include "BluetoothSerial.h"
#include <ESP32Servo.h>

BluetoothSerial SerialBT;

int motor1F = 18;  // MOTOR1 FORWARD
int motor1B = 19;  // MOTOR1 BACKWARD
int motor2F = 15;
int motor2B = 16;

#define PIN_Servo1 13  // SG90 DATA PIN
#define PIN_Servo2 14  // Valid GPIO pin
#define trig_F_Right 22
#define echo_F_Right 23
const int irSensorPin = 2; // OUT pin of the IR sensor

#define SOUND_SPEED 0.034
long duration_us;
float distanceCm;
bool autoparking = false;
int autoparkingState = 0; // Autoparking state
int sensorValue = 0;      // Stores IR sensor readings

Servo myServo1;
Servo myServo2; // Retained in case it's needed
unsigned long previousMillis = 0; // For non-blocking timing
unsigned long backMillis = 0;     // For non-blocking back logic
const unsigned long backInterval = 50; // Time interval for IR sensor checks
char lastCommand = '\0';  // Last Bluetooth command
bool isBacking = false;   // Indicates if the car is currently backing up

void setup() {
  Serial.begin(115200);           
  SerialBT.begin("autoparking_car_3 test");  // Device name
  SerialBT.println("Connected..."); 

  myServo1.attach(PIN_Servo1);
  myServo1.write(95); // Neutral position
  myServo2.attach(PIN_Servo2);

  pinMode(motor1F, OUTPUT);
  pinMode(motor1B, OUTPUT);
  pinMode(motor2F, OUTPUT);
  pinMode(motor2B, OUTPUT);
  pinMode(trig_F_Right, OUTPUT);
  pinMode(echo_F_Right, INPUT);
  pinMode(irSensorPin, INPUT); // IR sensor pin
}

void readBluetoothData() {
  if (SerialBT.available()) {
    char c = SerialBT.read();
    SerialBT.println(c);
    if (c == lastCommand) return; // Ignore repeated commands

    if (c == '0' && autoparking) return; // Ignore '0' during autoparking

    lastCommand = c;

    switch (c) {
      case '1': Left(); break;
      case '2': Leftup(); break;
      case '3': Ahead(); break;
      case '4': Rightup(); break;
      case '5': Right(); break;
      case '6': Slow(); break;
      case '7': Normal(); break;
      case '8': Fast(); break;
      case 'A': Forward(); break;
      case 'a': Forwards(); break;
      case 'B': StartBack(); break;
      case 'b': Stop(); isBacking = false; break;
      case 'C': Stop(); break;
      case '0': Autoparking(); break;
    }
  }
}

void Ahead() { myServo1.write(95); }
void Left() { myServo1.write(40); }
void Right() { myServo1.write(140); }
void Leftup() { myServo1.write(65); }
void Rightup() { myServo1.write(125); }
void Slow() { myServo2.write(0); }
void Normal() { myServo2.write(90); }
void Fast() { myServo2.write(180); }

void Forward() {
  digitalWrite(motor1F, HIGH); digitalWrite(motor1B, LOW);
  digitalWrite(motor2F, HIGH); digitalWrite(motor2B, LOW);
}

void Forwards() {
  digitalWrite(motor1F, LOW); digitalWrite(motor1B, LOW);
  digitalWrite(motor2F, LOW); digitalWrite(motor2B, LOW);
}

void StartBack() {
  Serial.println("Starting Backward Movement");
  isBacking = true;
}

void Back() {
  // Continually check the IR sensor while backing up
  unsigned long currentMillis = millis();
  if (currentMillis - backMillis >= backInterval) {
    backMillis = currentMillis;
    sensorValue = digitalRead(irSensorPin); // Read IR sensor
    Serial.print("IR Sensor Value: ");
    Serial.println(sensorValue);

    if (sensorValue == LOW) { // Object detected
      Serial.println("Object detected! Stopping...");
      digitalWrite(motor1F, HIGH); digitalWrite(motor1B, LOW);
      digitalWrite(motor2F, HIGH); digitalWrite(motor2B, LOW);
      delay(150);
      Stop(); // Call Stop to stop the car
      isBacking = false; // Stop backing logic
    } else {
      // Continue moving backward
      digitalWrite(motor1F, LOW); digitalWrite(motor1B, HIGH);
      digitalWrite(motor2F, LOW); digitalWrite(motor2B, HIGH);
    }
  }
}

void Stop() {
  digitalWrite(motor1F, LOW); digitalWrite(motor1B, LOW);
  digitalWrite(motor2F, LOW); digitalWrite(motor2B, LOW);
}

void Autoparking() {
  autoparking = true;
  autoparkingState = 1;
  Serial.println("Starting autoparking...");
}

void processAutoparking() {
  if (!autoparking) return;

  unsigned long currentMillis = millis();

  switch (autoparkingState) {
    case 1: // Measure distance
      digitalWrite(motor1F, HIGH); digitalWrite(motor1B, LOW);
      digitalWrite(motor2F, HIGH); digitalWrite(motor2B, LOW);
      digitalWrite(trig_F_Right, LOW);
      delayMicroseconds(2);
      digitalWrite(trig_F_Right, HIGH);
      delayMicroseconds(10);
      digitalWrite(trig_F_Right, LOW);

      duration_us = pulseIn(echo_F_Right, HIGH);
      distanceCm = duration_us * SOUND_SPEED / 2;

      if (distanceCm <= 3.5) autoparkingState = 2;
      break;

    case 2: // First phase of movement
      if (currentMillis - previousMillis >= 500) {
        //previousMillis = currentMillis;
        Forward();
        delay(400);
        myServo1.write(140);
        autoparkingState = 3;
      }
      break;

    case 3: // Second phase of movement
      if (currentMillis - previousMillis >= 1200) {
        //previousMillis = currentMillis;
        Back();
        delay(1200);
        myServo1.write(40);
        autoparkingState = 4;
      }
      break;

    case 4: // Final phase
      sensorValue = digitalRead(irSensorPin);
      if (sensorValue == LOW) {
        Stop();
        Serial.println("Autoparking complete!");
        myServo1.write(95);
        autoparking = false;
        autoparkingState = 0;
      } else {
        Back();
      }
      break;
  }
}

void loop() {
  readBluetoothData();

  // Process backward movement if triggered
  if (isBacking) {
    Back();
  }

  // Process autoparking if active
  processAutoparking();
}

