// Debounce and timing
const unsigned long DEBOUNCE_MS = 40;
const unsigned long CONTROL_SHORTPRESS_MAX_MS = 500;

unsigned long lastDebounceTimeControl = 0;
bool lastControlRaw = false;
bool lastControlStable = false;
unsigned long controlDownTime = 0;

// LED pin arrays (change these to match your wiring)
const uint8_t greenPins[12] = {
  22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44
};
const uint8_t bluePins[8] = {  // N, S, E, W, NE, NW, SE, SW
  A0, A1, A2, A3, A4, A5, A6, A7
};
const uint8_t yellowPins[8] = {
  A8, A9, A10, A11, A12, A13, A14, A15
};
const uint8_t orangePins[4] = {
  3, 4, 5, 6
};

// Pushbuttons
const uint8_t controlButtonPin = 12;  // short press = turn off active LEDs
const uint8_t pbPins[15] = {
  23, 25, 27, 29, 31, 33, 35, 37, 39, 41, 43, 45, 47, 49, 51
};

// Toggle switches (each toggle uses two pins: UP and DOWN)
// Toggle A selects N / OFF / S  (UP => N, DOWN => S)
// Toggle B selects E / OFF / W  (UP => E, DOWN => W)
const uint8_t toggleA_up_pin = 8;
const uint8_t toggleA_down_pin = 9;
const uint8_t toggleB_up_pin = 10;
const uint8_t toggleB_down_pin = 11;

// LED logical states
bool greenState[12];
bool blueState[8];
bool yellowState[8];
bool orangeState[4];

// Current direction index (0 = NULL, 1=N,2=S,3=E,4=W,5=NE,6=NW,7=SE,8=SW)
uint8_t currentDirectionIndex = 15;

uint8_t currentScenario = 1;  // set externally if you have scenario-select hardware

// ---------- Compact PB -> Direction -> Green Indices array ----------
// pbMap[PB_index][directionIndex][slot]  (max 4 entries per mapping, -1 terminator)
// Direction order follows constants: 0=NULL,1=N,2=S,3=E,4=W,5=NE,6=NW,7=SE,8=SW

