#include <Servo.h>

#define pino_Tg 12
#define pino_Tg_dois 7
#define pino_Echo 11
#define pino_Echo_dois 6
#define pino_led1 10
#define pino_led2 8
#define pino_bz 2
#define STEPPER_PIN_1 0
#define STEPPER_PIN_2 1
#define STEPPER_PIN_3 4
#define STEPPER_PIN_4 5
int step_number = 0;

Servo servo_ponte;
Servo servo_cancela;

// =====================================================
// TEMPOS
// =====================================================

unsigned long intervaloled = 0;
unsigned long temp_passo_motor = 0;
unsigned long temp_leitura = 0;
unsigned long tempo_buzzer = 0;
unsigned long tempo_cancela = 0;
unsigned long tempo_delay = 0;


// =====================================================
// ESTADOS
// =====================================================

bool led = 0;

bool ativar_motor_subir = 0;
bool ativar_motor_descer = 0;

bool ativar_cancela_descendo = 0;
bool ativar_cancela_subindo = 0;

bool ponte_aberta = 0;
bool cancela_fechada = 0;
bool cancela_em_movimento = 0;

bool ativar_delay = 0;

bool entrada = 0;
bool trava_entrada = 0;
bool saida = 0;

// =====================================================
// POSIÇÕES
// =====================================================

int posPonte = 0;
const int tempo_mov_cancela = 150;

// =====================================================

float distancia_um = 100;
float distancia_dois = 100;

bool estado_buzzer = 0;

// =====================================================

void setup() {

  Serial.begin(9600);

  servo_ponte.attach(3);
  servo_cancela.attach(9);

  servo_ponte.write(0);
  servo_cancela.write(90);

  pinMode(pino_Tg, OUTPUT);
  pinMode(pino_Tg_dois, OUTPUT);
  pinMode(pino_Echo, INPUT);
  pinMode(pino_Echo_dois, INPUT);

  pinMode(pino_led1, OUTPUT);
  pinMode(pino_led2, OUTPUT);

  pinMode(pino_bz, OUTPUT);

  pinMode(STEPPER_PIN_1, OUTPUT);
  pinMode(STEPPER_PIN_2, OUTPUT);
  pinMode(STEPPER_PIN_3, OUTPUT);
  pinMode(STEPPER_PIN_4, OUTPUT);
}

// =====================================================

