#include <SPI.h>
#include <Adafruit_MCP23X17.h>

Adafruit_MCP23X17 mcp;
#define CS_PIN 10

// Descriptive action names using enum
enum ButtonActions {
  LED0_ON = 1,
  LED0_OFF = 2,
  ALL_LEDS_ON = 3,
  ALL_LEDS_OFF = 4
};

// Ultra-fast polling settings
const unsigned long ULTRA_FAST_CHECK_INTERVAL = 5;    // Check every 5ms (very fast)
const unsigned long MINIMUM_PRESS_TIME = 10;          // Minimum 10ms press detection
const unsigned long ACTION_LOCKOUT = 50;              // 50ms between same button actions

// Button state tracking
uint8_t previous_button_state = 0xFF;  // Start with all released (HIGH)
unsigned long button_press_start_times[4] = {0, 0, 0, 0};
unsigned long last_action_times[4] = {0, 0, 0, 0};
bool button_action_triggered[4] = {false, false, false, false};
unsigned long last_button_check = 0;

// Command source tracking
String last_command_source = "System";  // "Physical", "Serial", or "System"
int last_serial_command = 0;  // Track which serial command was used
int last_physical_button = 0;  // Track which physical button was pressed

// Statistics
unsigned long total_button_checks = 0;
unsigned long successful_detections = 0;
unsigned long missed_presses = 0;

// Action tracking
String last_action = "System ready for ultra-fast detection";
unsigned long last_action_time = 0;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);
  
  Serial.println("==========================================");
  Serial.println("   MCP23S17 ULTRA-FAST Complete Debug   ");
  Serial.println("==========================================");
  
  if (!mcp.begin_SPI(CS_PIN)) {
    Serial.println("ERROR: MCP23S17 not found!");
    while (1) delay(1000);
  }
  
  Serial.println("MCP23S17 found!");
  initializeMCP23S17();
  
  Serial.println("Ultra-Fast Detection Settings:");
  Serial.println("- Check interval: " + String(ULTRA_FAST_CHECK_INTERVAL) + "ms");
  Serial.println("- Minimum press time: " + String(MINIMUM_PRESS_TIME) + "ms");
  Serial.println("- Action lockout: " + String(ACTION_LOCKOUT) + "ms");
  Serial.println("- Method: Enum-based action mapping with source tracking");
  Serial.println();
  Serial.println("Button-to-Action Mapping:");
  Serial.println("- Button 1 (GPIOA4) → LED0_ON");
  Serial.println("- Button 2 (GPIOA5) → LED0_OFF");
  Serial.println("- Button 3 (GPIOA6) → ALL_LEDS_ON");
  Serial.println("- Button 4 (GPIOA7) → ALL_LEDS_OFF");
  Serial.println("==========================================");
  
  last_action_time = millis();
  displayStatus();
  
  Serial.println("\n*** READY FOR ULTRA-FAST BUTTON DETECTION ***");
  Serial.println("Press physical buttons OR use serial commands 1-4");
}

void loop() {
  // Ultra-fast button checking
  if (millis() - last_button_check >= ULTRA_FAST_CHECK_INTERVAL) {
    checkButtonsUltraFast();
    last_button_check = millis();
    total_button_checks++;
  }
  
  // Handle serial commands (non-blocking)
  if (Serial.available()) {
    handleSerialCommands();
  }
  
  // Show statistics every 10 seconds
  static unsigned long last_stats = 0;
  if (millis() - last_stats > 10000) {
    showPerformanceStats();
    last_stats = millis();
  }
}

void initializeMCP23S17() {
  Serial.println("Configuring for ultra-fast detection...");
  
  // Configure button pins with pull-ups
  for (int i = 4; i < 8; i++) {
    mcp.pinMode(i, INPUT_PULLUP);
  }
  
  // Configure LED pins as outputs
  for (int i = 0; i < 4; i++) {
    mcp.pinMode(i, OUTPUT);
    mcp.digitalWrite(i, LOW);
  }
  for (int i = 8; i < 16; i++) {
    mcp.pinMode(i, OUTPUT);
    mcp.digitalWrite(i, LOW);
  }
  
  // Initialize button state
  previous_button_state = readButtonStatesFast();
  
  Serial.println("Ultra-fast configuration complete!");
}

