/*
  Forum: https://forum.arduino.cc/t/how-to-active-millis-only-when-i-call-it/1195428
  Wokwi: https://wokwi.com/projects/382889761036905473
*/

constexpr byte AUTOPRESENTE = 4;
constexpr byte SENSORSALIDA  = 3;
constexpr byte STOP = 5;
constexpr byte MARCHA = 6;
constexpr byte BARRIERCLOSED = 7;
constexpr byte BARRRIEROPEN  = 8;
constexpr byte MOTOR_SUBIR = 11;  // Up
constexpr byte MOTOR_BAJAR = 12;  // Down
constexpr byte SEMAFORO_ROJO = 13;
constexpr byte SEMAFORO_VERDE = 2;
constexpr byte INDICADOR_LUMINOSO = 10;
constexpr byte INDICADOR_SONORO = 9;
constexpr unsigned long WAITTIME = 5000;
constexpr int WAITTONE = 440;
constexpr int OPENTONE = 340;


enum tollStates {BARRIERDOWN, BARRIERUP, MOVINGUP, MOVINGDOWN, UNDEFINED};
tollStates tollState = BARRIERDOWN;

struct buttonType {
  byte pin;
  byte state;
  byte prevState;
  byte openState;
  unsigned long lastChange = 0;
  void init(byte aPin, byte startState) {
    openState = startState;
    pin = aPin;
    state = startState;
    prevState = startState;
    if (startState) {
      pinMode(pin, INPUT_PULLUP);
    } else {
      pinMode(pin, INPUT);
    }
  }
  boolean pressed() {
    byte actState = digitalRead(pin);
    if (actState != prevState) {
      lastChange = millis();
      prevState = actState;
    }
    if (state != actState && millis() - lastChange > 20) {
      state = actState;
         return (state != openState);
      }
      return false;
    }
    boolean getState(){
      state = digitalRead(pin);
      delay(10);  // poor man's debouncing
      return state;
    }
};

buttonType marcha, stop, sensorSalida, autoPresente, barrierClosedSwitch, barrierOpenSwitch;

void setup() {
  Serial.begin(115200);
  autoPresente.init(AUTOPRESENTE, LOW);
  sensorSalida.init(SENSORSALIDA,LOW);
  stop.init(STOP,HIGH);
  marcha.init(MARCHA, HIGH);
  barrierClosedSwitch.init(BARRIERCLOSED, LOW);
  barrierOpenSwitch.init(BARRRIEROPEN,LOW);
  pinMode(MOTOR_SUBIR, OUTPUT);
  pinMode(MOTOR_BAJAR, OUTPUT);
  pinMode(SEMAFORO_ROJO, OUTPUT);
  pinMode(SEMAFORO_VERDE, OUTPUT);
  pinMode(INDICADOR_LUMINOSO, OUTPUT);
  pinMode(INDICADOR_SONORO, OUTPUT);
  getInitialBarrierState();
}


unsigned long activationTime;
boolean waitActive = false;
boolean blink      = false;

void loop() {
  checkButtons();
  stateMachine();
  handleFailure();
}

void stateMachine() {
  switch (tollState) {
    case BARRIERDOWN:
      if (!barrierIsDown()) {
        tollState = BARRIERUP;
        semaforosGreen();
        Serial.println("Change to UP");
        break;
      }
      if (autoPresent() && !waitActive) {
        activationTime = millis();
        waitActive = true;
        activarSalidas(WAITTONE);
        Serial.println("Auto present");
      }
      if (waitActive && millis() - activationTime >= WAITTIME) {
        waitActive = false;
        tollState = MOVINGUP;
        Serial.println("Start moving up");
      }
      break;
    case BARRIERUP:
      if (!barrierIsUp()) {
        tollState = BARRIERDOWN;
        semaforosRed();
        detenerSalidas();
        Serial.println("Change to Down");
        break;
      }
      if (autoHasLeft()) {
        Serial.println("Auto has left, start moving down");
        tollState = MOVINGDOWN;
      }
      break;
    case MOVINGUP:
      if (!barrierIsUp()) {
        subirBarrera();
      } else {
        Serial.println("Barrier is up");
        stopBarrera();
        semaforosGreen();
        tollState = BARRIERUP;
      }
      break;
    case MOVINGDOWN:
      if (!barrierIsDown()) {
        bajarBarrera();
      } else {
        Serial.println("Barrier is down");
        stopBarrera();
        detenerSalidas();
        tollState = BARRIERDOWN;
      }
      break;
    case UNDEFINED:
      blink = true;
      break;
  }
}

void checkButtons() {
  if (stop.pressed()) {
    Serial.println("STOP pressed!");
    tollState = MOVINGDOWN;
  }
  if (marcha.pressed()) {
    Serial.println("MARCHA pressed!");
    activarSalidas(WAITTONE);
    tollState = MOVINGUP;
  }
}



void getInitialBarrierState() {
  semaforosRed();
  if (barrierIsDown()) {
    tollState = BARRIERDOWN;
    Serial.println("Barrier is down");
  }
  if (barrierIsUp()) {
    semaforosGreen();
    tollState = BARRIERUP;
    Serial.println("Barrier is up");
  }
  if (barrierIsUp() == barrierIsDown()) {
    Serial.println("Undefined State!");
    tollState = UNDEFINED;
  }
}

boolean barrierIsDown() {
  return barrierClosedSwitch.getState();
}

boolean barrierIsUp() {
  return barrierOpenSwitch.getState();
}

boolean autoHasLeft() {
  return sensorSalida.pressed();
}

boolean autoPresent() {
  return autoPresente.pressed();
}


void activarSalidas(int aTone) {  // Signals on
  digitalWrite(INDICADOR_LUMINOSO, HIGH);
  digitalWrite(INDICADOR_SONORO, HIGH);
  // Just for Wokwi
  tone(INDICADOR_SONORO,aTone);
}

void detenerSalidas() {  // Signals off
  digitalWrite(INDICADOR_LUMINOSO, LOW);
  digitalWrite(INDICADOR_SONORO, LOW);
  // Just for Wokwi
  noTone(INDICADOR_SONORO);
}

void stopBarrera() { // Stop Barrier
  digitalWrite(MOTOR_BAJAR, LOW);
  digitalWrite(MOTOR_SUBIR, LOW);
}

void subirBarrera() { // Open Barrier
  semaforosRed();  // Always RED when moving!
  digitalWrite(MOTOR_BAJAR, LOW);
  digitalWrite(MOTOR_SUBIR, HIGH);
}

void bajarBarrera() {  // Close Barrier
  semaforosRed();  // Always RED when moving!
  digitalWrite(MOTOR_SUBIR, LOW);
  digitalWrite(MOTOR_BAJAR, HIGH);
}

void semaforosRed() {
  if (!blink) {
    digitalWrite(SEMAFORO_ROJO, HIGH);
    digitalWrite(SEMAFORO_VERDE, LOW);
  }
}

void semaforosGreen() {
  if (!blink) {
    activarSalidas(OPENTONE); // Everytime with Green the signals will be switched on as well
    digitalWrite(SEMAFORO_ROJO, LOW);
    digitalWrite(SEMAFORO_VERDE, HIGH);
  }
}


void handleFailure() {
  static unsigned long lastBlink = 0;
  static byte state = HIGH;
  if (!blink) {
    return;
  }
  if (millis() - lastBlink > 500) {
    lastBlink = millis();
    state = !state;
    digitalWrite(SEMAFORO_ROJO, state);
    digitalWrite(SEMAFORO_VERDE, LOW);
  }
}