// =========================================
//                DEFINES
// =========================================

#define NUMELEMENTS(x) (sizeof(x) / sizeof(x[0]))
#define ISPRESSED LOW

bool isDark = false;  // changed from macro to real variable
long int lastTimePressed;


// =========================================
//                ENUMS
// =========================================

enum StateNames {
  st_Blink,
  st_Red_Cars_Red_Pedestrians,
  st_Green_Cars_Red_Pedestrians,
  stX,
  st_Orange_Cars_Red_Pedestrians,
  st_Red_Cars_Red_Pedestrians_After_Cars_Loop,
  st_Red_Cars_Green_Pedestrians,
  st_Red_Cars_Green_Blinking_Pedestrians,
  st_Red_Cars_Red_Pedestrians_after_Pedestrians_Loop,
};



// =========================================
//               STRUCTS
// =========================================

struct Button {
  const uint8_t pin;
  unsigned long lastDeBounceTime;
  uint8_t lastButtonState;
  uint8_t currDebouncedState;
  uint8_t prevDebouncedState;
};

struct Timer {
  // ======================
  unsigned long startTime = 0;
  bool reset = true;

  void start() {
    startTime = millis();
    reset = false;
  }

  bool expired(unsigned long interval) {
    unsigned long now = millis();
    if (reset) start();

    if (now - startTime >= interval) {
      reset = true;
      return true;
    }
    return false;
  }

  void restart() {
    start();
  }
};

struct Timers {
  Timer flashCar;
  Timer flashPed;
  Timer loop;
} timers;



// =========================================
//           STATE TABLE
// =========================================

struct StateInfo
{
  const StateNames name;
  const char* label;
  // ======================
  unsigned long duration;
  const StateNames next;
};

StateInfo stateTable[] = {
  { st_Blink, "Blink", 6000, st_Red_Cars_Red_Pedestrians },
  { st_Red_Cars_Red_Pedestrians, "Red/Red", 5000, st_Green_Cars_Red_Pedestrians },
  { st_Green_Cars_Red_Pedestrians, "Green/Red", 0, stX },
  { stX, "Crossing", 4000, st_Orange_Cars_Red_Pedestrians },
  { st_Orange_Cars_Red_Pedestrians, "Orange/Red", 3000, st_Red_Cars_Red_Pedestrians_After_Cars_Loop },
  { st_Red_Cars_Red_Pedestrians_After_Cars_Loop, "Red/Red", 3000, st_Red_Cars_Green_Pedestrians },
  { st_Red_Cars_Green_Pedestrians, "Red/Green", 3000, st_Red_Cars_Green_Blinking_Pedestrians },
  { st_Red_Cars_Green_Blinking_Pedestrians, "Red/Blink", 6000, st_Red_Cars_Red_Pedestrians_after_Pedestrians_Loop },
  { st_Red_Cars_Red_Pedestrians_after_Pedestrians_Loop, "Red/Red", 3000, st_Green_Cars_Red_Pedestrians },
};


// Retrieve a state entry by enum
const StateInfo* getStateInfo(StateNames state) {
  for (uint8_t i = 0; i < NUMELEMENTS(stateTable); i++)
    if (stateTable[i].name == state)
      return &stateTable[i];

  return nullptr;  // important fix
}



// =========================================
//               GLOBALS
// =========================================

Button buttonList[] = {
  { 3, 0, !ISPRESSED, !ISPRESSED, !ISPRESSED }
};

uint32_t debounceDelay = 20;

uint8_t pinsTL[] = { 8, 7, 6 };  // Car:   Red, Orange, Green
uint8_t pinsPL[] = { 2, 4 };   // Ped:   Red, Green

int interval_flash = 1000;
static boolean firstRun = true;

unsigned long int timeChangeIntoGreen = 0;
unsigned long int TimeButtonPressed = 0; 



// =========================================
//              UTILITIES
// =========================================

void digitalWrite2(uint8_t pin1, uint8_t pin2, uint8_t value) {
  digitalWrite(pin1, value);
  digitalWrite(pin2, value);
}

void allLightsOff() {
  for (uint8_t i = 0; i < NUMELEMENTS(pinsTL); i++)
    digitalWrite(pinsTL[i], LOW);

  for (uint8_t i = 0; i < NUMELEMENTS(pinsPL); i++)
    digitalWrite(pinsPL[i], LOW);
}

void resetState(bool lights = true) {
  timers.flashCar.reset = true;
  timers.flashPed.reset = true;
  timers.loop.reset = true;

  if (lights) allLightsOff();
}



// =========================================
//               BUTTON HANDLER
// =========================================

void readButton() {
  for (uint8_t i = 0; i < NUMELEMENTS(buttonList); i++) {

    uint8_t buttonState = digitalRead(buttonList[i].pin);

    if (buttonState != buttonList[i].lastButtonState)
      buttonList[i].lastDeBounceTime = millis();

    if (millis() - buttonList[i].lastDeBounceTime > debounceDelay) {
      buttonList[i].prevDebouncedState = buttonList[i].currDebouncedState;
      buttonList[i].currDebouncedState = buttonState;
    }

    buttonList[i].lastButtonState = buttonState;
  }
}



