#include <LiquidCrystal_I2C.h>
#include <LED.h>
#include "Button.h"
#include "FiniteStateMachine.h"

#define vmc 7
#define amr 6
#define vdc 5
#define vmp 4
#define vdp 3

//Inicializa componentes
LiquidCrystal_I2C lcd(0x27, 16, 2);
Button botao(2);

//Declara o número de estados
const byte NUMBER_OF_STATES = 4;

//Inicializa os estados
State Fechado = State(fechado);
State Amarelo = State(amarelo);
State Aberto = State(aberto);
State AbertoPCD = State(abertoPCD);

//Inicializa a máquina de estado no primeiro estado
FSM semaforo = FSM(Fechado);

//Inicializa os timers para controle do contador
unsigned long millisInicial = 0;
unsigned long millisAtual = 0;

void setup() {
  lcd.init(); //Instancia a tela
  botao.begin(); //Instancia o botão
  Serial.begin(9600);

  //Botões 3 a 5 como OUTPUT
  for(int i = 3; i < 8; i++)
    pinMode(i, OUTPUT);
}

void loop() {
  //Transiciona o status quando o botão for pressionado no status adequado
  if (botao.pressed() && semaforo.isInState(Aberto)){
    semaforo.immediateTransitionTo(AbertoPCD);
  }

  //Atualiza a máquina de estados
  semaforo.update();
}

void fechado(){
  digitalWrite(vmc, LOW);
  digitalWrite(vdp, LOW);
  digitalWrite(vdc, HIGH);
  digitalWrite(vmp, HIGH);

  millisAtual = millis();

  if(millisAtual - millisInicial > 3000) {
    millisInicial = millis();
    semaforo.transitionTo(Amarelo);
  }
}

void amarelo(){
  digitalWrite(vdc, LOW);
  digitalWrite(amr, HIGH);
  digitalWrite(vmp, HIGH);

  millisAtual = millis();

  if(millisAtual - millisInicial > 3000) {
    millisInicial = millis();
    semaforo.transitionTo(Aberto);
  }
}

void aberto(){
  digitalWrite(amr, LOW);
  digitalWrite(vmp, LOW);
  digitalWrite(vmc, HIGH);
  digitalWrite(vdp, HIGH);

  millisAtual = millis();

  if(millisAtual - millisInicial > 3000) {
    millisInicial = millis();
    semaforo.transitionTo(Fechado);
  }
}

void abertoPCD(){
  Serial.println("PCD");
  digitalWrite(amr, LOW);
  digitalWrite(vmp, LOW);
  digitalWrite(vmc, HIGH);
  digitalWrite(vdp, HIGH);

  millisAtual = millis();

  if(millisAtual - millisInicial > 10000) {
    millisInicial = millis();
    semaforo.transitionTo(Fechado);
  }
}