/*
  LED Control System for Arduino Mega
  - 12 x Green LEDs
  - 8  x Blue  LEDs (direction indicators)
  - 8  x Yellow LEDs
  - 4  x Orange LEDs
  - 1  x Control momentary pushbutton (global short-press off)
  - 15 x Condition pushbuttons
  - 2  x 3-position toggles (read as two digital inputs each: UP / DOWN; center = neither)
  - 20x4 I2C LCD (LiquidCrystal_I2C)
  
  Features:
  - Arrays for pin definitions so you can change wiring quickly.
  - Debounced button reads.
  - Blue LED patterns reflect direction combos (N, S, E, W, NE, NW, SE, SW).
  - When a PB is pressed the mapping for the current direction is used to set LEDs ON/OFF.
  - If toggles change (direction changes) any active PB-leds are turned OFF (per requirement).
  - Scenarios 3,4,5,6,7: regardless of toggle state, all 8 yellow LEDs turn ON and LCD shows "Shelter in Place"
    (you can adjust which scenario numbers correspond; currently code supports checking a "scenario mode" pin or variable).
  - LCD shows which PB pressed and current direction on line 3.
  
  NOTE:
  - Fill the `pbDirectionMap` structure with your actual mapping (from your doc). See the mapping placeholder and example entries below.
  - This code expects each 3-position toggle to be wired as two digital signals:
      toggle_up_pin is HIGH when toggle is up (N or E)
      toggle_down_pin is HIGH when toggle is down (S or W)
    Centre position = both LOW.
  - Uses LiquidCrystal_I2C library; set correct I2C address if different (default 0x27).
*/

#include <Wire.h>
// #include <LiquidCrystal_I2C.h>

// -------------------- CONFIG: pins & constants --------------------

// I2C LCD
#define LCD_I2C_ADDR 0x27
// LiquidCrystal_I2C lcd(LCD_I2C_ADDR, 20, 4);

// 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] = {
  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;

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

// -------------------- INTERNAL STATE --------------------

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

// Button last read timestamps and state for debouncing
unsigned long lastDebounceTimeControl = 0;
bool lastControlRaw = false;
bool lastControlStable = false;
unsigned long controlDownTime = 0;

// For PBs
unsigned long pbLastDebounce[15];
bool pbLastRaw[15];
bool pbStable[15];

// Keep track of which PBs are currently "active" (pressed to turn leds on)
bool pbActive[15];

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

// Scenario mode variable (some scenarios force "Shelter in Place")
// The uploaded doc lists scenarios 3..7 => Shelter in Place. If you're using
// a scenario selection (separate hardware), read it here. For now, we'll
// provide a simple variable you can change programmatically.
uint8_t currentScenario = 0;  // set externally if you have scenario-select hardware

// -------------------- DIRECTION & BLUE PATTERNS --------------------
// Index mapping for direction patterns (bit i = LED i)
const uint8_t DIR_NULL = 0;
const uint8_t DIR_N = 1;
const uint8_t DIR_S = 2;
const uint8_t DIR_E = 3;
const uint8_t DIR_W = 4;
const uint8_t DIR_NE = 5;
const uint8_t DIR_NW = 6;
const uint8_t DIR_SE = 7;
const uint8_t DIR_SW = 8;

// Blue LED patterns for each direction, bit0 -> bluePins[0], etc.
// Using the Blue LED Logic from your spec: each direction lights exactly one of the 8 blue LEDs
const uint8_t bluePatterns[9] = {
  0b00000000,  // NULL
  0b00000001,  // N  -> LED1
  0b00000010,  // S  -> LED2
  0b00000100,  // E  -> LED3
  0b00001000,  // W  -> LED4
  0b00010000,  // NE -> LED5
  0b00100000,  // NW -> LED6
  0b01000000,  // SE -> LED7
  0b10000000   // SW -> LED8
};

// Helper to get direction string for LCD display
const char *directionNames[9] = {
  "NULL", "N", "S", "E", "W", "NE", "NW", "SE", "SW"
};

// -------------------- PB -> Direction -> LED mapping (USER EDIT) --------------------
/*
  pbDirectionMap[PB_index][directionIndex] = list of LED actions to perform when PB pressed
  For each PB and direction you supply a small structure describing which green/yellow/orange LEDs to turn ON.
  This is intentionally extensible - fill it from your spreadsheet/table in the uploaded doc.

  Here we define a small structure for an action: arrays of indices into color arrays.
  - Use -1 to terminate lists.
  Example: if pressing PB 0 in direction N should turn on greenPins[0] and orangePins[1], you'd set:
     pbDirectionMap[0][DIR_N].greens = {0, -1}
     pbDirectionMap[0][DIR_N].oranges = {1, -1}
     pbDirectionMap[0][DIR_N].yellows = {-1}

  NOTE: Indices are 0-based into the corresponding color arrays.
*/

