// Arduino UNO - Car/Pedestrian crossing semaphore state machine demo
#include <Chrono.h>

// Pin mapping (change as needed)
const uint8_t CAR_RED_PIN = 10;
const uint8_t CAR_YELLOW_PIN = 9;
const uint8_t CAR_GREEN_PIN = 8;
const uint8_t PED_RED_PIN = 7;
const uint8_t PED_GREEN_PIN = 6;
const uint8_t PED_BUTTON_PIN = 5; // using INPUT_PULLUP, pressed = LOW
const uint8_t BUZZER_PIN = 4;

// Timing (milliseconds)
const unsigned long MIN_CAR_GREEN_MS = 5000;
const unsigned long CAR_YELLOW_MS = 2000;
const unsigned long ALL_RED_BEFORE_PED_MS = 1000;
const unsigned long PED_GREEN_MS = 5000;
const unsigned long ALL_RED_AFTER_PED_MS = 1000;
const unsigned long CAR_RED_YELLOW_MS = 1500;
const unsigned long STATE_LOG_INTERVAL_MS = 1000;
const unsigned long BUTTON_DEBOUNCE_MS = 30;
const unsigned long BUZZER_BEEP_ON_MS = 20;
const unsigned long BUZZER_INTERVAL_PED_RED_MS = 1200;
const unsigned long BUZZER_INTERVAL_PED_GREEN_MS = 300;
const unsigned int BUZZER_TONE_HZ = 400;

enum State {
  CAR_GREEN_PED_RED,
  CAR_YELLOW_PED_RED,
  ALL_RED_BEFORE_PED,
  CAR_RED_PED_GREEN,
  ALL_RED_AFTER_PED,
  CAR_RED_YELLOW_PED_RED
};

State currentState = CAR_GREEN_PED_RED;

Chrono stateChrono;
Chrono stateLogChrono;
Chrono buttonDebounceChrono(false);

bool pedestrianRequest = false;
unsigned long currentBuzzerIntervalMs = BUZZER_INTERVAL_PED_RED_MS;
unsigned long lastBeepStartMs = 0;
bool buzzerActive = false;

// Debounce state
int lastRawButton = HIGH;
int debouncedButton = HIGH;
bool debouncePending = false;

const char* stateName(State s) {
  switch (s) {
    case CAR_GREEN_PED_RED:      return "CAR_GREEN_PED_RED";
    case CAR_YELLOW_PED_RED:     return "CAR_YELLOW_PED_RED";
    case ALL_RED_BEFORE_PED:     return "ALL_RED_BEFORE_PED";
    case CAR_RED_PED_GREEN:      return "CAR_RED_PED_GREEN";
    case ALL_RED_AFTER_PED:      return "ALL_RED_AFTER_PED";
    case CAR_RED_YELLOW_PED_RED: return "CAR_RED_YELLOW_PED_RED";
    default:                     return "UNKNOWN";
  }
}

void applyOutputsForState(State s) {
  switch (s) {
    case CAR_GREEN_PED_RED:
      digitalWrite(CAR_RED_PIN, LOW);
      digitalWrite(CAR_YELLOW_PIN, LOW);
      digitalWrite(CAR_GREEN_PIN, HIGH);
      digitalWrite(PED_RED_PIN, HIGH);
      digitalWrite(PED_GREEN_PIN, LOW);
      break;

    case CAR_YELLOW_PED_RED:
      digitalWrite(CAR_RED_PIN, LOW);
      digitalWrite(CAR_YELLOW_PIN, HIGH);
      digitalWrite(CAR_GREEN_PIN, LOW);
      digitalWrite(PED_RED_PIN, HIGH);
      digitalWrite(PED_GREEN_PIN, LOW);
      break;

    case ALL_RED_BEFORE_PED:
    case ALL_RED_AFTER_PED:
      digitalWrite(CAR_RED_PIN, HIGH);
      digitalWrite(CAR_YELLOW_PIN, LOW);
      digitalWrite(CAR_GREEN_PIN, LOW);
      digitalWrite(PED_RED_PIN, HIGH);
      digitalWrite(PED_GREEN_PIN, LOW);
      break;

    case CAR_RED_PED_GREEN:
      digitalWrite(CAR_RED_PIN, HIGH);
      digitalWrite(CAR_YELLOW_PIN, LOW);
      digitalWrite(CAR_GREEN_PIN, LOW);
      digitalWrite(PED_RED_PIN, LOW);
      digitalWrite(PED_GREEN_PIN, HIGH);
      break;

    case CAR_RED_YELLOW_PED_RED:
      digitalWrite(CAR_RED_PIN, HIGH);
      digitalWrite(CAR_YELLOW_PIN, HIGH);
      digitalWrite(CAR_GREEN_PIN, LOW);
      digitalWrite(PED_RED_PIN, HIGH);
      digitalWrite(PED_GREEN_PIN, LOW);
      break;
  }
}

