/*
  LEDControl.ino
  ----------------
  Complete firmware for Arduino Mega controlling:
    - 12 Green LEDs
    - 8 Blue LEDs (direction indicators)
    - 8 Yellow LEDs
    - 4 Orange LEDs
    - 15 Condition push-buttons + 1 Control button
    - 2 x 3-position ON-OFF-ON toggle switches (direction)
    - 20x4 I2C LCD

  Uses arrays for all pin mappings so you can easily reconfigure.
*/

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

// -------------------------
// 1) PIN MAPPINGS (edit here)
// -------------------------

// Green LEDs (12)
const uint8_t greenLEDs[] = {22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44};
const size_t NUM_GREEN = sizeof(greenLEDs) / sizeof(greenLEDs[0]);

// Blue LEDs (8)
const uint8_t blueLEDs[]  = {A0, A1, A2, A3, A4, A5, A6, A7};
const size_t NUM_BLUE  = sizeof(blueLEDs)  / sizeof(blueLEDs[0]);

// Yellow LEDs (8)
const uint8_t yellowLEDs[]  = {A8, A9, A10, A11, A12, A13, A14, A15};
const size_t NUM_YELLOW  = sizeof(yellowLEDs)  / sizeof(yellowLEDs[0]);

// Orange LEDs (4)
const uint8_t orangeLEDs[] = {3, 4, 5, 6};
const size_t NUM_ORANGE = sizeof(orangeLEDs) / sizeof(orangeLEDs[0]);

// Condition Push-Buttons (15)
const uint8_t buttonPins[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
const size_t NUM_BTN = sizeof(buttonPins) / sizeof(buttonPins[0]);

// Control button
const uint8_t controlPin = 17;

// Toggle switches (each: two digital pins)
// Wire each ON terminal to one pin; center OFF leaves both pulled HIGH
const uint8_t toggle1A = A0, toggle1B = A1;
const uint8_t toggle2A = A2, toggle2B = A3;

// I2C LCD (20x4)
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Debounce parameters
const unsigned long DEBOUNCE_MS = 50;

// -------------------------
// 2) SIMPLE DEBOUNCE CLASS
// -------------------------
class DebouncedButton {
public:
  DebouncedButton() {}
  void begin(uint8_t p) {
    pin = p;
    pinMode(pin, INPUT_PULLUP);
    state = lastRead = HIGH;
    lastChange = millis();
  }
  void update() {
    int r = digitalRead(pin);
    if (r != lastRead) lastChange = millis();
    if ((millis() - lastChange) > DEBOUNCE_MS) {
      if (r != state) {
        state = r;
        changed = true;
      }
    }
    lastRead = r;
  }
  bool fell() {
    if (changed && state == LOW) { changed = false; return true; }
    changed = false;
    return false;
  }
private:
  uint8_t pin;
  int state, lastRead;
  unsigned long lastChange;
  bool changed;
};

DebouncedButton btns[NUM_BTN];
DebouncedButton controlBtn;

// -------------------------
// 3) DIRECTION HANDLING
// -------------------------
enum Direction { DIR_NULL, DIR_N, DIR_NE, DIR_E, DIR_SE, DIR_S, DIR_SW, DIR_W, DIR_NW };

// Read one toggle: returns 0=first ON, 1=OFF, 2=second ON
uint8_t readToggle(uint8_t aPin, uint8_t bPin) {
  if (digitalRead(aPin) == LOW && digitalRead(bPin) == HIGH) return 0;
  if (digitalRead(aPin) == HIGH && digitalRead(bPin) == HIGH) return 1;
  if (digitalRead(aPin) == HIGH && digitalRead(bPin) == LOW ) return 2;
  return 1;
}

// Compute combined direction
Direction readDirection() {
  uint8_t v1 = readToggle(toggle1A, toggle1B);
  uint8_t v2 = readToggle(toggle2A, toggle2B);
  if (v1==0 && v2==1) return DIR_N;
  if (v1==2 && v2==1) return DIR_S;
  if (v1==1 && v2==0) return DIR_E;
  if (v1==1 && v2==2) return DIR_W;
  if (v1==0 && v2==0) return DIR_NE;
  if (v1==0 && v2==2) return DIR_NW;
  if (v1==2 && v2==0) return DIR_SE;
  if (v1==2 && v2==2) return DIR_SW;
  return DIR_NULL;
}

// Update blue LEDs to show direction
void updateBlue(Direction d) {
  // turn all off
  for (size_t i = 0; i < NUM_BLUE; i++) digitalWrite(blueLEDs[i], LOW);
  // turn on the one index
  int idx = -1;
  switch (d) {
    case DIR_N:  idx = 0; break;
    case DIR_S:  idx = 1; break;
    case DIR_E:  idx = 2; break;
    case DIR_W:  idx = 3; break;
    case DIR_NE: idx = 4; break;
    case DIR_NW: idx = 5; break;
    case DIR_SE: idx = 6; break;
    case DIR_SW: idx = 7; break;
    default: break;
  }
  if (idx >= 0) digitalWrite(blueLEDs[idx], HIGH);
}

// -------------------------
// 4) SCENARIO LOGIC STUBS
// -------------------------
// Evaluate which scenario (1..7)
int evaluateScenario() {
  // TODO: combine controlBtn and any additional logic to decide scenario index
  // For now, always Scenario 1
  return 1;
}

void applyScenario(int scen, Direction dir) {
  // 3-7: all yellow ON + fixed LCD text
  if (scen >=3 && scen<=7) {
    for (size_t i=0;i<NUM_YELLOW;i++) digitalWrite(yellowLEDs[i], HIGH);
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("Shelter in Place");
    return;
  }

  // Scenario 1 & 2: per-button logic
  // TODO: implement full table mapping based on your spec
  // Example: if btn #0 pressed in scenario 1 & dir == DIR_N => turn on greenLEDs[0]
  // You can build arrays of vectors to map (scen, btnIdx, dir) -> list of LED indexes.
}

// -------------------------
// 5) SETUP & LOOP
// -------------------------
void setup() {
  // Initialize LEDs
  for (auto p : greenLEDs ) pinMode(p, OUTPUT), digitalWrite(p, LOW);
  for (auto p : blueLEDs  ) pinMode(p, OUTPUT), digitalWrite(p, LOW);
  for (auto p : yellowLEDs) pinMode(p, OUTPUT), digitalWrite(p, LOW);
  for (auto p : orangeLEDs) pinMode(p, OUTPUT), digitalWrite(p, LOW);

  // Buttons
  for (size_t i=0;i<NUM_BTN;i++) btns[i].begin(buttonPins[i]);
  controlBtn.begin(controlPin);

  // Toggles
  pinMode(toggle1A, INPUT_PULLUP);
  pinMode(toggle1B, INPUT_PULLUP);
  pinMode(toggle2A, INPUT_PULLUP);
  pinMode(toggle2B, INPUT_PULLUP);

  // LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
}

void loop() {
  // 1) Read inputs
  for (auto &b : btns) b.update();
  controlBtn.update();
  Direction d = readDirection();

  // 2) Update blue LEDs for direction
  updateBlue(d);

  // 3) Determine scenario
  int scen = evaluateScenario();

  // 4) Apply scenario logic
  applyScenario(scen, d);

  // 5) Display button & dir status on LCD (line 3)
  // TODO: update as part of applyScenario for scen 1 & 2
}