// =========================================
//               FLASHER
// =========================================

void flashLED(uint8_t pin, Timer& timer, unsigned long interval) {

  if (timer.expired(interval)) {
    int status = digitalRead(pin);
    digitalWrite(pin, !status);
  }
}



// =========================================
//             STATE HANDLING
// =========================================

void enterState(StateNames state) {
  const StateInfo* info = getStateInfo(state);

  if (!info) {
    Serial.println(F("ERROR: Invalid state"));
    return;
  }

  
  Serial.print(F("Entering state: "));
  Serial.println(info->label);

  if (state == stX) {
    resetState(false);
  } else {
    resetState();
  }
}

StateNames nextByTable(StateNames current) {
  const StateInfo* info = getStateInfo(current);
  if (!info) return current;

  if (timers.loop.expired(info->duration)) {

    Serial.print("Tijd voor entering state: ");
    Serial.println(millis()) ;
    enterState(info->next);
    return info->next;  // fixed
  }
  return current;
}



// =========================================
//                SETUP
// =========================================

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

  // Initialize pins
  for (uint8_t i = 0; i < NUMELEMENTS(pinsTL); i++)
    pinMode(pinsTL[i], OUTPUT);

  for (uint8_t i = 0; i < NUMELEMENTS(pinsPL); i++)
    pinMode(pinsPL[i], OUTPUT);

  for (uint8_t i = 0; i < NUMELEMENTS(buttonList); i++)
    pinMode(buttonList[i].pin, INPUT_PULLUP);

  allLightsOff();
}



// =========================================
//                 MAIN LOOP
// =========================================

void loop() {

  static StateNames currentState = st_Blink;

  readButton();  // keep button debounced


  switch (currentState) {

    case st_Blink:
      if (!isDark) {
        if (timers.loop.expired(getStateInfo(currentState)->duration)) {
          currentState = st_Red_Cars_Red_Pedestrians;
          enterState(currentState);
          break;
        }
      }

      // Flash orange (pinTL[1])
      flashLED(pinsTL[1], timers.flashCar, interval_flash);
      break;


     case st_Red_Cars_Red_Pedestrians:
      digitalWrite2(pinsTL[0], pinsPL[0], HIGH);  // red/red
      currentState = nextByTable(currentState);
      timeChangeIntoGreen = millis();                             // "noteer" de tijd dat het licht groen wordt
      break;


   case st_Green_Cars_Red_Pedestrians:
      digitalWrite2(pinsTL[2], pinsPL[0], HIGH);

      if (buttonList[0].currDebouncedState == ISPRESSED)
      {
        TimeButtonPressed = millis();  // "noteer" de tijd dat de button ingedrukt wordt
        if (TimeButtonPressed - timeChangeIntoGreen > 40000 || firstRun == true)
        {
          Serial.print(millis());
          Serial.println(F("\tstGR to stOR"));
          currentState = st_Orange_Cars_Red_Pedestrians;
          firstRun = false;
        }
        else
        {
          Serial.print(millis());
          Serial.println(F("\tstGR to stX"));
          currentState = stX;
          Serial.print(F("TimeButtonPressed = "));
          Serial.println(TimeButtonPressed);
          Serial.print(F("timeChangeIntoGreen = "));
          Serial.println(timeChangeIntoGreen);
          Serial.print(F("delta = "));
          Serial.println(TimeButtonPressed - timeChangeIntoGreen);
          // adjust duration
          stateTable[currentState].duration = 40000 - (TimeButtonPressed - timeChangeIntoGreen);
          //getStateInfo(currentState)->duration = 40000 - (TimeButtonPressed - timeChangeIntoGreen);
        }
        lastTimePressed = millis();
      }
      break;

    case stX:

      currentState = nextByTable(currentState);
      break;

    case st_Orange_Cars_Red_Pedestrians:
      digitalWrite2(pinsTL[1], pinsPL[0], HIGH);
      currentState = nextByTable(currentState);
      break;


    case st_Red_Cars_Red_Pedestrians_After_Cars_Loop:
      digitalWrite2(pinsTL[0], pinsPL[0], HIGH);
      currentState = nextByTable(currentState);
      break;


    case st_Red_Cars_Green_Pedestrians:
      digitalWrite(pinsTL[0], HIGH);  // cars red
      digitalWrite(pinsPL[1], HIGH);  // ped green
      currentState = nextByTable(currentState);
      break;


    case st_Red_Cars_Green_Blinking_Pedestrians:
      digitalWrite(pinsTL[0], HIGH);
      flashLED(pinsPL[1], timers.flashPed, interval_flash);
      currentState = nextByTable(currentState);
      break;


    case st_Red_Cars_Red_Pedestrians_after_Pedestrians_Loop:
      digitalWrite2(pinsTL[0], pinsPL[0], HIGH);
      currentState = nextByTable(currentState);
      timeChangeIntoGreen = millis();
      break;
  }
}