#include <Wire.h>
#include <Servo.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"

#define I2C_ADDRESS 0x3C

SSD1306AsciiWire oled;

const int TRIG1 = 5, ECHO1 = 8;    
const int TRIG2 = 10, ECHO2 = 12; 
const int BUZZER = 7;
const int pinServoE = 3;
const int pinServoS = 9;

Servo servoEntrada;
Servo servoSalida;

int slots = 9;
const int distanciaUmbral = 15;

void setup() {
  Wire.begin();
  Wire.setClock(400000L);

  oled.begin(&Adafruit128x64, I2C_ADDRESS);
  oled.setFont(Adafruit5x7);
  oled.clear();

  pinMode(TRIG1, OUTPUT); pinMode(ECHO1, INPUT);
  pinMode(TRIG2, OUTPUT); pinMode(ECHO2, INPUT);
  pinMode(BUZZER, OUTPUT);


  servoEntrada.attach(pinServoE);
  servoSalida.attach(pinServoS);

  servoEntrada.write(90); 
  servoSalida.write(90);

  oled.println("   Bienvenido");
   delay(2000);
  oled.clear();
  oled.println(" Estacionamientos");
  delay(2000);
  oled.clear();
}

long leerDistancia(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  return pulseIn(echo, HIGH) * 0.034 / 2;
}

void loop() {
  long d1 = leerDistancia(TRIG1, ECHO1);
  long d2 = leerDistancia(TRIG2, ECHO2);

  if (d1 > 0 && d1 < distanciaUmbral) {
    if (slots > 0) {
      oled.setCursor(0,0);
      oled.println("ENTRANDO...     ");
      servoEntrada.write(0);
      slots--;
      delay(2000);
      servoEntrada.write(90);
    } else {
      oled.setCursor(0,0);
      oled.println("NO HAY ESPACIO  ");
      digitalWrite(BUZZER, HIGH);
      delay(500);
      digitalWrite(BUZZER, LOW);
    }
  }

  if (d2 > 0 && d2 < distanciaUmbral) {
    oled.setCursor(0,0);
    oled.println("SALIENDO...     ");
    servoSalida.write(0);
    if (slots < 9) slots++;
    delay(2000);
    servoSalida.write(90);
  }



  oled.setCursor(0, 2);
  if (slots <= 0) {
    oled.println("ESTADO: LLENO   ");
  } else {
    oled.print("LIBRES: ");
    oled.print(slots);
    oled.println(" de 9   ");
  }
  
  delay(100); 
}