extern "C" {
  #include "traffic_ctrl.h"
}

#include <SevenSegmentTM1637.h>

// ---- Pin map per diagram.json ----
constexpr uint8_t PIN_MASS_RED    = A0;
constexpr uint8_t PIN_MASS_YELLOW = A1;
constexpr uint8_t PIN_MASS_GREEN  = A2;

constexpr uint8_t PIN_BEACON_RED    = A5;
constexpr uint8_t PIN_BEACON_YELLOW = A6;
constexpr uint8_t PIN_BEACON_GREEN  = A7;

constexpr uint8_t PIN_PED_RED   = A3;
constexpr uint8_t PIN_PED_GREEN = A4;

constexpr uint8_t PIN_BTN    = PB0;   // active-low, pull-up
constexpr uint8_t PIN_CLK    = PB10;
constexpr uint8_t PIN_DIO    = PB1;
constexpr uint8_t BUZZER_PIN = PC15;

// ---- Controller ----
static TrafficController g_ctrl;

// ---- 7-seg ----
static SevenSegmentTM1637 tm(PIN_CLK, PIN_DIO);
static uint16_t last_display_val = 0xFFFF;
static bool display_blank = true;

// ---- Fixed-rate scheduler ----
static uint32_t next_tick_1ms   = 0;
static uint32_t next_tick_10ms  = 0;
static uint32_t next_tick_200ms = 0;

// ---- Button debounce (edge-triggered) ----
static uint8_t stable_count = 0;
static bool debounced_released = true; // true = released
static bool prev_debounced_released = true;

static void button_step_1ms(uint32_t now) {
  bool raw_released = (digitalRead(PIN_BTN) == HIGH);

  if (raw_released == debounced_released) {
    if (stable_count < 8) stable_count++;
  } else {
    stable_count = 0;
  }

  if (stable_count >= 8) {
    prev_debounced_released = debounced_released;
    debounced_released = raw_released;

    // Falling edge: released -> pressed
    if (prev_debounced_released == true && debounced_released == false) {
      traffic_ctrl_on_button_press(&g_ctrl, now);
    }
  }
}

// ---- Buzzer sequencer (non-blocking, jitter-tolerant) ----
static bool buzzer_on = false;
static uint32_t next_beep_start = 0;
static uint32_t beep_off_at = 0;

static void buzzer_step_1ms(uint32_t now, bool crosswalk_mode) {
  const uint16_t freq   = crosswalk_mode ? 800  : 1700;
  const uint32_t period = crosswalk_mode ? 200  : 500;   // cadence
  const uint32_t on_ms  = crosswalk_mode ? 50   : 40;    // audible + jitter-safe

  if (buzzer_on && (int32_t)(now - beep_off_at) >= 0) {
    noTone(BUZZER_PIN);
    buzzer_on = false;
  }

  if (!buzzer_on && (int32_t)(now - next_beep_start) >= 0) {
    tone(BUZZER_PIN, freq);
    buzzer_on = true;
    beep_off_at = now + on_ms;
    next_beep_start = now + period;
  }
}

// ---- LEDs ----
static void apply_leds(const TrafficOutputs &o) {
  digitalWrite(PIN_MASS_RED,    o.mass_red);
  digitalWrite(PIN_MASS_YELLOW, o.mass_yellow);
  digitalWrite(PIN_MASS_GREEN,  o.mass_green);

  digitalWrite(PIN_BEACON_RED,    o.beacon_red);
  digitalWrite(PIN_BEACON_YELLOW, o.beacon_yellow);
  digitalWrite(PIN_BEACON_GREEN,  o.beacon_green);

  digitalWrite(PIN_PED_RED,   o.ped_red);
  digitalWrite(PIN_PED_GREEN, o.ped_green);
}

// ---- Display (rate-limited; update-on-change) ----
static void display_step_200ms(uint16_t countdown_s) {
  if (countdown_s == 0) {
    if (!display_blank) {
      tm.clear();
      display_blank = true;
      last_display_val = 0;
    }
    return;
  }

  if (display_blank || countdown_s != last_display_val) {
    tm.clear();             // prevents “9 shows as 90” ghosting
    tm.print((int)countdown_s);
    last_display_val = countdown_s;
    display_blank = false;
  }
}

void setup() {
  pinMode(PIN_MASS_RED, OUTPUT);
  pinMode(PIN_MASS_YELLOW, OUTPUT);
  pinMode(PIN_MASS_GREEN, OUTPUT);

  pinMode(PIN_BEACON_RED, OUTPUT);
  pinMode(PIN_BEACON_YELLOW, OUTPUT);
  pinMode(PIN_BEACON_GREEN, OUTPUT);

  pinMode(PIN_PED_RED, OUTPUT);
  pinMode(PIN_PED_GREEN, OUTPUT);

  pinMode(PIN_BTN, INPUT_PULLUP);

  pinMode(BUZZER_PIN, OUTPUT);
  noTone(BUZZER_PIN);

  tm.begin();
  tm.setBacklight(100);
  tm.setColonOn(false);
  tm.clear();

  uint32_t now = millis();
  traffic_ctrl_init(&g_ctrl, now);

  next_tick_1ms   = now;
  next_tick_10ms  = now;
  next_tick_200ms = now;
}

void loop() {
  uint32_t now = millis();

  // 1ms tasks: button + buzzer
  while ((int32_t)(now - next_tick_1ms) >= 0) {
    button_step_1ms(next_tick_1ms);
    buzzer_step_1ms(next_tick_1ms, g_ctrl.outputs.buzzer_crosswalk_mode);
    next_tick_1ms += 1;
  }

  // 10ms tasks: controller + LEDs
  while ((int32_t)(now - next_tick_10ms) >= 0) {
    traffic_ctrl_step(&g_ctrl, next_tick_10ms);
    apply_leds(g_ctrl.outputs);
    next_tick_10ms += 10;
  }

  // 200ms tasks: display
  while ((int32_t)(now - next_tick_200ms) >= 0) {
    display_step_200ms(traffic_ctrl_get_countdown_s(&g_ctrl));
    next_tick_200ms += 200;
  }

  // No delay()
}