// HEADSTATE MACHINE /// !!!(@@@@
// --- ATTRIBUTES --- //

int headState; // What state the machine is currently on
boolean readyToChange; // Check if state is done and can go back to idle

const int TRAFFIC_IDLE = 0;
const int TRAFFIC_LEFT = 1;
const int TRAFFIC_RIGHT = 2;
const int TRAFFIC_PASSAGE = 3;
const int NIGHT_TIME = 4;

const byte STOP = B10110111; // STOP SIGN
const byte WALK = B00011100; // WALK SIGN
const byte WARNING = B01101111; // WARNING SIGN

const int RED = 0;
const int ORANGE = 1;
const int GREEN = 2;

const int LEFT = 0; // LEFT TRAFFIC LIGHT
const int RIGHT = 1; // RIGHT TRAFFIC LIGHT

const int FAST = 250; // FAST INTERVAL FOR BUZZER
const int SLOW = 1000; // SLOW INTERVAL FOR BUZZER

const int CAR_LEFT = 0;
const int CAR_RIGHT = 1;
const int PASSAGE_MIDDLE = 2;

// --- SETUP --- //

void headStateMachineSetup() {
  // put your setup code here, to run once:
  readyToChange = false;

  headState = TRAFFIC_IDLE;
  TRAFFIC_IDLE_entry();
}

// --- FUNCTIONS --- //

void headStateMachineLoop() {
  // put your main code here, to run repeatedly:
  switch (headState) {
    case TRAFFIC_IDLE :
      buttonLoop();
      TRAFFIC_IDLE_do();
      Serial.println("im still here");
      if (buttonWasPressed(CAR_LEFT)) {
        TRAFFIC_IDLE_exit();
        headState = TRAFFIC_LEFT;
        TRAFFIC_LEFT_entry();
      } else if (buttonWasPressed(CAR_RIGHT)) {
        TRAFFIC_IDLE_exit();
        headState = TRAFFIC_RIGHT;
        TRAFFIC_RIGHT_entry();
      } else if (buttonWasPressed(PASSAGE_MIDDLE)) {
        TRAFFIC_IDLE_exit();
        headState = TRAFFIC_PASSAGE;
        TRAFFIC_PASSAGE_entry();
      }
      break;
    case TRAFFIC_LEFT :
      TRAFFIC_LEFT_do();
      if (readyToChange) {
        TRAFFIC_LEFT_exit();
        headState = TRAFFIC_IDLE;
        TRAFFIC_IDLE_entry();
      }
      break;
    case TRAFFIC_RIGHT :
      TRAFFIC_RIGHT_do();
      if (readyToChange) {
        TRAFFIC_RIGHT_exit();
        headState = TRAFFIC_IDLE;
        TRAFFIC_IDLE_entry();
      }
      break;
    case TRAFFIC_PASSAGE :
      TRAFFIC_PASSAGE_do();
      if (readyToChange) {
        TRAFFIC_RIGHT_exit();
        headState = TRAFFIC_IDLE;
        TRAFFIC_IDLE_entry();
      }
      break;
      //    case NIGHT_TIME :
      //      NIGHT_TIME_do();
      //      if (isNotNight){
      //        NIGHT_TIME_exit();
      //        headStateMachineSetup();
      //      }
      //      break;
  }
}



// --- SETTERS --- //

// --- GETTERS --- //

int getFastIntervalBuzzer() {
  return FAST;
}
int getSlowIntervalBuzzer() {
  return SLOW;
}
byte getStopSign() {
  return STOP;
}
byte getWalkSign() {
  return WALK;
}
byte getWarningSign() {
  return WARNING;
}

// --- ENTRY | DO | EXIT --- //

// || -- TRAFFIC IDLE -- || //
void TRAFFIC_IDLE_entry() {
  trafficSetLights(RED);
  myfnUpdateDisplay(STOP);
}
void TRAFFIC_IDLE_do() {
  buzzerTicking(SLOW); // Buzzer ticks slowly
}
void TRAFFIC_IDLE_exit() {
  /*NOTHING*/
}

// || -- TRAFFIC LEFT -- || //
void TRAFFIC_LEFT_entry() {
  Serial.println("LEFT");
  subStateMachineTrafficSetup(LEFT);
}
void TRAFFIC_LEFT_do() {
  subStateMachineTrafficLoop(LEFT);
}
void TRAFFIC_LEFT_exit() {
  /*NOTHING*/
}

// || -- TRAFFIC RIGHT -- || //
void TRAFFIC_RIGHT_entry() {
  Serial.println("RIGHT");
  subStateMachineTrafficSetup(RIGHT);
}
void TRAFFIC_RIGHT_do() {
  myfnUpdateDisplay(STOP);
  subStateMachineTrafficLoop(RIGHT);
}
void TRAFFIC_RIGHT_exit() {
  /*NOTHING*/
}