void loop() {

  unsigned long agora = millis();

  // =====================================================
  // LED piscando
  // =====================================================

  if ((ativar_motor_subir ||
       ativar_motor_descer ||
       ativar_cancela_descendo ||
       ativar_cancela_subindo ||
       ponte_aberta) &&
      agora - intervaloled >= 500) {

    led = !led;

    digitalWrite(pino_led1, led);
    digitalWrite(pino_led2, !led);

    intervaloled = agora;
  }

  else if (
      !ativar_motor_subir &&
      !ativar_motor_descer &&
      !ponte_aberta &&
      !ativar_cancela_descendo &&
      !ativar_cancela_subindo) {

    digitalWrite(pino_led1, LOW);
    digitalWrite(pino_led2, LOW);
  }

  // =====================================================
  // SENSOR
  // =====================================================

  if (agora - temp_leitura >= 1000) {

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

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

    float duration = pulseIn(pino_Echo, HIGH);
    
    distancia_um = duration / 58.0;

    Serial.print("Distancia sensor 1: ");
    Serial.println(distancia_um);

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

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

    float duration_dois = pulseIn(pino_Echo_dois, HIGH);
    
    distancia_dois = duration_dois/58.0;

    Serial.print("Distancia sensor 2: ");
    Serial.println(distancia_dois);

    temp_leitura = agora;
  }

  // =====================================================
  // GATILHOS DE ACIONAMENTO
  // =====================================================

  if(!ponte_aberta && 
      !ativar_cancela_descendo &&
      !cancela_fechada &&
      !entrada &&
      (distancia_um <= 5 || distancia_dois <= 5)){
        entrada = 1;
        trava_entrada = 1; 
      }
  
  if(trava_entrada && distancia_um > 5 && distancia_dois > 5){
    trava_entrada = 0;
  }

  if(!trava_entrada &&
      ponte_aberta &&
      entrada &&
    (distancia_um <= 5 || distancia_dois <= 5)){
    saida = 1;
  }


  // =====================================================
  // INICIAR PROCESSO
  // =====================================================

  if (entrada &&
      !ponte_aberta &&
      !ativar_cancela_descendo &&
      !cancela_fechada) {
    ativar_cancela_descendo = 1;
  }

  // =====================================================
  // CANCELA DESCENDO
  // =====================================================
  
  if (ativar_cancela_descendo && !cancela_em_movimento) {
  
    servo_cancela.write(0); // gira para fechar
  
    tempo_cancela = agora;
  
    cancela_em_movimento = 1;
  }
  
  // tempo necessário para fechar
  if (ativar_cancela_descendo &&
      cancela_em_movimento &&
      agora - tempo_cancela >= tempo_mov_cancela) {
  
    servo_cancela.write(90); // para servo
  
    ativar_cancela_descendo = 0;
  
    cancela_fechada = 1;
  
    cancela_em_movimento = 0;
  
    ativar_motor_subir = 1;

    ativar_delay = 1;

    tempo_delay = agora;
  }

  // =====================================================
  // SUBIR PONTE
  // =====================================================

  if (!ativar_delay && ativar_motor_subir &&
      agora - temp_passo_motor >= 20) {

    temp_passo_motor = agora;

    if (posPonte < 90) {

      posPonte++;

      servo_ponte.write(posPonte);
    }
    else {

      ativar_motor_subir = 0;

      ponte_aberta = 1;
    }
  }

  // =====================================================
  // FECHAR PONTE
  // =====================================================

  if(entrada &&
      saida &&
      distancia_um > 5 && 
      distancia_dois > 5 && 
      ponte_aberta &&
      !ativar_motor_descer){
        ativar_motor_descer = 1;
      }

  // =====================================================
  // DESCER PONTE
  // =====================================================

  if (ativar_motor_descer &&
      agora - temp_passo_motor >= 20) {

    temp_passo_motor = agora;

    if (posPonte > 0) {

      posPonte--;

      servo_ponte.write(posPonte);
    }
    else {

      ativar_motor_descer = 0;

      ponte_aberta = 0;

      ativar_cancela_subindo = 1;

      tempo_delay = agora;

      ativar_delay = 1;
    }
  }

  // =====================================================
  // CANCELA SUBINDO
  // =====================================================
  
  if (!ativar_delay & ativar_cancela_subindo && !cancela_em_movimento) {
  
    servo_cancela.write(180); // gira para abrir
  
    tempo_cancela = agora;
  
    cancela_em_movimento = 1;
  }
  
  // tempo necessário para abrir
  if (ativar_cancela_subindo &&
      cancela_em_movimento &&
      agora - tempo_cancela >= tempo_mov_cancela) {
  
    servo_cancela.write(90); // para servo
  
    ativar_cancela_subindo = 0;
  
    cancela_fechada = 0;
  
    cancela_em_movimento = 0;

    entrada = 0;
    saida = 0;
  }

  // =====================================================
  // BUZZER
  // =====================================================

  if ((ativar_motor_subir ||
       ativar_motor_descer ||
       ativar_cancela_descendo ||
       ativar_cancela_subindo) &&
      agora - tempo_buzzer >= 200) {

    tempo_buzzer = agora;

    estado_buzzer = !estado_buzzer;

    if (estado_buzzer)
      tone(pino_bz, 1000);
    else
      noTone(pino_bz);
  }

  if (!ativar_motor_subir &&
      !ativar_motor_descer &&
      !ativar_cancela_descendo &&
      !ativar_cancela_subindo) {

    noTone(pino_bz);

    estado_buzzer = 0;
  }

  // =====================================================
  // DELAY
  // =====================================================

  if(ativar_delay){
    if ( agora > tempo_delay +500){
      tempo_delay = agora;
      ativar_delay = 0;
    }
  }

  switch(step_number){
      case 0:
        digitalWrite(STEPPER_PIN_1, HIGH);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 1:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, HIGH);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 2:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, HIGH);
        digitalWrite(STEPPER_PIN_4, LOW);
        break;
      case 3:
        digitalWrite(STEPPER_PIN_1, LOW);
        digitalWrite(STEPPER_PIN_2, LOW);
        digitalWrite(STEPPER_PIN_3, LOW);
        digitalWrite(STEPPER_PIN_4, HIGH);
        break;
  }

  step_number++;
    if(step_number > 3){
        step_number = 0;
          }
}