const int8_t pbMap[15][9][4] = {
  // PB01
  { { 6, -1, -1, -1 }, { 4, -1, -1, -1 }, { 5, 6, -1, -1 }, { 4, 5, -1, -1 }, { 6, -1, -1, -1 }, { 4, -1, -1, -1 }, { 6, -1, -1, -1 }, { 5, 6, -1, -1 }, { 4, 5, -1, -1 } },
  // PB02
  { { 11, -1, -1, -1 }, { 0, 11, -1, -1 }, { 1, -1, -1, -1 }, { 1, -1, -1, -1 }, { 11, -1, -1, -1 }, { 0, 11, -1, -1 }, { 0, 11, -1, -1 }, { 11, -1, -1, -1 }, { 1, -1, -1, -1 } },
  // PB03
  { { 7, -1, -1, -1 }, { 7, -1, -1, -1 }, { 6, -1, -1, -1 }, { 5, -1, -1, -1 }, { 7, -1, -1, -1 }, { 7, -1, -1, -1 }, { 5, -1, -1, -1 }, { 6, 7, -1, -1 }, { 6, -1, -1, -1 } },
  // PB04
  { { 2, -1, -1, -1 }, { 3, 2, -1, -1 }, { 4, -1, -1, -1 }, { 3, 2, -1, -1 }, { 4, -1, -1, -1 }, { 3, 2, -1, -1 }, { 3, 2, -1, -1 }, { 4, -1, -1, -1 }, { 3, -1, -1, -1 } },
  // PB05
  { { 2, -1, -1, -1 }, { 2, -1, -1, -1 }, { 3, -1, -1, -1 }, { 3, 2, -1, -1 }, { 2, -1, -1, -1 }, { 2, -1, -1, -1 }, { 3, 2, -1, -1 }, { 3, -1, -1, -1 }, { 3, -1, -1, -1 } },
  // PB06
  { { 1, -1, -1, -1 }, { 2, -1, -1, -1 }, { 3, -1, -1, -1 }, { 3, 2, -1, -1 }, { 2, -1, -1, -1 }, { 2, -1, -1, -1 }, { 3, 2, -1, -1 }, { 3, -1, -1, -1 }, { 3, -1, -1, -1 } },
  // PB07
  { { 3, -1, -1, -1 }, { 1, -1, -1, -1 }, { 2, -1, -1, -1 }, { 3, 2, -1, -1 }, { 2, 1, -1, -1 }, { 1, -1, -1, -1 }, { 1, 2, -1, -1 }, { 2, -1, -1, -1 }, { 2, -1, -1, -1 } },
  // PB08
  { { 2, -1, -1, -1 }, { 1, -1, -1, -1 }, { 2, -1, -1, -1 }, { 2, 1, -1, -1 }, { 2, -1, -1, -1 }, { 1, -1, -1, -1 }, { 1, -1, -1, -1 }, { 2, -1, -1, -1 }, { 2, 1, -1, -1 } },
  // PB09
  { { 8, -1, -1, -1 }, { 8, -1, -1, -1 }, { 6, 7, -1, -1 }, { 8, -1, -1, -1 }, { 7, 8, -1, -1 }, { 7, -1, -1, -1 }, { 8, -1, -1, -1 }, { 6, 7, -1, -1 }, { 6, 7, -1, -1 } },
  // PB10
  { { 8, -1, -1, -1 }, { 8, -1, -1, -1 }, { 7, -1, -1, -1 }, { 7, -1, -1, -1 }, { 7, 8, -1, -1 }, { 8, -1, -1, -1 }, { 8, -1, -1, -1 }, { 7, -1, -1, -1 }, { 6, 7, -1, -1 } },
  // PB11
  { { 7, -1, -1, -1 }, { 9, -1, -1, -1 }, { 8, -1, -1, -1 }, { 7, -1, -1, -1 }, { 8, 9, -1, -1 }, { 9, -1, -1, -1 }, { 9, -1, -1, -1 }, { 8, -1, -1, -1 }, { 6, 7, -1, -1 } },
  // PB12
  { { 7, 8, -1, -1 }, { 9, -1, -1, -1 }, { 8, -1, -1, -1 }, { 8, -1, -1, -1 }, { 8, 9, -1, -1 }, { 9, -1, -1, -1 }, { 9, -1, -1, -1 }, { 8, -1, -1, -1 }, { 8, -1, -1, -1 } },
  // PB13
  { { 8, -1, -1, -1 }, { 10, -1, -1, -1 }, { 9, -1, -1, -1 }, { 10, -1, -1, -1 }, { 9, 10, -1, -1 }, { 9, -1, -1, -1 }, { 10, -1, -1, -1 }, { 9, 10, -1, -1 }, { 9, -1, -1, -1 } },
  // PB14
  { { 9, -1, -1, -1 }, { 10, -1, -1, -1 }, { 9, -1, -1, -1 }, { 9, -1, -1, -1 }, { 9, 10, -1, -1 }, { 10, -1, -1, -1 }, { 10, -1, -1, -1 }, { 9, -1, -1, -1 }, { 9, -1, -1, -1 } },
  // PB15
  { { 9, -1, -1, -1 }, { 11, -1, -1, -1 }, { 10, -1, -1, -1 }, { 11, -1, -1, -1 }, { 11, -1, -1, -1 }, { 11, -1, -1, -1 }, { 11, -1, -1, -1 }, { 9, 10, -1, -1 }, { 9, -1, -1, -1 } }
};

void applyBluePattern(uint8_t dirIdx) {
  setLedState(bluePins, blueState, 8, LOW, HIGH);
  // else
    digitalWrite(bluePins[dirIdx], HIGH);  // Turn on blue led according to index
}
// Read toggle that has up/down pins. Return +1 for UP, 0 for center, -1 for DOWN.
int8_t read3PosToggle(uint8_t upPin, uint8_t downPin) {
  bool up = digitalRead(upPin);
  bool down = digitalRead(downPin);
  if (up && !down) return 1;
  if (down && !up) return -1;
  return 0;
}