void checkButtonsUltraFast() {
  unsigned long current_time = millis();
  
  // Fast button state read (optimized)
  uint8_t current_button_state = readButtonStatesFast();
  
  // Check each button for changes
  for (int i = 0; i < 4; i++) {
    uint8_t button_mask = (0x10 << i);  // GPIOA4, 5, 6, 7
    
    bool current_pressed = !(current_button_state & button_mask);  // Inverted for pull-up
    bool previous_pressed = !(previous_button_state & button_mask);
    
    // Button press start detection
    if (current_pressed && !previous_pressed) {
      // Button just pressed
      button_press_start_times[i] = current_time;
      button_action_triggered[i] = false;
    }
    
    // Button held check (trigger action after minimum press time)
    if (current_pressed && button_press_start_times[i] > 0 && !button_action_triggered[i]) {
      unsigned long press_duration = current_time - button_press_start_times[i];
      
      if (press_duration >= MINIMUM_PRESS_TIME) {
        // Check action lockout
        if ((current_time - last_action_times[i]) >= ACTION_LOCKOUT) {
          // Mark as physical button press
          last_command_source = "Physical";
          last_serial_command = 0;
          last_physical_button = i + 1;
          
          // Map buttons to actions using descriptive enum names
          switch (i) {
            case 0: triggerButtonAction(LED0_ON, press_duration); break;       // Button 1 → LED0 ON
            case 1: triggerButtonAction(LED0_OFF, press_duration); break;      // Button 2 → LED0 OFF
            case 2: triggerButtonAction(ALL_LEDS_ON, press_duration); break;   // Button 3 → All ON
            case 3: triggerButtonAction(ALL_LEDS_OFF, press_duration); break;  // Button 4 → All OFF
          }
          
          button_action_triggered[i] = true;
          last_action_times[i] = current_time;
          successful_detections++;
        }
      }
    }
    
    // Button release detection
    if (!current_pressed && previous_pressed) {
      unsigned long press_duration = current_time - button_press_start_times[i];
      
      // If button was released too quickly, count as missed
      if (press_duration < MINIMUM_PRESS_TIME && !button_action_triggered[i]) {
        missed_presses++;
      }
      
      button_press_start_times[i] = 0;
      button_action_triggered[i] = false;
    }
  }
  
  previous_button_state = current_button_state;
}

void triggerButtonAction(int action_type, unsigned long press_duration) {
  // Create source information string
  String source_info = "";
  if (last_command_source == "Serial") {
    source_info = " (via Serial command " + String(last_serial_command) + ")";
  } else if (last_command_source == "Physical") {
    source_info = " (via Physical button " + String(last_physical_button) + ")";
  }
  
  // Execute action using descriptive enum names
  switch (action_type) {
    case LED0_ON:  // LED0_ON = 1
      mcp.digitalWrite(0, HIGH);
      Serial.println("***** Button 1 pressed: LED0 turned ON" + source_info + " *****");
      last_action = "LED0 turned ON" + source_info;
      break;
      
    case LED0_OFF:  // LED0_OFF = 2
      mcp.digitalWrite(0, LOW);
      Serial.println("***** Button 2 pressed: LED0 turned OFF" + source_info + " *****");
      last_action = "LED0 turned OFF" + source_info;
      break;
      
    case ALL_LEDS_ON:  // ALL_LEDS_ON = 3
      for (int i = 0; i < 4; i++) mcp.digitalWrite(i, HIGH);
      for (int i = 8; i < 16; i++) mcp.digitalWrite(i, HIGH);
      Serial.println("***** Button 3 pressed: All LEDs turned ON" + source_info + " *****");
      last_action = "All LEDs turned ON" + source_info;
      break;
      
    case ALL_LEDS_OFF:  // ALL_LEDS_OFF = 4
      for (int i = 0; i < 4; i++) mcp.digitalWrite(i, LOW);
      for (int i = 8; i < 16; i++) mcp.digitalWrite(i, LOW);
      Serial.println("***** Button 4 pressed: All LEDs turned OFF" + source_info + " *****");
      last_action = "All LEDs turned OFF" + source_info;
      break;
      
    default:
      Serial.println("***** Unknown action: " + String(action_type) + " *****");
      last_action = "Unknown action: " + String(action_type);
      break;
  }
  
  last_action_time = millis();
  displayStatus();
}

uint8_t readButtonStatesFast() {
  // Optimized register read for maximum speed
  digitalWrite(CS_PIN, LOW);
  SPI.transfer(0x41);  // Read command
  SPI.transfer(0x12);  // GPIOA register
  uint8_t result = SPI.transfer(0x00);
  digitalWrite(CS_PIN, HIGH);
  return result;
}