struct LEDAction {
  int8_t greens[8];   // indices of green LEDs to toggle ON (terminate with -1)
  int8_t yellows[8];  // indices of yellow LEDs to toggle ON (terminate with -1)
  int8_t oranges[8];  // indices of orange LEDs to toggle ON (terminate with -1)
};

// Initialize with empty actions (all -1)
LEDAction emptyAction;
LEDAction pbDirectionMap[15][9];  // 15 PBs x 9 directions

// Example mapping entries: replace/extend these with your actual mapping from the uploaded file.
// This is only illustrative. Replace numbers as needed (indices refer to the arrays above).

// ---------- 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 } }
};

// ---------- Loader: copy pbMap into pbDirectionMap (LEDAction structure) ----------
void initMappingFromArray() {
  for (int p = 0; p < 15; ++p) {
    for (int d = 0; d < 9; ++d) {
      // copy greens
      for (int s = 0; s < 4; ++s) {
        pbDirectionMap[p][d].greens[s] = pbMap[p][d][s];
      }
      // ensure unused yellow/orange lists are empty (-1)
      for (int s = 0; s < 8; ++s) {
        pbDirectionMap[p][d].yellows[s] = -1;
        pbDirectionMap[p][d].oranges[s] = -1;
      }
    }
  }
}

// -------------------- HELPERS --------------------

void digitalWriteSafe(uint8_t pin, bool val) {
  digitalWrite(pin, val ? HIGH : LOW);
}

void setGreenByIndex(uint8_t idx, bool val) {
  if (idx < 12) {
    greenState[idx] = val;
    digitalWriteSafe(greenPins[idx], val);
  }
}
void setBlueByIndex(uint8_t idx, bool val) {
  if (idx < 8) {
    blueState[idx] = val;
    digitalWriteSafe(bluePins[idx], val);
  }
}
void setYellowByIndex(uint8_t idx, bool val) {
  if (idx < 8) {
    yellowState[idx] = val;
    digitalWriteSafe(yellowPins[idx], val);
  }
}
void setOrangeByIndex(uint8_t idx, bool val) {
  if (idx < 4) {
    orangeState[idx] = val;
    digitalWriteSafe(orangePins[idx], val);
  }
}

void clearAllColors() {
  for (int i = 0; i < 12; i++) setGreenByIndex(i, false);
  for (int i = 0; i < 8; i++) setBlueByIndex(i, false);
  for (int i = 0; i < 8; i++) setYellowByIndex(i, false);
  for (int i = 0; i < 4; i++) setOrangeByIndex(i, false);
}

// Turn all yellows ON (for shelter in place)
void setAllYellowsOn(bool on) {
  for (int i = 0; i < 8; i++) setYellowByIndex(i, on);
}
void changeBlueLED(int index){
  
}
// Apply blue pattern for currentDirectionIndex
void applyBluePattern(uint8_t dirIdx) {
  // Serial.print("Apply blue pattern:");
  // Serial.println(dirIdx);
  uint8_t pattern = 0;
  if (dirIdx <= 8) pattern = bluePatterns[dirIdx];
  for (int i = 0; i < 8; i++) {
    bool bit = (pattern >> i) & 0x01;
    setBlueByIndex(i, bit);
  }
}

// Toggle LEDs defined in an action (used when PB pressed)
// If pbActive[pb] was false, turn specified LEDs ON and mark active.
// If pbActive[pb] was true, turn specified LEDs OFF and clear active.
void applyActionForPB(uint8_t pbIndex, uint8_t dirIdx) {
  LEDAction &act = pbDirectionMap[pbIndex][dirIdx];
  if (!pbActive[pbIndex]) {
    // Turn ON listed LEDs
    for (int i = 0; i < 8; i++) {
      int8_t g = act.greens[i];
      if (g < 0) break;
      setGreenByIndex(g, true);
    }
    for (int i = 0; i < 8; i++) {
      int8_t y = act.yellows[i];
      if (y < 0) break;
      setYellowByIndex(y, true);
    }
    for (int i = 0; i < 8; i++) {
      int8_t o = act.oranges[i];
      if (o < 0) break;
      setOrangeByIndex(o, true);
    }
    pbActive[pbIndex] = true;
  } else {
    // Turn OFF listed LEDs
    for (int i = 0; i < 8; i++) {
      int8_t g = act.greens[i];
      if (g < 0) break;
      setGreenByIndex(g, false);
    }
    for (int i = 0; i < 8; i++) {
      int8_t y = act.yellows[i];
      if (y < 0) break;
      setYellowByIndex(y, false);
    }
    for (int i = 0; i < 8; i++) {
      int8_t o = act.oranges[i];
      if (o < 0) break;
      setOrangeByIndex(o, false);
    }
    pbActive[pbIndex] = false;
  }
}

