#include <ESP32Servo.h>

#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
#define sinal_alarme 16

bool interrupcao_ocorreu = false;

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

bool LAE = 0; // sensor de entrada (lado A)
bool LAS = 0; // sensor de saida (lado A)
bool LBE = 0; // sensor de entrada (lado B)
bool LBS = 0; // sensor de saida (lado B)


int contador = 0;
int limit = 10;
int tempo = 1;

int servos1 = 0;
int servos2 = 0;

int delaysensorsaida_A = 0; // travamento de delay no sensor de entrada do outro lado
int delaysensorsaida_B = 0;

int trava1 = 0; // trava do sensor de entrada do lado A
int trava2 = 0; // trava do sensor de saida do lado A (ele abre a cancela do outro lado para a passagem da empilhadeira)
int trava3 = 0; // trava do sensor de entrada do lado B
int trava4 = 0; // trava do sensor de saida do lado B (ele abre a cancela do outro lado para a passagem da empilhadeira)
int trava5 = 0; // intertravamento para a passagem das empilhadeiras lado A
int trava6 = 0; // intertravamento para a passagem das empilhadeiras lado B

void setup() {

  Serial.begin(115200);

  pinMode(lado_A_entrada, INPUT_PULLDOWN);
  pinMode(lado_A_saida, INPUT_PULLDOWN);
  pinMode(lado_B_entrada, INPUT_PULLDOWN);
  pinMode(lado_B_saida, INPUT_PULLDOWN);
  pinMode(LA_red, OUTPUT);
  pinMode(LA_green, OUTPUT);
  pinMode(LB_red, OUTPUT);
  pinMode(LB_green, OUTPUT);
  pinMode(sinal_alarme, 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(digitalRead(lado_A_entrada) == 1 && digitalRead(lado_B_entrada) && contador == 0 || tempo > 0){
    digitalWrite(LA_red, 1);
    digitalWrite(LB_red, 1);
    digitalWrite(LB_green, 0);
    digitalWrite(LA_green, 0);
    digitalWrite(sinal_alarme, 1);
    

    servos1 = 0;
    servos2 = 0;
    tempo++;
    if(tempo > 100)tempo = 0;

  }else{
    tempo == 1;
    digitalWrite(sinal_alarme, 0);
    if (LAE == 1 && trava1 == 0 && contador < limit && servos2 == 0 && trava6 == 0){
      
      servos1 = 90;
      trava1 = 1;

      // receptor da entrada do lado A, abre a cancela e bloqueia o receptor do lado B 
    }
    if (LAE == 0 && trava1 == 1){
      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 == 0){
      servos2 = 90;
      delaysensorsaida_A = 1;
      trava2 = 1;
    }
    if (LBE == 1 && delaysensorsaida_A == 1 && trava5 == 0){
      trava5 = 1;
    }
    if (LBE == 0 && delaysensorsaida_A == 1 && trava5 == 1){
      contador--;
      trava5 = 0;
      Serial.println(contador);
    } if (contador == 0 && trava2 == 1){
      servos1 = 0;
      servos2 = 0;
      delaysensorsaida_A = 0;
      trava5 = 0;
      trava2 = 0;
    }  

    /////////////////////////////////////////////////////////////////////
    
    if (LBE == 1 && trava3 == 0 && contador < limit && servos1 == 0 && trava5 == 0){
    
      servos2 = 90;
      trava3 = 1;
    }
    if (LBE == 0 && trava3 == 1){
      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 == 0){
      servos1 = 90;
      delaysensorsaida_B = 1;
      trava4 = 1;
    }
    if (LAE == 1 && delaysensorsaida_B == 1 && trava6 == 0){
      trava6 = 1;
    }  
    if (LAE == 0 && delaysensorsaida_B == 1 && trava6 == 1){
      contador--;
      trava6 = 0;
      Serial.println(contador);
    }
    if (contador == 0 && trava4 == 1){
      servos1 = 0;
      servos2 = 0;
      delaysensorsaida_B = 0;
      trava6 = 0;
      trava4 = 0;
    }
  }
  ////////////////////////////////////////////////////////////////////

  servoMotor1.write(servos1); 
  servoMotor2.write(servos2); 

  delay(10);
}