uint8_t readRegisterDirect(uint8_t reg) {
  // Direct register read for debug purposes
  digitalWrite(CS_PIN, LOW);
  SPI.transfer(0x41);  // Read command
  SPI.transfer(reg);
  uint8_t result = SPI.transfer(0x00);
  digitalWrite(CS_PIN, HIGH);
  return result;
}

void showPerformanceStats() {
  Serial.println("\n===== PERFORMANCE STATISTICS =====");
  Serial.println("Ultra-Fast Detection Performance:");
  Serial.println("- Total button checks: " + String(total_button_checks));
  Serial.println("- Successful detections: " + String(successful_detections));
  Serial.println("- Missed presses (too fast): " + String(missed_presses));
  Serial.println("- Check frequency: " + String(1000.0 / ULTRA_FAST_CHECK_INTERVAL) + " Hz");
  if (successful_detections + missed_presses > 0) {
    Serial.println("- Detection rate: " + String(successful_detections * 100.0 / (successful_detections + missed_presses)) + "%");
  }
  Serial.println();
  Serial.println("Command Usage:");
  Serial.println("- Last command source: " + last_command_source);
  if (last_command_source == "Serial") {
    Serial.println("- Last serial command: " + String(last_serial_command));
  }
  if (last_command_source == "Physical") {
    Serial.println("- Last physical button: " + String(last_physical_button));
  }
  Serial.println("===================================\n");
}

void handleSerialCommands() {
  String input = Serial.readStringUntil('\n');
  input.trim();
  
  if (input.length() == 0) return;
  
  char command = input.charAt(0);
  
  switch (command) {
    case 's':
    case 'S':
      displayStatus();
      break;
      
    case 'h':
    case 'H':
      printHelp();
      break;
      
    case 't':
    case 'T':
      testAllLEDs();
      break;
      
    case 'p':
    case 'P':
      showPerformanceStats();
      break;
      
    case 'r':
    case 'R':
      resetStatistics();
      break;
      
    case '1':
      last_command_source = "Serial";  // Mark as serial command
      last_serial_command = 1;
      last_physical_button = 0;
      triggerButtonAction(LED0_ON, 100);  // 100ms simulated press duration
      break;
      
    case '2':
      last_command_source = "Serial";
      last_serial_command = 2;
      last_physical_button = 0;
      triggerButtonAction(LED0_OFF, 100);
      break;
      
    case '3':
      last_command_source = "Serial";
      last_serial_command = 3;
      last_physical_button = 0;
      triggerButtonAction(ALL_LEDS_ON, 100);
      break;
      
    case '4':
      last_command_source = "Serial";
      last_serial_command = 4;
      last_physical_button = 0;
      triggerButtonAction(ALL_LEDS_OFF, 100);
      break;
      
    default:
      Serial.println("Unknown command '" + String(command) + "'. Enter 'h' for help.");
      break;
  }
}

void resetStatistics() {
  total_button_checks = 0;
  successful_detections = 0;
  missed_presses = 0;
  Serial.println("Performance statistics reset!");
}