// When direction changes, spec says: "If user switches a scenario in that case as well if the led’s are on turn them off."
// We'll implement: when direction changes, clear any PB-active LEDs (all colors except blue).
void handleDirectionChangeReset() {
  for (int p = 0; p < 15; p++) {
    if (pbActive[p]) {
      // turn off the LEDs that were turned on by that PB for the previous direction(s)
      // We'll iterate through all directions to be safe and turn off any possible LEDs assigned to that PB.
      for (int d = 0; d < 9; d++) {
        LEDAction &act = pbDirectionMap[p][d];
        for (int i = 0; i < 8; i++) {
          int8_t g = act.greens[i];
          if (g < 0) break;
          setGreenByIndex(g, false);
        }
        for (int i = 0; i < 8; i++) {
          int8_t y = act.yellows[i];
          if (y < 0) break;
          setYellowByIndex(y, false);
        }
        for (int i = 0; i < 8; i++) {
          int8_t o = act.oranges[i];
          if (o < 0) break;
          setOrangeByIndex(o, false);
        }
      }
      pbActive[p] = false;
    }
  }
}

// -------------------- INPUT READ HELPERS --------------------

// 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 DIR_N;
  if (tA == -1 && tB == 0) return DIR_S;
  if (tA == 0 && tB == 1) return DIR_E;
  if (tA == 0 && tB == -1) return DIR_W;
  if (tA == 1 && tB == 1) return DIR_NE;
  if (tA == 1 && tB == -1) return DIR_NW;
  if (tA == -1 && tB == 1) return DIR_SE;
  if (tA == -1 && tB == -1) return DIR_SW;
  return DIR_NULL;
}

// -------------------- SETUP --------------------

void setupPins() {
  initLEDs(greenPins, greenState, 12);
  initLEDs(bluePins, blueState, 8);
  initLEDs(yellowPins, yellowState, 8);
  initLEDs(orangePins, orangeState, 4);


  // Buttons
  pinMode(controlButtonPin, INPUT_PULLUP);  // active LOW (pressed => LOW)
  for (int i = 0; i < 15; i++) {
    pinMode(pbPins[i], INPUT_PULLUP);  // active LOW
    pbLastDebounce[i] = 0;
    pbLastRaw[i] = digitalRead(pbPins[i]) == LOW;
    pbStable[i] = pbLastRaw[i];
    pbActive[i] = false;
  }

  // Toggles; assume they drive HIGH when active; use pull-down or input_pullup depending on wiring.
  // Here we'll use INPUT_PULLUP + wiring that grounds the pin when active? To avoid confusion,
  // we assume toggles provide HIGH when active so use INPUT.
  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 initLEDs(const uint8_t pins[], bool states[], size_t count) {
  for (size_t i = 0; i < count; i++) {
    pinMode(pins[i], OUTPUT);
    digitalWriteSafe(pins[i], LOW);  // Your custom safe function
    states[i] = false;
  }
}


void setup() {
  Serial.begin(115200);
  setupPins();
  initMappingFromArray();

  delay(800);
  Serial.println("Starting");
}

// -------------------- MAIN LOOP --------------------

void loop() {
  // 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 changed, reset PB-actives (per requirement)
  if (newDir != currentDirectionIndex) {
    currentDirectionIndex = newDir;
    handleDirectionChangeReset();
  }

  // 2) Apply blue LEDs according to direction (blue LEDs always reflect toggle, independent of other state)
  applyBluePattern(currentDirectionIndex);

  // // 3) If currentScenario is 3..7: shelter in place - all yellow on & LCD message
  if (currentScenario >= 3 && currentScenario <= 7) {
    Serial.print("Current Scenario:");
    Serial.println(currentScenario);
    setAllYellowsOn(true);
    // we still continue reading PBs but ignore PB-based changes (spec says "regardless of toggle show these results")
    // If you want to block PBs entirely in this mode, add a conditional to skip PB handling below.
  } else {
    // If not shelter mode, ensure yellows not forcibly ON (they are controlled by PB actions)
    // We don't automatically turn them off here because PB actions control them. Keep as is.
  }


  // // 4) Read control button with debounce and short-press detection
  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;
          Serial.print("Button pressed for more than 3 second, change scenario to:");
          Serial.println(currentScenario);
          // long press (not specified) - ignore or implement later
        }
      }
    }
  }

  // 5) Read PBs (debounced). If pressed, apply action for currentDirectionIndex.
  for (int i = 0; i < 15; i++) {
    bool raw = (digitalRead(pbPins[i]) == LOW);  // active LOW
    if (raw != pbLastRaw[i]) {
      pbLastDebounce[i] = now;
      pbLastRaw[i] = raw;
    }
    if ((now - pbLastDebounce[i]) > DEBOUNCE_MS) {
      if (pbStable[i] != raw) {
        pbStable[i] = raw;
        if (pbStable[i]) {
          // button i pressed -- apply PB -> action for current direction
          // If there is no action defined for this direction, nothing happens.
          applyActionForPB(i, currentDirectionIndex);
          // Show on LCD which PB pressed + direction on line 3 (index 2 used above for direction; we'll show PB on line 3)
          char buf[21];
          snprintf(buf, sizeof(buf), "PB %02d pressed", i + 1);
        }
      }
    }
  }

  // small loop delay
  delay(30);
}