/*
estados:
  0-cor semaforo verde on
    cor semaforo amarelo off
    cor semaforo vermelho off 
    barreira aberta  
    ponte fechada
 1-cor semaforo verde off
   cor semaforo amarelo on
   cor semaforo vermeho off
   barreira aberta  
   ponte fechada
 2-cor semaforo verde off
   cor semaforo amarelo off
   cor semaforo vermeho on
   barreira aberta  
   ponte fechada
 3-cor semaforo verde off
   cor semaforo amarelo off
   cor semaforo vermeho on
   barreira fechada  
   ponte fechada
 4-cor semaforo verde off
   cor semaforo amareo off
   cor semaforo vermeho on
   barreira fechada  
   ponte aberta
 5-cor semaforo verde off
   cor semaforo amarelo off
   cor semaforo vermeho on (intermitente )
   barreira fechada    
   ponte parada 


*/
#include <RBD_Timer.h>

#include <Servo.h>

#include <RBD_Button.h>

RBD::Timer tempo_mudanca_luz_vermelho;
RBD::Timer tempo_mudanca_luz_amarelo;
RBD::Timer tempo_mudanca_luz_verde;
RBD::Timer tempo_velocidade_cancela_ponte;
RBD::Timer tempo_ponte_abrir;
RBD::Timer tempo_velocidade_inverter_func;
RBD::Button botao_inicio(2);



// constants won't change. Used here to set a pin number:
const int led_vermelhoPin = 6;  //pino do led vermelho
const int led_amareloPin = 7;  //pino do led amarelo
const int led_verdePin = 8;  //pino do led verde
const unsigned long mudanca_luz_vermelho = 2000; //tempo de mudança de luz vermelho
const unsigned long mudanca_luz_amarelo = 2000;//tempo de mudança de luz amarelo
const unsigned long mudanca_luz_verde = 2000;//tempo de mudança de luz verde
const unsigned long velocidade_servos = 2; //velocidade da cancela e ponte 
const unsigned long ponte_abrir = 1000;  //tempo para a ponte comecar a abrir depois da cancela fechar
const unsigned long ponte_fechar = 2000; //tempo para a ponte comecar a fechar depois de abrir
const unsigned long velocidade_inverter_func = 2000;
 
// Variables will change:
bool ledState_red = false;  //estado do led vermelho no inicio
bool ledState_green = false;//estado do led amarelo no inicio
bool ledState_yellow = false;//estado do led verde no inicio

Servo meuservo_ponte; //Declaracao do servo utilizado 
int angulo_ponte_fechado = 0; //Angulo do servo
int angulo_ponte_aberto = 90;  //angulo da ponte aberta
int servo_ponte = 9;  //pino do servo ponte
int incremento_ponte = 1;  //Incremento do angulo do servo 


Servo meuservo_cancela;//declaracao do servo utilizado 
int servo_cancela = 10; //pino do servo cancela
int angulo_cancela_aberto = 0; //Angulo do servo 
int angulo_cancela_fechado = 90;//angulo da cancela fechada
int angulo_atual = 0;//angulo inicial
int incremento_cancela = 1;  //Incremento do angulo do servo 


bool ponte_aberta = false;//variavel botão carregado
int estado = 0;//primeiro estado
bool ponte_a_abrir = true;

void setup() {
  Serial.begin(9600);

  // set the digital pin as output:
  pinMode(led_vermelhoPin, OUTPUT);
  pinMode(led_amareloPin, OUTPUT);
  pinMode(led_verdePin, OUTPUT);

  meuservo_ponte.attach(servo_ponte);
  meuservo_cancela.attach(servo_cancela);
  
 
  tempo_mudanca_luz_vermelho.setTimeout(mudanca_luz_vermelho);
  tempo_mudanca_luz_vermelho.restart();
  tempo_mudanca_luz_vermelho.stop();

  tempo_mudanca_luz_amarelo.setTimeout(mudanca_luz_amarelo);  
  tempo_mudanca_luz_amarelo.restart();
  tempo_mudanca_luz_amarelo.stop();

  tempo_mudanca_luz_verde.setTimeout(mudanca_luz_verde);
  tempo_mudanca_luz_verde.restart();
  tempo_mudanca_luz_verde.stop();

  tempo_ponte_abrir.setTimeout(ponte_abrir);
  tempo_ponte_abrir.stop();
  tempo_velocidade_cancela_ponte.setTimeout(velocidade_servos);
  //tempo_velocidade_cancela_ponte.restart();
  tempo_velocidade_inverter_func.setTimeout(velocidade_inverter_func);
  tempo_velocidade_inverter_func.stop();


   estado = 0;

}

