#include <ESP32Servo.h>

// Pin assignment
const int trigPins[4] = { 12, 13, 14, 15 };  // Trig pins for 4 ultrasonic sensors
const int echoPins[4] = { 2, 4, 5, 18 };     // Echo pins for 4 ultrasonic sensors
const int servoAtap[] = {16, 17, 25, 26};
const int entranceButtonPin = 32;            // Pin for entrance button
const int exitButtonPin = 33;                // Pin for exit button
Servo servoEntrance, servoExit, servoatap[4];              // Servo for entrance and exit

// Threshold distance for detecting a car
const int thresholdDistance = 20;            // in centimeters
const int openDuration = 3000;               // Servo open duration in milliseconds

bool entranceOpen = false;
bool exitOpen = false;
unsigned long entranceStartTime;
unsigned long exitStartTime;

void setup() {
  Serial.begin(115200);

  // Initialize servo motors
  servoEntrance.attach(19); // Attach servo entrance to pin 19
  servoExit.attach(21);     // Attach servo exit to pin 21
  servoEntrance.write(90);   // Initial closed position
  servoExit.write(90);       // Initial closed position
  

  // Initialize buttons
  pinMode(entranceButtonPin, INPUT_PULLUP);  // Button for entrance (use INPUT_PULLUP for active low)
  pinMode(exitButtonPin, INPUT_PULLUP);      // Button for exit (use INPUT_PULLUP for active low)

  // Initialize ultrasonic sensors
  for (int i = 0; i < 4; i++) {
    pinMode(trigPins[i], OUTPUT);
    pinMode(echoPins[i], INPUT);
    
  }
}

void loop() {
  int smallestAvailableSpot = findSmallestAvailableSpot();  // Get smallest empty spot

  // Check if entrance button is pressed and not already open
  if (digitalRead(entranceButtonPin) == LOW && !entranceOpen) {
    if (smallestAvailableSpot != -1) {
      Serial.print("Ruang parkir tersedia di nomor: ");
      Serial.println(smallestAvailableSpot + 1);  // Print the available spot
      servoEntrance.write(0);                    // Open entrance gate
      entranceStartTime = millis();               // Record time the gate opened
      entranceOpen = true;                        // Set flag to indicate entrance is open
    } else {
      Serial.println("Parkir penuh.");            // Print no available spots
    }
  }

  // Automatically close entrance gate after specified duration
  if (entranceOpen && millis() - entranceStartTime >= openDuration) {
    servoEntrance.write(90);                      // Close entrance gate
    entranceOpen = false;                        // Reset the flag
  }

  // Check if exit button is pressed and not already open
  if (digitalRead(exitButtonPin) == LOW && !exitOpen) {
    Serial.println("Mobil keluar.");             // Print exit notification
    servoExit.write(0);                         // Open exit gate
    exitStartTime = millis();                    // Record time the gate opened
    exitOpen = true;                             // Set flag to indicate exit is open
  }

  // Automatically close exit gate after specified duration
  if (exitOpen && millis() - exitStartTime >= openDuration) {
    servoExit.write(90);                          // Close exit gate
    exitOpen = false;                            // Reset the flag
  }

  delay(100);  // Small delay to stabilize button reading
  for (int i = 0; i < 4; i++){
    int distance = getDistance(trigPins[i], echoPins[i]);
    if (distance <= thresholdDistance){
      servoatap[i].attach(servoAtap[i]);
      servoatap[i].write(0);
      delay(100);
    }
    else{
    servoatap[i].attach(servoAtap[i]);
      servoatap[i].write(90);
      delay(100);
    }
  }
}

// Function to calculate distance using ultrasonic sensor
int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

// Function to find the smallest available parking spot
int findSmallestAvailableSpot() {
  int smallestSpot = -1;  // Initialize to -1 indicating no spot available
  
  // Iterate through all parking spots
  for (int i = 0; i < 4; i++) {
    int distance = getDistance(trigPins[i], echoPins[i]);
    
    // If the spot is available and has the smallest number
    if (distance > thresholdDistance) {
      smallestSpot = i;  // Update to the smallest available spot number
      break;  // Exit loop since we found the smallest spot
    }
  }
  
  return smallestSpot;  // Return the number of the smallest available spot, or -1 if none
}