void transitionTo(State next, const char* reason) {
  if (next == currentState) return;

  Serial.print("Transition: ");
  Serial.print(stateName(currentState));
  Serial.print(" -> ");
  Serial.print(stateName(next));
  Serial.print(" | reason: ");
  Serial.print(reason);
  Serial.print(" | t=");
  Serial.println(millis());

  currentState = next;
  stateChrono.restart();
  applyOutputsForState(currentState);

  Serial.print("State entered: ");
  Serial.println(stateName(currentState));
}

void setup() {
  pinMode(CAR_RED_PIN, OUTPUT);
  pinMode(CAR_YELLOW_PIN, OUTPUT);
  pinMode(CAR_GREEN_PIN, OUTPUT);
  pinMode(PED_RED_PIN, OUTPUT);
  pinMode(PED_GREEN_PIN, OUTPUT);

  pinMode(PED_BUTTON_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);

  Serial.begin(9600);

  currentState = CAR_GREEN_PED_RED;
  stateChrono.restart();
  stateLogChrono.restart();

  applyOutputsForState(currentState);

  Serial.println("Semaphore FSM started");
  Serial.print("Initial state: ");
  Serial.println(stateName(currentState));
}

bool isPedestrianGreen(State s) {
  return s == CAR_RED_PED_GREEN;
}

void updateBuzzer() {
  unsigned long now = millis();
  unsigned long targetInterval = isPedestrianGreen(currentState)
                                 ? BUZZER_INTERVAL_PED_GREEN_MS
                                 : BUZZER_INTERVAL_PED_RED_MS;

  // Apply pace changes immediately when ped signal changes.
  if (targetInterval != currentBuzzerIntervalMs) {
    currentBuzzerIntervalMs = targetInterval;
    lastBeepStartMs = 0;
  }

  if (!buzzerActive) {
    if (lastBeepStartMs == 0 || (now - lastBeepStartMs) >= currentBuzzerIntervalMs) {
      lastBeepStartMs = now;
      buzzerActive = true;
      tone(BUZZER_PIN, BUZZER_TONE_HZ);
    }
  } else {
    if ((now - lastBeepStartMs) >= BUZZER_BEEP_ON_MS) {
      buzzerActive = false;
      noTone(BUZZER_PIN);
    }
  }
}

void pollPedestrianButton() {
  int raw = digitalRead(PED_BUTTON_PIN);

  if (raw != lastRawButton) {
    lastRawButton = raw;
    buttonDebounceChrono.restart();
    debouncePending = true;
  }

  if (debouncePending && buttonDebounceChrono.hasPassed(BUTTON_DEBOUNCE_MS)) {
    debouncePending = false;

    if (lastRawButton != debouncedButton) {
      debouncedButton = lastRawButton;

      // Active LOW press edge
      if (debouncedButton == LOW) {
        pedestrianRequest = true;
        Serial.print("Button pressed -> pedestrian request queued | t=");
        Serial.println(millis());
      }
    }
  }
}

void logCurrentStatePeriodically() {
  if (stateLogChrono.hasPassed(STATE_LOG_INTERVAL_MS, true)) {
    Serial.print("Current state: ");
    Serial.print(stateName(currentState));
    Serial.print(" | in state for ");
    Serial.print(stateChrono.elapsed());
    Serial.print(" ms");
    Serial.print(" | pedRequest=");
    Serial.println(pedestrianRequest ? "true" : "false");
  }
}

void runStateMachine() {
  switch (currentState) {
    case CAR_GREEN_PED_RED:
      if (pedestrianRequest && stateChrono.hasPassed(MIN_CAR_GREEN_MS)) {
        transitionTo(CAR_YELLOW_PED_RED, "ped request and minimum green elapsed");
      }
      break;

    case CAR_YELLOW_PED_RED:
      if (stateChrono.hasPassed(CAR_YELLOW_MS)) {
        transitionTo(ALL_RED_BEFORE_PED, "yellow timeout");
      }
      break;

    case ALL_RED_BEFORE_PED:
      if (stateChrono.hasPassed(ALL_RED_BEFORE_PED_MS)) {
        transitionTo(CAR_RED_PED_GREEN, "all-red before pedestrian timeout");
      }
      break;

    case CAR_RED_PED_GREEN:
      if (stateChrono.hasPassed(PED_GREEN_MS)) {
        transitionTo(ALL_RED_AFTER_PED, "pedestrian green timeout");
      }
      break;

    case ALL_RED_AFTER_PED:
      if (stateChrono.hasPassed(ALL_RED_AFTER_PED_MS)) {
        transitionTo(CAR_RED_YELLOW_PED_RED, "all-red after pedestrian timeout");
      }
      break;

    case CAR_RED_YELLOW_PED_RED:
      if (stateChrono.hasPassed(CAR_RED_YELLOW_MS)) {
        pedestrianRequest = false; // served
        transitionTo(CAR_GREEN_PED_RED, "car red+yellow timeout");
      }
      break;
  }
}

void loop() {
  pollPedestrianButton();
  runStateMachine();
  updateBuzzer();
  logCurrentStatePeriodically();
}