void loop()
{/*
    Serial.print(estado);
    Serial.print("  ");
    Serial.print(angulo_atual);
    Serial.print("  ");
    Serial.println(ponte_a_abrir);*/
   if (botao_inicio.onPressed() &&
      estado == 0)
   {
     tempo_mudanca_luz_verde.restart();
   }
   else if (botao_inicio.onPressed() &&
      estado == 4)
   {
     tempo_mudanca_luz_verde.restart();
   }


  switch(estado)
  {
    case 0://estado 0
        digitalWrite(led_vermelhoPin, false);
        digitalWrite(led_amareloPin, false);
        digitalWrite(led_verdePin, true);
        meuservo_cancela.write(angulo_cancela_aberto);
        meuservo_ponte.write(angulo_ponte_fechado);
      break;
    case 1://estado 1
        digitalWrite(led_vermelhoPin, false);
        digitalWrite(led_amareloPin, true);
        digitalWrite(led_verdePin, false);
      break;
    case 2://estado 2
      digitalWrite(led_vermelhoPin, true);
      digitalWrite(led_amareloPin, false);
      digitalWrite(led_verdePin, false);
      break;
    case 3://estado 3
      digitalWrite(led_vermelhoPin, true);
      digitalWrite(led_amareloPin, false);
      digitalWrite(led_verdePin, false);
      
        if((angulo_atual <= angulo_cancela_fechado) &&
            tempo_velocidade_cancela_ponte.isExpired() &&  ponte_a_abrir)
        {
          meuservo_cancela.write(angulo_atual);
          tempo_velocidade_cancela_ponte.restart();
        }
        else if ((angulo_atual > angulo_cancela_fechado) &&
                  ponte_a_abrir)
        {

          tempo_ponte_abrir.restart();
          estado = 4;
          angulo_atual = angulo_ponte_fechado;
        }
        else  if((angulo_atual > angulo_cancela_aberto) &&
              tempo_velocidade_cancela_ponte.isExpired() &&              
              !ponte_a_abrir)
        {
          meuservo_cancela.write(angulo_atual);
          tempo_velocidade_cancela_ponte.restart();
        }
        else  if((angulo_atual <= angulo_cancela_aberto) &&
              tempo_velocidade_cancela_ponte.isExpired() &&              
              !ponte_a_abrir)
        {
         estado = 0;
         ponte_a_abrir = true;
         tempo_velocidade_inverter_func.stop();
        }
        
      break;
    case 4://estado 4      
        digitalWrite(led_vermelhoPin, true);
        digitalWrite(led_amareloPin, false);
        digitalWrite(led_verdePin, false);
      
        //Ponte abrir
        if((angulo_atual <= angulo_ponte_aberto) &&
          tempo_velocidade_cancela_ponte.isExpired() &&
          tempo_ponte_abrir.isExpired() &&
          ponte_a_abrir)
        {
          meuservo_ponte.write(angulo_atual);
          tempo_velocidade_cancela_ponte.restart(); 
        }
        //Ponte aberta
        else if ((angulo_atual > angulo_ponte_aberto) &&
                  tempo_velocidade_inverter_func.isStopped() &&
                  ponte_a_abrir)
        {
          tempo_velocidade_inverter_func.restart();
        }
        //Ponte fechar
        if((angulo_atual > angulo_ponte_fechado) &&
          tempo_velocidade_cancela_ponte.isExpired() &&
          tempo_ponte_abrir.isExpired() &&
          !ponte_a_abrir)
        {
          meuservo_ponte.write(angulo_atual);
          tempo_velocidade_cancela_ponte.restart(); 
        }
         else if ((angulo_atual <= angulo_ponte_fechado) &&
                tempo_velocidade_cancela_ponte.isExpired() &&              
                !ponte_a_abrir &&
                tempo_ponte_abrir.isExpired())
        {
          tempo_ponte_abrir.restart();
        }
      break;
   }

    //led verde do semaforo ligado
    




  if(tempo_mudanca_luz_verde.onExpired()) //o tempo de mudanca de verde acaba
  {
    // led amarelo do semaforo ligado e os outros desligados
    tempo_mudanca_luz_verde.stop(); //parar o tempo do verde
  // if (ponte_a_abrir)
    {
          estado = 1;
    tempo_mudanca_luz_amarelo.restart(); //ligar o tempo do led amarelo
    
    }
  }

  if(tempo_mudanca_luz_amarelo.onExpired())//tempo do led amarelo acaba
  {
    // led vermelho ligado e os outros desligados
    tempo_mudanca_luz_amarelo.stop(); //paragem do tempo amarelo
    tempo_mudanca_luz_vermelho.restart(); //inicio do tempo do led vermelho
    if (ponte_a_abrir)
    estado = 2;
  }

  if(tempo_mudanca_luz_vermelho.onExpired())  //tempo do led vermlho acaba
  {
    
    tempo_mudanca_luz_vermelho.stop();  //paragem do tempo do led vermelho
    if (ponte_a_abrir)
      estado = 3;
    else
      estado = 1;
  }
  
  if(tempo_ponte_abrir.onExpired())
  {
    if (ponte_a_abrir)
    {
    }
    else
    {
      estado = 3;
      angulo_atual = angulo_cancela_fechado;
    }
  }

  if(tempo_velocidade_cancela_ponte.onExpired()) 
  {
    if (ponte_a_abrir)
      angulo_atual = angulo_atual + incremento_ponte;
    else    
      angulo_atual = angulo_atual - incremento_ponte;
  }

  if(tempo_velocidade_inverter_func.onExpired())
  {
    ponte_a_abrir = false; 

  }

}