#include <Wire.h>
#include <Servo.h>
#include <U8g2lib.h>

// OLED display
U8G2_SSD1306_128X64_NONAME_F_HW_I2C oled(
  U8G2_R0, U8X8_PIN_NONE
);

// Ultrasonic sensor pins
#define TRIG_ALL 2

#define ECHO_IN 3
#define ECHO_OUT 4

const int ECHO_SLOT[6] = {5, 6, 7, 8, 12, 13};

// Servo pins
#define SERVO_IN_PIN 9
#define SERVO_OUT_PIN 10

// LED pin
#define LED_FULL 11

// Parking configuration
#define TOTAL_SLOTS 6
#define DIST_GATE 30
#define DIST_SLOT 15

#define GATE_OPEN_TIME 3000

// Servo objects
Servo servoIn;
Servo servoOut;

// Parking variables
int slotOccupied = 0;
int availableSlots = 0;

bool previousEntry = false;
bool previousExit = false;

unsigned long entryOpenTime = 0;
unsigned long exitOpenTime = 0;

bool gateInOpen = false;
bool gateOutOpen = false;

// All echo pins
const int echoPins[8] = {
  ECHO_IN, ECHO_OUT,
  5, 6, 7, 8, 12, 13
};

float distanceCM[8];

// Read all eight sensors after one common trigger
void readAllDistances()
{
  bool started[8] = {false};
  bool finished[8] = {false};

  unsigned long startTime[8] = {0};
  unsigned long duration[8] = {0};

  pinMode(TRIG_ALL, OUTPUT);

  digitalWrite(TRIG_ALL, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_ALL, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_ALL, LOW);

  unsigned long overallStart = micros();

  // Capture all echo pulses simultaneously
  while (micros() - overallStart < 25000)
  {
    for (int i = 0; i < 8; i++)
    {
      if (!finished[i])
      {
        if (digitalRead(echoPins[i]) == HIGH)
        {
          if (!started[i])
          {
            started[i] = true;
            startTime[i] = micros();
          }
        }
        else
        {
          if (started[i])
          {
            duration[i] = micros() - startTime[i];
            finished[i] = true;
          }
        }
      }
    }

    bool allFinished = true;

    for (int i = 0; i < 8; i++)
    {
      if (!finished[i])
        allFinished = false;
    }

    if (allFinished)
      break;
  }

  for (int i = 0; i < 8; i++)
  {
    if (finished[i])
      distanceCM[i] = duration[i] * 0.0343 / 2;
    else
      distanceCM[i] = 999;
  }
}

// Calculate parking availability
void checkParking()
{
  slotOccupied = 0;

  for (int i = 0; i < TOTAL_SLOTS; i++)
  {
    if (distanceCM[i + 2] < DIST_SLOT)
    {
      slotOccupied++;
    }
  }

  availableSlots = TOTAL_SLOTS - slotOccupied;
}

// Open entry gate
void openEntryGate()
{
  if (availableSlots > 0)
  {
    servoIn.write(90);
    gateInOpen = true;
    entryOpenTime = millis();
  }
}

// Open exit gate
void openExitGate()
{
  servoOut.write(90);
  gateOutOpen = true;
  exitOpenTime = millis();
}

// Display parking information
void displayParking()
{
  oled.clearBuffer();

  oled.setFont(u8g2_font_6x12_tf);

  oled.drawStr(0, 11, "SMART PARKING");

  oled.drawHLine(0, 15, 128);

  char buffer[25];

  sprintf(buffer, "Available: %d / 6", availableSlots);
  oled.drawStr(0, 29, buffer);

  sprintf(buffer, "Occupied : %d / 6", slotOccupied);
  oled.drawStr(0, 42, buffer);

  oled.drawStr(0, 56, "S1 S2 S3 S4 S5 S6");

  for (int i = 0; i < TOTAL_SLOTS; i++)
  {
    int x = i * 20 + 2;

    if (distanceCM[i + 2] < DIST_SLOT)
      oled.drawBox(x, 58, 10, 6);
    else
      oled.drawFrame(x, 58, 10, 6);
  }

  oled.sendBuffer();
}

// Setup
void setup()
{
  Serial.begin(9600);

  pinMode(TRIG_ALL, OUTPUT);

  for (int i = 0; i < 8; i++)
  {
    pinMode(echoPins[i], INPUT);
  }

  pinMode(LED_FULL, OUTPUT);

  servoIn.attach(SERVO_IN_PIN);
  servoOut.attach(SERVO_OUT_PIN);

  servoIn.write(0);
  servoOut.write(0);

  oled.begin();

  oled.clearBuffer();
  oled.setFont(u8g2_font_6x12_tf);
  oled.drawStr(10, 30, "SMART PARKING");
  oled.drawStr(10, 45, "Initializing...");
  oled.sendBuffer();

  delay(2000);
}

// Main program
void loop()
{
  readAllDistances();

  checkParking();

  // Detect vehicles at entrance and exit
  bool entryDetected = (distanceCM[0] < DIST_GATE);
  bool exitDetected = (distanceCM[1] < DIST_GATE);

  // Entry gate control
  if (entryDetected && !previousEntry)
  {
    openEntryGate();
  }

  // Exit gate control
  if (exitDetected && !previousExit)
  {
    openExitGate();
  }

  previousEntry = entryDetected;
  previousExit = exitDetected;

  // Close entry gate after timeout
  if (gateInOpen &&
      millis() - entryOpenTime >= GATE_OPEN_TIME)
  {
    servoIn.write(0);
    gateInOpen = false;
  }

  // Close exit gate after timeout
  if (gateOutOpen &&
      millis() - exitOpenTime >= GATE_OPEN_TIME)
  {
    servoOut.write(0);
    gateOutOpen = false;
  }

  // Full parking indication
  if (availableSlots == 0)
  {
    digitalWrite(LED_FULL, HIGH);
  }
  else
  {
    digitalWrite(LED_FULL, LOW);
  }

  displayParking();

  // Serial monitor
  Serial.print("Occupied: ");
  Serial.print(slotOccupied);

  Serial.print(" | Available: ");
  Serial.println(availableSlots);

  delay(100);
}