// Convert toggle combination to direction index
uint8_t getDirectionFromToggles(int8_t tA, int8_t tB) {
  // tA: +1=N, 0=OFF, -1=S
  // tB: +1=E, 0=OFF, -1=W
  if (tA == 1 && tB == 0) return 0;
  if (tA == -1 && tB == 0) return 1;
  if (tA == 0 && tB == 1) return 2;
  if (tA == 0 && tB == -1) return 3;
  if (tA == 1 && tB == 1) return 4;
  if (tA == 1 && tB == -1) return 5;
  if (tA == -1 && tB == 1) return 6;
  if (tA == -1 && tB == -1) return 7;
  return 8;
}

// -------------------- SETUP --------------------
void setupPins() {
  setLedState(greenPins, greenState, 12, LOW, HIGH);
  setLedState(bluePins, blueState, 8, LOW, HIGH);
  setLedState(yellowPins, yellowState, 8, LOW, HIGH);
  setLedState(orangePins, orangeState, 4, LOW, HIGH);


  // Buttons
  pinMode(controlButtonPin, INPUT_PULLUP);  // active LOW (pressed => LOW)
  for (int i = 0; i < 15; i++) {
    pinMode(pbPins[i], INPUT_PULLUP);  // active LOW
  }
  pinMode(toggleA_up_pin, INPUT_PULLUP);
  pinMode(toggleA_down_pin, INPUT_PULLUP);
  pinMode(toggleB_up_pin, INPUT_PULLUP);
  pinMode(toggleB_down_pin, INPUT_PULLUP);
}
void setLedState(const uint8_t pins[], bool states[], size_t count, bool ledState, bool initialize) {
  for (size_t i = 0; i < count; i++) {
    if (initialize) pinMode(pins[i], OUTPUT);
    digitalWrite(pins[i], ledState);  // Your custom safe function
    states[i] = false;
  }
}
void setup() {
  Serial.begin(115200);
  setupPins();
  Serial.println("Starting");
}

void loop() {
  readToggleSwitches();  // Read toggle switches and take action

  bool rawControl = (digitalRead(controlButtonPin) == LOW);  // pressed active LOW
  unsigned long now = millis();
  if (rawControl != lastControlRaw) {
    lastDebounceTimeControl = now;
    lastControlRaw = rawControl;
  }
  if ((now - lastDebounceTimeControl) > DEBOUNCE_MS) {
    if (rawControl != lastControlStable) {
      lastControlStable = rawControl;
      if (lastControlStable) {
        // button just pressed
        controlDownTime = now;
      } else {
        // button released: check press length
        unsigned long len = now - controlDownTime;
        Serial.print("Control button was pressed for ");
        Serial.println(len);
        if (len <= CONTROL_SHORTPRESS_MAX_MS) {
          Serial.println("Control Button Short Pressed");
          // short press: turn off all PB-activated LEDs (not blue)
          // handleDirectionChangeReset();
        } else if (len > 3000) {
          currentScenario = currentScenario + 1 > 7 ? 1 : currentScenario + 1;
          if (currentScenario > 2) setLedState(yellowPins, yellowState, 8, HIGH, LOW);  // Turn on yellow leds
          else setLedState(yellowPins, yellowState, 8, LOW, LOW);                       // Turn off yellow leds
          Serial.print("Button pressed for more than 3 second, change scenario to:");
          Serial.println(currentScenario);
          // long press (not specified) - ignore or implement later
        }
      }
    }
  }
}
void readToggleSwitches() {
  // 1) Read toggles and compute direction
  int8_t tA = read3PosToggle(toggleA_up_pin, toggleA_down_pin);
  int8_t tB = read3PosToggle(toggleB_up_pin, toggleB_down_pin);
  uint8_t newDir = getDirectionFromToggles(tA, tB);

  // If direction changes, turn off all leds
  if (newDir != currentDirectionIndex) {
    currentDirectionIndex = newDir;
    setLedState(greenPins, greenState, 12, LOW, LOW);  // Turn off green leds
    // setLedState(yellowPins, yellowState, 8, LOW, LOW);  // Turn off yellow leds
    setLedState(orangePins, orangeState, 4, LOW, LOW);  // Turn off orange leds
    // 2) Apply blue LEDs according to direction (blue LEDs always reflect toggle, independent of other state)
    applyBluePattern(currentDirectionIndex);
  }
}