void displayStatus() {
  // Read all register states at once for consistency
  uint8_t iodira = readRegisterDirect(0x00);   // IODIRA
  uint8_t iodirb = readRegisterDirect(0x01);   // IODIRB
  uint8_t gppua = readRegisterDirect(0x0C);    // GPPUA
  uint8_t gppub = readRegisterDirect(0x0D);    // GPPUB
  uint8_t gpioa = readRegisterDirect(0x12);    // GPIOA
  uint8_t gpiob = readRegisterDirect(0x13);    // GPIOB
  uint8_t olata = readRegisterDirect(0x14);    // OLATA
  uint8_t olatb = readRegisterDirect(0x15);    // OLATB
  
  Serial.println();
  Serial.println("===== MCP23S17 STATUS =====");
  Serial.println();
  
  // Show last command source and info
  Serial.println("== Last Command Info ==");
  Serial.println("Command Source: " + last_command_source);
  if (last_command_source == "Serial") {
    Serial.println("Serial Command: " + String(last_serial_command));
    Serial.println("Note: Physical button states below show actual hardware, not serial simulation");
  } else if (last_command_source == "Physical") {
    Serial.println("Physical Button: " + String(last_physical_button));
    Serial.println("Press Duration: Actual hardware timing");
  }
  Serial.println("Last Action: " + last_action);
  Serial.println();
  
  // Physical Button States (always show actual hardware state)
  Serial.println("== Button States (Active Low with Pull-ups) ==");
  for (int i = 4; i < 8; i++) {
    bool raw_state = (gpioa & (1 << i)) != 0;
    bool button_pressed = !raw_state;  // Inverted because of pull-ups
    Serial.println("Button " + String(i - 3) + " (GPIOA" + String(i) + "): " + 
                   String(button_pressed ? "PRESSED" : "RELEASED") + 
                   " [Raw: " + String(raw_state ? "HIGH" : "LOW") + "]");
  }
  
  // Add explanatory note for serial commands
  if (last_command_source == "Serial") {
    Serial.println("(Above shows physical button states - not affected by serial commands)");
  }
  Serial.println();
  
  // LED States
  Serial.println("== LED States ==");
  Serial.println();
  
  // Port A LEDs
  Serial.println("== Port A LEDs (GPIOA0-GPIOA3) ==");
  for (int i = 0; i < 4; i++) {
    bool led_state = mcp.digitalRead(i);
    Serial.println("LED" + String(i) + " (GPIOA" + String(i) + "): " + 
                   String(led_state ? "ON" : "OFF"));
  }
  Serial.println();
  
  // Port B LEDs
  Serial.println("== Port B LEDs (GPIOB0-GPIOB7) ==");
  for (int i = 8; i < 16; i++) {
    bool led_state = mcp.digitalRead(i);
    Serial.println("LED" + String(i - 4) + " (GPIOB" + String(i - 8) + "): " + 
                   String(led_state ? "ON" : "OFF"));
  }
  Serial.println();
  
  // Register Values
  Serial.println("== Register Values: ==");
  Serial.println("IODIRA (Direction A): 0x" + String(iodira, HEX) + " = " + formatBinary(iodira));
  Serial.println("IODIRB (Direction B): 0x" + String(iodirb, HEX) + " = " + formatBinary(iodirb));
  Serial.println("GPPUA (Pull-ups A):   0x" + String(gppua, HEX) + " = " + formatBinary(gppua));
  Serial.println("GPPUB (Pull-ups B):   0x" + String(gppub, HEX) + " = " + formatBinary(gppub));
  Serial.println("GPIOA (Port A):       0x" + String(gpioa, HEX) + " = " + formatBinary(gpioa));
  Serial.println("GPIOB (Port B):       0x" + String(gpiob, HEX) + " = " + formatBinary(gpiob));
  Serial.println("OLATA (Latch A):      0x" + String(olata, HEX) + " = " + formatBinary(olata));
  Serial.println("OLATB (Latch B):      0x" + String(olatb, HEX) + " = " + formatBinary(olatb));
  Serial.println();
  
  /// Port A Bit Mapping
  Serial.println("== Port A Bit Mapping ==");
  Serial.println("Bit:     |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0  |");
  Serial.println("Function |BTN4 |BTN3 |BTN2 |BTN1 |LED3 |LED2 |LED1 |LED0 |");
  Serial.print("GPIOA:   |");
  for (int i = 7; i >= 0; i--) {
    bool state = (gpioa & (1 << i)) != 0;
    Serial.print("  " + String(state ? "1" : "0") + "  |");
  }
  Serial.println();
  Serial.print("OLATA:   |");
  for (int i = 7; i >= 0; i--) {
    bool state = (olata & (1 << i)) != 0;
    Serial.print("  " + String(state ? "1" : "0") + "  |");
  }
  Serial.println();
  Serial.println();
  
  // Port B Bit Mapping (CORRECTED)
  Serial.println("== Port B Bit Mapping ==");
  Serial.println("Bit:     |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0  |");
  Serial.println("Function |LED15|LED14|LED13|LED12|LED11|LED10|LED9 |LED8 |");
  Serial.print("GPIOB:   |");
  for (int i = 7; i >= 0; i--) {  //
    bool state = (gpiob & (1 << i)) != 0;
    Serial.print("  " + String(state ? "1" : "0") + "  |");
  }
  Serial.println();
  Serial.print("OLATB:   |");
  for (int i = 7; i >= 0; i--) {  //
    bool state = (olatb & (1 << i)) != 0;
    Serial.print("  " + String(state ? "1" : "0") + "  |");
  }
  Serial.println();
  Serial.println();
  
  // Count active LEDs
  int activeLEDs = 0;
  for (int i = 0; i < 4; i++) {
    if (mcp.digitalRead(i)) activeLEDs++;
  }
  for (int i = 8; i < 16; i++) {
    if (mcp.digitalRead(i)) activeLEDs++;
  }
  
  Serial.println("Active LEDs: " + String(activeLEDs) + "/12 (4 Port A + 8 Port B)");
  Serial.println("Command Source: " + last_command_source + 
                 (last_command_source == "Serial" ? " (Command " + String(last_serial_command) + ")" : 
                  last_command_source == "Physical" ? " (Button " + String(last_physical_button) + ")" : ""));
  Serial.println("Library: Adafruit MCP23X17 v2.0.0 (SPI Mode)");
  Serial.println("============================");
}

