#include <Servo.h>

#define TRIGGER_PIN_FRONT 2        // Trigger pin for front ultrasonic sensors (shared)
#define ECHO_PIN_FRONT_LEFT 3      // Echo pin of front sensor on the left side
#define ECHO_PIN_FRONT_RIGHT 4     // Echo pin of front sensor on the right side

#define TRIGGER_PIN_REAR_BACK 5    // Trigger pin for rear sensor at the back of the car
#define ECHO_PIN_REAR_BACK 6       // Echo pin of rear sensor at the back of the car
#define TRIGGER_PIN_REAR 7         // Trigger pin for rear ultrasonic sensors (shared)
#define ECHO_PIN_REAR_LEFT 8       // Echo pin of rear sensor on the left side
#define ECHO_PIN_REAR_RIGHT 9      // Echo pin of rear sensor on the right side

#define SERVO_PIN 10               // Pin for servo motor control
#define LED_PIN_1 11               // Pin for LED 1
#define LED_PIN_2 12               // Pin for LED 2

#define DISTANCE_THRESHOLD 20      // Minimum distance threshold to consider a spot as open (in cm)
#define MOVEMENT_SPEED 100         // Speed of the movement (adjust as needed)
#define TURN_ANGLE_RIGHT 45        // Angle to turn the steering to the right (adjust as needed)
#define TURN_ANGLE_LEFT 30         // Angle to turn the steering to the left (adjust as needed)

Servo steeringServo;

void setup() {
  Serial.begin(9600);
  
  // Initialize servo
  steeringServo.attach(SERVO_PIN);
  
  // Initialize ultrasonic sensors
  pinMode(TRIGGER_PIN_FRONT, OUTPUT);
  pinMode(ECHO_PIN_FRONT_LEFT, INPUT);
  pinMode(ECHO_PIN_FRONT_RIGHT, INPUT);
  
  pinMode(TRIGGER_PIN_REAR, OUTPUT);
  pinMode(TRIGGER_PIN_REAR_BACK, OUTPUT);
  pinMode(ECHO_PIN_REAR_LEFT, INPUT);
  pinMode(ECHO_PIN_REAR_RIGHT, INPUT);
  pinMode(ECHO_PIN_REAR_BACK, INPUT);

  // Initialize LEDs
  pinMode(LED_PIN_1, OUTPUT);
  pinMode(LED_PIN_2, OUTPUT);
}

void loop() {
  Serial.println("Test Code");
  // Fire both front and rear sensors at the same time
  digitalWrite(TRIGGER_PIN_FRONT, LOW);
  digitalWrite(TRIGGER_PIN_REAR, LOW);
  digitalWrite(TRIGGER_PIN_REAR_BACK, LOW);
  delayMicroseconds(10); // Wait for the sensors to settle
  digitalWrite(TRIGGER_PIN_FRONT, HIGH);
  digitalWrite(TRIGGER_PIN_REAR, HIGH);
  digitalWrite(TRIGGER_PIN_REAR_BACK, HIGH);

  // Measure the duration of the pulse for both front sensors
  long durationFrontLeft = pulseIn(ECHO_PIN_FRONT_LEFT, HIGH);
  // delayMicroseconds(10);
  long durationFrontRight = pulseIn(ECHO_PIN_FRONT_RIGHT, HIGH);
  // delayMicroseconds(10);

  // Measure the duration of the pulse for both rear sensors
  long durationRearLeft = pulseIn(ECHO_PIN_REAR_LEFT, HIGH);
  // delayMicroseconds(10);
  long durationRearRight = pulseIn(ECHO_PIN_REAR_RIGHT, HIGH);
  // delayMicroseconds(10);

  long durationRearBack = pulseIn(ECHO_PIN_REAR_BACK, HIGH);
  // delayMicroseconds(10);

  // Calculate the distance for both front sensors
  int distanceFrontLeft = durationFrontLeft * 0.034 / 2; // Divide by 2 for one-way distance
  Serial.println(distanceFrontLeft);
  int distanceFrontRight = durationFrontRight * 0.034 / 2; // Divide by 2 for one-way 
  Serial.println(distanceFrontRight);

  // Calculate the distance for both rear sensors
  int distanceRearLeft = durationRearLeft * 0.034 / 2; // Divide by 2 for one-way distance
  Serial.println(distanceRearLeft);
  int distanceRearRight = durationRearRight * 0.034 / 2; // Divide by 2 for one-way distance
  Serial.println(distanceRearRight);

  int distanceRearBack = durationRearBack * 0.034 / 2; // Divide by 2 for one-way distance

  // Check if there is an open space on the left side
  if (distanceFrontLeft > DISTANCE_THRESHOLD && distanceRearLeft > DISTANCE_THRESHOLD) {
    // Move forward
    moveForward();
    
    // Turn steering to the right
    turnSteering(TURN_ANGLE_RIGHT);
    
    // Move in reverse
    moveReverse();
    
    // Stop
    stop();
  } 
  // Check if there is an open space on the right side
  else if (distanceFrontRight > DISTANCE_THRESHOLD && distanceRearRight > DISTANCE_THRESHOLD) {
    // Move forward
    moveForward();
    
    // Turn steering to the left
    turnSteering(TURN_ANGLE_LEFT);
    
    // Move in reverse
    moveReverse();
    
    // Stop
    stop();
  }
}

void moveForward() {
  digitalWrite(LED_PIN_1, HIGH); // Turn on LED 1
  digitalWrite(LED_PIN_2, LOW);  // Turn off LED 2
  Serial.println("Forward");
  // Code to move the car forward
}

void moveReverse() {
  Serial.print("Reverse");
  digitalWrite(LED_PIN_1, LOW);  // Turn off LED 1
  digitalWrite(LED_PIN_2, HIGH); // Turn on LED 2
  // Code to move the car in reverse
}

void turnSteering(int angle) {
  steeringServo.write(angle);
  delay(1000); // Adjust delay as needed based on the servo speed
}

void stop() {
  // Code to stop the car
}