// #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 120        // Angle to turn the steering to the right (adjust as needed)
// #define TURN_ANGLE_LEFT 0         // 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() {
//   // Measure front left distance
//   Serial.println("Loop Starts");
//   int distanceFrontLeft = measureDistance(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT_LEFT);
//   Serial.println(String(distanceFrontLeft) + " Left_Front");
//   delay(50);
  
//   // Measure rear left distance
//   int distanceRearLeft = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_LEFT);
//   Serial.println(String(distanceRearLeft) + " Left_Rear");
//   delay(50);
  
//   // Measure front right distance
//   int distanceFrontRight = measureDistance(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT_RIGHT);
//   Serial.println(String(distanceFrontRight) + " Right_Front");
//   delay(50);
  
//   // Measure rear right distance
//   int distanceRearRight = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_RIGHT);
//   Serial.println(String(distanceRearRight) + " Right_Rear");
//   delay(50);
  
//   // Measure rear back distance
//   int distanceRearBack = measureDistance(TRIGGER_PIN_REAR_BACK, ECHO_PIN_REAR_BACK);
//   Serial.println(String(distanceRearBack) + " Rear_Back");
//   delay(50);

// }

// int measureDistance(int triggerPin, int echoPin) {
//   digitalWrite(triggerPin, LOW);
//   delayMicroseconds(2);
//   digitalWrite(triggerPin, HIGH);
//   delayMicroseconds(5);
//   digitalWrite(triggerPin, LOW);
//   long duration = pulseIn(echoPin, HIGH);
//   int distance = duration * (0.034 / 2); // Calculate distance in cm
//   return distance;
//   delay(10);
// }

// 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.println("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
// }



#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 SEARCH_SPEED 250           // Speed for searching car (adjust as needed)
#define TURN_ANGLE_RIGHT 120       // Angle to turn the steering to the right (adjust as needed)
#define TURN_ANGLE_LEFT 0          // Angle to turn the steering to the left (adjust as needed)
#define SEARCH_DURATION 2000       // Duration to search for a car before reducing speed (in milliseconds)

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() {
  search_car();
}



void search_car() {
  Serial.println("Searching for car lane");
  unsigned long startTime = millis();
  bool carDetected = false;
  while (true) {
    // Measure front left distance
    int distanceFrontLeft = measureDistance(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT_LEFT);
    if (distanceFrontLeft < 30) {
      // Car detected
      if (!carDetected) {
        // Start timer
        startTime = millis();
        carDetected = true;
      } if (millis() - startTime >= 2000) {
        // Car detected for more than 2 seconds, move to parking_spot_search
        moveForward(150);
        // Serial.println("Car Lane found..");
        parking_spot_search();
        // carDetected = true;
        // return;
      }
    }
    // Move forward with search speed
    moveForward(SEARCH_SPEED);
  }
}




// void parking_spot_search() {
//   Serial.println("Searching for parking spot");
//   unsigned long startTime = millis(); // Set the start time at the beginning
//   bool rearSensorActivated = false;
//   bool carStopped = false;
//   int initialDistance = 0;
//   int distanceMoved = 0;

//   while (true) {
//     // Measure left front distance
//     int distanceLeftFront = measureDistance(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT_LEFT);

//     if (distanceLeftFront > 60) {
//       // Reduce speed to 100 and start timer only if this is the initial distance check
//       if (initialDistance == 0) {
//         moveForward(100);
//         startTime = millis();
//         Serial.println("ST " + String(startTime));
//         initialDistance = distanceLeftFront;
//       }
//     }

//     // Measure rear left distance if rear sensor is not activated
//     if (!rearSensorActivated) {
//       int distanceLeftRear = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_LEFT);
//       if (distanceLeftRear > 60 && distanceLeftRear <= distanceLeftFront) {
//         // Rear sensor activated, start moving and stop the car when distances are in range
//         rearSensorActivated = true;
//         moveForward(100);
//       }
//     } else {
//       // Measure rear left distance
//       int distanceLeftRear = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_LEFT);
//       if (distanceLeftRear > 60 && distanceLeftRear <= distanceLeftFront) {
//         // Calculate distance moved only if this is the initial distance check
//         if (initialDistance != 0) {
//           unsigned long currentTime = millis();
//           Serial.println("CT " + String(currentTime));
//           Serial.println(String(currentTime - startTime));
//           distanceMoved += (currentTime - startTime) * MOVEMENT_SPEED / 1000;
//           initialDistance = 0; // Reset initial distance check
//         }
//         // Stop the car and timer
//         stop();
//         Serial.println("Car stopped at parking spot.");
//         Serial.print("Distance moved: ");
//         Serial.print(distanceMoved);
//         Serial.println(" cm");
//         // Move forward for 1 second
//         moveForward(100);
//         delay(1000);
//         stop();
//         return;
//       }
//     }
//   }
// }