String formatBinary(uint8_t value) {
  String binary = "";
  for (int i = 7; i >= 0; i--) {
    binary += (value & (1 << i)) ? "1" : "0";
  }
  return binary;
}

void testAllLEDs() {
  last_command_source = "System";
  last_serial_command = 0;
  last_physical_button = 0;
  last_action = "LED test sequence started";
  
  Serial.println("***** Testing all LEDs... *****");
  
  // Turn off all first
  for (int i = 0; i < 4; i++) mcp.digitalWrite(i, LOW);
  for (int i = 8; i < 16; i++) mcp.digitalWrite(i, LOW);
  delay(500);
  
  // Test Port A LEDs
  for (int i = 0; i < 4; i++) {
    mcp.digitalWrite(i, HIGH);
    Serial.println("LED" + String(i) + " (GPIOA" + String(i) + ") ON");
    delay(300);
    mcp.digitalWrite(i, LOW);
  }
  
  // Test Port B LEDs
  for (int i = 8; i < 16; i++) {
    mcp.digitalWrite(i, HIGH);
    Serial.println("LED" + String(i - 4) + " (GPIOB" + String(i - 8) + ") ON");
    delay(300);
    mcp.digitalWrite(i, LOW);
  }
  
  last_action = "LED test sequence completed";
  Serial.println("***** Test complete - All LEDs turned off *****");
  displayStatus();
}

void printHelp() {
  Serial.println("\n===== ULTRA-FAST DETECTION HELP =====");
  Serial.println("Method: Enum-based Action Mapping with Source Tracking");
  Serial.println();
  Serial.println("Performance Settings:");
  Serial.println("- Check interval: " + String(ULTRA_FAST_CHECK_INTERVAL) + "ms (" + String(1000/ULTRA_FAST_CHECK_INTERVAL) + " Hz)");
  Serial.println("- Minimum press time: " + String(MINIMUM_PRESS_TIME) + "ms");
  Serial.println("- Action lockout: " + String(ACTION_LOCKOUT) + "ms");
  Serial.println();
  Serial.println("Button Functions (Using Enum Names):");
  Serial.println("Button 1 (GPIOA4): LED0_ON (" + String(LED0_ON) + ")");
  Serial.println("Button 2 (GPIOA5): LED0_OFF (" + String(LED0_OFF) + ")");
  Serial.println("Button 3 (GPIOA6): ALL_LEDS_ON (" + String(ALL_LEDS_ON) + ")");
  Serial.println("Button 4 (GPIOA7): ALL_LEDS_OFF (" + String(ALL_LEDS_OFF) + ")");
  Serial.println();
  Serial.println("Serial Commands:");
  Serial.println("s : Show detailed status");
  Serial.println("h : Show this help");
  Serial.println("t : Test all LEDs");
  Serial.println("p : Show performance statistics");
  Serial.println("r : Reset statistics");
  Serial.println("1 : Execute LED0_ON action (simulates Button 1)");
  Serial.println("2 : Execute LED0_OFF action (simulates Button 2)");
  Serial.println("3 : Execute ALL_LEDS_ON action (simulates Button 3)");
  Serial.println("4 : Execute ALL_LEDS_OFF action (simulates Button 4)");
  Serial.println();
  Serial.println("Important Notes:");
  Serial.println("- Physical buttons: Show actual GPIO pin states");
  Serial.println("- Serial commands: Simulate button actions without changing GPIO states");
  Serial.println("- Press duration 100ms: Used for serial command simulation");
  Serial.println("- Command source tracking: Shows whether last action was physical or serial");
  Serial.println();
  Serial.println("Enum Values for Reference:");
  Serial.println("LED0_ON = " + String(LED0_ON));
  Serial.println("LED0_OFF = " + String(LED0_OFF));
  Serial.println("ALL_LEDS_ON = " + String(ALL_LEDS_ON));
  Serial.println("ALL_LEDS_OFF = " + String(ALL_LEDS_OFF));
  Serial.println("======================================\n");
}