// || -- TRAFFIC PASSAGE -- || //
void TRAFFIC_PASSAGE_entry() {
  Serial.println("PASSAGE");
  subStateMachinePassageSetup();
}
void TRAFFIC_PASSAGE_do() {
  subStateMachinePassageLoop();
}
void TRAFFIC_PASSAGE_exit() {
  headStateMachineSetup();
}
// || -- NIGHT TIME -- || //
void NIGHT_TIME_entry() {
  setServoUp(); // Opens the servo
  myfnUpdateDisplay(getStopSign()); // Displays a stop sign on the 7-segment display
  buzzerTicking(SLOW); // Buzzer ticks slowly
}
void NIGHT_TIME_do() {
  trafficLightBlink(); // Both traffic lights start blinking yellow
  setBuzzerOff(); // Buzzer is turned off
}
void NIGHT_TIME_exit() {
  setServoDown(); // Closes the servo
  myfnUpdateDisplay(getStopSign()); // Displays a stop sign on the 7-segment display
  buzzerTicking(SLOW); // Buzzer ticks slowly
  for (int i = 0; i < getMaxLights; i++) {
    trafficSetLights(ORANGE); // Yellow lamps stop blinking and keep staying on
  }
}

// (@#*### BUTTON STATE MACHINE // @*(#&#


// --- ATTRIBUTES --- //

const int NR_OF_BUTTONS = 3;

const int BUTTON_RELEASED = 1; // STATE 1
const int BUTTON_CHECK_PRESSED = 2; // STATE 2
const int BUTTON_PRESSED = 3; // STATE 3
const int BUTTON_CHECK_RELEASED = 4; // STATE 4

int buttonState[NR_OF_BUTTONS];

const int BUTTON_INTERVAL = 10; // INTERVAL BUTTON
unsigned long buttonPreviousMillis[NR_OF_BUTTONS]; // PREVIOUS MILLIS BUTTON

boolean buttonPressed[NR_OF_BUTTONS];

// --- SETUP --- //

void buttonSetup() {
  buttonHardwareSetup(NR_OF_BUTTONS);
  for (int buttonNumber = 0; buttonNumber < NR_OF_BUTTONS; buttonNumber++) {
    buttonPreviousMillis[buttonNumber] = 0 - BUTTON_INTERVAL; // Set timer
    buttonPressed[buttonNumber] = false;  // Set button switch
    buttonState[buttonNumber] = BUTTON_RELEASED; // Start button state
    BUTTON_RELEASED_entry(buttonNumber); // Start button entry
  }
}

// --- FUNCTIONS --- //
void buttonLoop() {
  for (int buttonNumber = 0; buttonNumber < NR_OF_BUTTONS; buttonNumber++) {
    buttonSwitch(buttonNumber);
  }
}

void buttonSwitch(int buttonNumber) {
  switch (buttonState[buttonNumber]) {
    case BUTTON_RELEASED:
      BUTTON_RELEASED_do(buttonNumber);
      if (buttonHardwareDown(buttonNumber)) {
        BUTTON_RELEASED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_CHECK_PRESSED;
        BUTTON_CHECK_PRESSED_entry(buttonNumber);
      }
      break;
    case BUTTON_CHECK_PRESSED:
      BUTTON_CHECK_PRESSED_do(buttonNumber);
      if (buttonHardwareUp(buttonNumber)) {
        BUTTON_CHECK_PRESSED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_RELEASED;
        BUTTON_RELEASED_entry(buttonNumber);
      }
      else if ((buttonHardwareDown(buttonNumber)) && (millis() - BUTTON_INTERVAL >= buttonPreviousMillis[buttonNumber])) { // Button down && interval over
        BUTTON_CHECK_PRESSED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_PRESSED;
        BUTTON_PRESSED_entry(buttonNumber);
      }
      break;
    case BUTTON_PRESSED:
      BUTTON_PRESSED_do(buttonNumber);
      if (buttonHardwareUp(buttonNumber)) {
        BUTTON_CHECK_PRESSED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_CHECK_RELEASED;
        BUTTON_CHECK_RELEASED_entry(buttonNumber);
      }
      break;
    case BUTTON_CHECK_RELEASED:
      BUTTON_CHECK_RELEASED_do(buttonNumber);
      if (buttonHardwareDown(buttonNumber)) {
        BUTTON_CHECK_RELEASED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_PRESSED;
        BUTTON_PRESSED_entry(buttonNumber);
      }
      else if ((buttonHardwareUp(buttonNumber)) && (millis() - BUTTON_INTERVAL >= buttonPreviousMillis[buttonNumber])) { // Button up && interval over
        BUTTON_CHECK_RELEASED_exit(buttonNumber);
        buttonState[buttonNumber] = BUTTON_RELEASED;
        BUTTON_RELEASED_entry(buttonNumber);
      }
      break;
  }
}

// --- ENTRY | DO | EXIT --- //

// --- BUTTON RELEASED --- //
void BUTTON_RELEASED_entry(int buttonNumber) {
  // <nothing>
}