// Define the data table with distance moved and corresponding steer angles
const int NUM_DATA_POINTS = 6;
const int distanceMovedTable[NUM_DATA_POINTS] = {10, 15, 20, 25, 30, 35}; // Rename the array variable
const int steerAngle[NUM_DATA_POINTS] = {45, 38, 30, 20, 15, 7};

void parking_process(int currentDistanceMoved, int steerAngle) { // Rename the parameter
  unsigned long startTime = millis();
  unsigned long currentTime = startTime;
  Serial.println(currentDistanceMoved);
  Serial.println(steerAngle);
  
  // Move forward for 2 seconds to calculate distance moved
  while (currentTime - startTime < 2000) {
    moveForward(100);
    currentTime = millis();
  }
  
  // Stop the car
  stop();
  Serial.println("car stopped");
  
  // Set the steer angle
  turnSteering(steerAngle);
  Serial.println(steerAngle);
  
  // Move in reverse until rear_back sensor reads very small value
  while (true) {
    // Measure rear back distance
    int distanceRearBack = measureDistance(TRIGGER_PIN_REAR_BACK, ECHO_PIN_REAR_BACK);
    Serial.println(distanceRearBack);
    
    // Check if the rear_back sensor reads very small value
    if (distanceRearBack < 5) {
      // Stop the car
      stop();
      Serial.println("car stopped 2");
      break;
    }
    
    // Move in reverse
    moveReverse();
    Serial.println("moving reverse 2");
  }
}

void parking_spot_search() {
  Serial.println("Searching for parking spot");
  unsigned long startTime = millis(); // Set the start time at the beginning
  bool rearSensorActivated = false;
  bool carStopped = false;
  int initialDistance = 0;
  int currentDistanceMoved = 0; // Rename the variable
  
  while (true) {
    // Measure left front distance
    int distanceLeftFront = measureDistance(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT_LEFT);

    if (distanceLeftFront > 60) {
      // Reduce speed to 100 and start timer only if this is the initial distance check
      if (initialDistance == 0) {
        moveForward(100);
        startTime = millis();
        initialDistance = distanceLeftFront;
      }
    }

    // Measure rear left distance if rear sensor is not activated
    if (!rearSensorActivated) {
      int distanceLeftRear = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_LEFT);
      if (distanceLeftRear > 60 && distanceLeftRear <= distanceLeftFront) {
        // Rear sensor activated, start moving and stop the car when distances are in range
        rearSensorActivated = true;
        moveForward(100);
      }
    } else {
      // Measure rear left distance
      int distanceLeftRear = measureDistance(TRIGGER_PIN_REAR, ECHO_PIN_REAR_LEFT);
      if (distanceLeftRear > 60 && distanceLeftRear <= distanceLeftFront) {
        // Calculate distance moved only if this is the initial distance check
        if (initialDistance != 0) {
          unsigned long currentTime = millis();
          currentDistanceMoved += (currentTime - startTime) * MOVEMENT_SPEED / 1000; // Update the variable
          initialDistance = 0; // Reset initial distance check
        }
        // Stop the car and timer
        stop();
        Serial.println("Car stopped at parking spot.");
        Serial.print("Distance moved: ");
        Serial.print(currentDistanceMoved); // Update the variable
        Serial.println(" cm");
        // Find the corresponding steer angle from the data table
        int selectedSteerAngle = 0;
        for (int i = 0; i < NUM_DATA_POINTS; ++i) {
          if (currentDistanceMoved <= distanceMovedTable[i]) { // Update the variable name
            selectedSteerAngle = steerAngle[i];
            break;
          }
        }
        // Perform parking process
        parking_process(currentDistanceMoved, selectedSteerAngle); // Update the variable
        return;
      }
    }
  }
}






  // Implement parking spot search logic here

int measureDistance(int triggerPin, int echoPin) {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(triggerPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * (0.034 / 2); // Calculate distance in cm
  return distance;
}

void moveForward(int speed) {
  digitalWrite(LED_PIN_1, HIGH); // Turn on LED 1
  digitalWrite(LED_PIN_2, LOW);  // Turn off LED 2
  // Set motor speed
  // Code to move the car forward with given speed
}

void moveReverse() {
  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
}