#define lado_A_entrada 13
#define lado_A_saida 12
#define lado_B_entrada 14
#define lado_B_saida 25
#define LA_red 19
#define LA_green 18
#define LB_red 5
#define LB_green 17

#include <ESP32Servo.h>

const int servoPin1 = 27;
Servo servoMotor1;
const int servoPin2 = 26;
Servo servoMotor2;

bool LAE = 0;
bool LAS = 0;
bool LBE = 0;
bool LBS = 0;

int contador = 0;

int servos1 = 90;
int servos2 = 90;

int trava1 = 0;
int trava2 = 0;
int trava3 = 0;
int trava4 = 0;

void setup() {

  Serial.begin(115200);

  pinMode(lado_A_entrada, INPUT);
  pinMode(lado_A_saida, INPUT);
  pinMode(lado_B_entrada, INPUT);
  pinMode(lado_B_saida, INPUT);
  pinMode(LA_red, OUTPUT);
  pinMode(LA_green, OUTPUT);
  pinMode(LB_red, OUTPUT);
  pinMode(LB_green, OUTPUT);

  servoMotor1.attach(servoPin1);
  servoMotor2.attach(servoPin2);
  
  servoMotor1.write(servos1); 
  servoMotor2.write(servos2); 

}

void loop() {

  LAE = digitalRead(lado_A_entrada);
  LAS = digitalRead(lado_A_saida);
  LBE = digitalRead(lado_B_entrada);
  LBS = digitalRead(lado_B_saida);
  
  if (contador == 0){
    digitalWrite(LB_green, 1);
    digitalWrite(LA_green, 1);
    digitalWrite(LA_red, 0);
    digitalWrite(LB_red, 0);
  }
  if (LAE == 1 && trava1 == 0 && contador < 3 && servos2 == 90){
    servos1 = 0;
    delay(50);
    trava1 = 1;
  }
  if (LAE == 0 && trava1 == 1){
    delay(50);
    contador++;
    trava1 = 0;
    Serial.println(contador);
    digitalWrite(LB_green, 0);
    digitalWrite(LA_green, 1);
    digitalWrite(LB_red, 1);
    digitalWrite(LA_red, 0);
  }
  if (LAS == 1 && trava2 == 0 && contador > 0 && servos2 == 90){
    servos1 = 90;
    delay(50);
    trava2 = 1;
  }
  if (LAS == 0 && trava2 == 1){
    delay(50);
    contador--;
    trava2 = 0;
    Serial.println(contador);
  }
  
  if (LBE == 1 && trava3 == 0 && contador < 3 && servos1 == 90){
    servos2 = 0;
    delay(50);
    trava3 = 1;
  }
  if (LBE == 0 && trava3 == 1){
    delay(50);
    contador++;
    trava3 = 0;
    Serial.println(contador);
    digitalWrite(LB_green, 1);
    digitalWrite(LA_green, 0);
    digitalWrite(LB_red, 0);
    digitalWrite(LA_red, 1);
  }
  if (LBS == 1 && trava4 == 0 && contador > 0 && servos1 == 90){
    servos2 = 90;
    delay(50);
    trava4 = 1;
    //Serial.println(LBS);

  }
  if (LBS == 0 && trava4 == 1){
    delay(50);
    contador--;
    trava4 = 0;
    Serial.println(contador);
  }  
  servoMotor1.write(servos1); 
  servoMotor2.write(servos2); 

  delay(10);

}