#include<ESP32Servo.h>
#include<Adafruit_GFX.h>
#include<Adafruit_SSD1306.h>
#include<Wire.h>
#include<SPI.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3c
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH,SCREEN_HEIGHT,&Wire,OLED_RESET);

int pwm1 = 13;
int semaforo1 = 26;
int semaforo2 = 33;
int trigpin1 = 14;
int trigpin2 = 27;
int entrada1 = 34;
int entrada2 = 35;
Servo servo;

byte largo, ancho;
int16_t x, y;
uint16_t w, h;

void setup() {
  // put your setup code here, to run once:
  pinMode(semaforo1,OUTPUT);
  pinMode(semaforo2,OUTPUT);
  pinMode(entrada1,INPUT);
  pinMode(entrada2,INPUT);
  pinMode(trigpin1,OUTPUT);
  pinMode(trigpin2,OUTPUT);
  servo.attach(pwm1);
  Serial.begin(115200);

  if(!display.begin(SSD1306_SWITCHCAPVCC,SCREEN_ADDRESS)){
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.display();
  display.clearDisplay();
}

void loop() {
  // put your main code here, to run repeatedly:
  
  digitalWrite(trigpin1,0);
  delayMicroseconds(5);
  
  digitalWrite(trigpin1,1);
  delayMicroseconds(10);

  digitalWrite(trigpin1,0);

  long duracion1 = pulseIn(entrada1,1);
  long distancia_cm1 = duracion1 * 0.034 / 2;

  Serial.println(distancia_cm1);

  display.clearDisplay();

  if(distancia_cm1 < 10){
    duracion1 = distancia_cm1 * 11;

    digitalWrite(semaforo1,0);
    digitalWrite(semaforo2,0);

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.getTextBounds("Cerrando...", 0, 0, &x, &y, &w, &h);
    largo = (SCREEN_WIDTH - w) / 2;
    ancho = (SCREEN_HEIGHT - h) / 2;
    display.setCursor(largo,ancho);
    display.println("Cerrando...");
    display.display();

    for(int x=90;x>=0;x--){
      servo.write(x);
      delay(50);
      int lectura = servo.read();
      Serial.println(lectura);
    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.getTextBounds("Carretera cerrada", 0, 0, &x, &y, &w, &h);
    largo = (SCREEN_WIDTH - w) / 2;
    ancho = (SCREEN_HEIGHT - h) / 2;
    display.setCursor(largo,ancho);
    display.println("Carretera cerrada");
    display.display();

    delay(duracion1);
  }

  digitalWrite(trigpin2,0);
  delayMicroseconds(5);
  
  digitalWrite(trigpin2,1);
  delayMicroseconds(10);

  digitalWrite(trigpin2,0);

  long duracion2 = pulseIn(entrada2,1);
  long distancia_cm2 = duracion2 * 0.034 / 2;

  Serial.println(distancia_cm2);

  if(distancia_cm2 < 10){
    duracion2 = distancia_cm2 * 11;
    
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.getTextBounds("Abriendo...", 0, 0, &x, &y, &w, &h);
    largo = (SCREEN_WIDTH - w) / 2;
    ancho = (SCREEN_HEIGHT - h) / 2;
    display.setCursor(largo,ancho);
    display.println("Abriendo...");
    display.display();

    for(int x=0;x<=90;x++){
      servo.write(x);

      if(x == 10 || x == 30 || x == 50 || x == 70 || x == 90){
        digitalWrite(semaforo1,1);
        digitalWrite(semaforo2,0);
      }
      if(x == 0 || x == 20 || x == 40 || x == 60 || x == 80){
        digitalWrite(semaforo1,0);
        digitalWrite(semaforo2,1);
      }

      delay(50);
      int lectura = servo.read();
      Serial.println(lectura);
    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.getTextBounds("Carretera abierta", 0, 0, &x, &y, &w, &h);
    largo = (SCREEN_WIDTH - w) / 2;
    ancho = (SCREEN_HEIGHT - h) / 2;
    display.setCursor(largo,ancho);
    display.println("Carretera abierta");
    display.display();

    delay(duracion2);
  }         
  delay(50);                                                                                                                                                                                                                              
}