void BUTTON_RELEASED_do(int buttonNumber) {
  // <nothing>
}

void BUTTON_RELEASED_exit(int buttonNumber) {
  // <nothing>
}

// --- BUTTON CHECK PRESSED  --- //
void BUTTON_CHECK_PRESSED_entry(int buttonNumber) {
  buttonPreviousMillis[buttonNumber] = millis(); // Reset button timer
}

void BUTTON_CHECK_PRESSED_do(int buttonNumber) {
  // <nothing>
}

void BUTTON_CHECK_PRESSED_exit(int buttonNumber) {
  // <nothing>
}

// --- BUTTON PRESSED --- //
void BUTTON_PRESSED_entry(int buttonNumber) {
  // <nothing>
}

void BUTTON_PRESSED_do(int buttonNumber) {
  // <nothing>
}

void BUTTON_PRESSED_exit(int buttonNumber) {
  // <nothing>
}

// --- BUTTON_STATE_CHECK_RELEASED --- //
void BUTTON_CHECK_RELEASED_entry(int buttonNumber) {
  buttonPreviousMillis[buttonNumber] = millis(); // Reset button timer
}

void BUTTON_CHECK_RELEASED_do(int buttonNumber) {
  // <nothing>
}

void BUTTON_CHECK_RELEASED_exit(int buttonNumber) {
  buttonPressed[buttonNumber] = true; // Button state is set on true
}

// --- Event -----------
boolean buttonWasPressed(int buttonNumber) { // Returns true if button is pressed && resets button
  if (buttonPressed[buttonNumber]) {
    buttonPressed[buttonNumber] = false;
    return true;
  }
  else return false;
}

// RIGHT ? LEFT SUBSTATEMACHINE @@@


// --- ATTRIBUTES --- //

int subStateMachineTraffic;
boolean subStateMachineTrafficBusy = false; // Is the substateBusy?
boolean canBeChangedSubTraffic;

const int TRAFFIC_OPEN = 0;
const int TRAFFIC_CLOSING = 1;
const int TRAFFIC_CLOSED = 2;

// --- SETUP --- //

void subStateMachineTrafficSetup(int trafficLight) {
  subStateMachineTrafficBusy = true;
  canBeChangedSubTraffic = false;

  subStateMachineTraffic = TRAFFIC_OPEN;
  TRAFFIC_OPEN_entry(trafficLight);
}

// --- FUNCTIONS --- //

void subStateMachineTrafficLoop(int trafficLight) {
  switch (subStateMachineTraffic)  {
    case TRAFFIC_OPEN :
      TRAFFIC_OPEN_do();
      if (canBeChangedSubTraffic){
        TRAFFIC_OPEN_exit();
        subStateMachineTraffic = TRAFFIC_CLOSING;
        TRAFFIC_CLOSING_entry(trafficLight);
      }
      break;
    case TRAFFIC_CLOSING :
      TRAFFIC_CLOSING_do();
      if (canBeChangedSubTraffic){
        TRAFFIC_CLOSING_exit();
        subStateMachineTraffic = TRAFFIC_CLOSED;
        TRAFFIC_CLOSED_entry(trafficLight);
      }
      break;
    case TRAFFIC_CLOSED :
      TRAFFIC_CLOSED_do();
      if (canBeChangedSubTraffic){
        TRAFFIC_CLOSED_exit();
      }
      break;
  }
}

// --- SETTERS --- //
void setCanBeChanged() {
  canBeChangedSubTraffic = true;
}
void setCantBeChanged() {
  canBeChangedSubTraffic = false;
}


// --- ENTRY | DO | EXIT --- //

// || -- TRAFFIC OPEN -- || //
void TRAFFIC_OPEN_entry( int trafficLight){
  trafficSetLights(GREEN);
}
void TRAFFIC_OPEN_do()
{
  buzzerTicking(SLOW); // Buzzer ticks slowly
  myfnUpdateDisplay(getStopSign()); // Displays a stop sign on the 7-segment display
  setCanBeChanged();
}
void TRAFFIC_OPEN_exit()
{
  setCantBeChanged();
}

// || -- TRAFFIC CLOSING -- || //
void TRAFFIC_CLOSING_entry(int trafficLight)
{
  trafficSetLights(ORANGE);
}
void TRAFFIC_CLOSING_do()
{
  buzzerTicking(SLOW); // Buzzer ticks slowly
  setCanBeChanged();
}
void TRAFFIC_CLOSING_exit()
{
  setCantBeChanged();
}

// || -- TRAFFIC CLOSED -- || //
void TRAFFIC_CLOSED_entry(int trafficLight)
{
  trafficSetLights(RED);
}
void TRAFFIC_CLOSED_do()
{
  buzzerTicking(SLOW); // Buzzer ticks slowly
  setCanBeChanged();
}
void TRAFFIC_CLOSED_exit()
{
  if (getDeadTimer())
  {
    subStateMachineTrafficBusy = false;
  }
}