/* 
Antenna Switch System for Arduino Mega
Based on original Nano design by 4L7ZS, 4L4CR and 4L0VE

Pin Configuration:
===============
8 Antenna System:
- Buttons (Inputs): D22-D29
- Relays/RX LEDs (Outputs): D30-D37 (parallel)
- TX LEDs (Outputs): D38-D45

7 Additional Toggle System:
- Buttons (Inputs): D10-D16 (moved from A0-A6)
- Relays/LEDs (Outputs): D46-D52 (relays with parallel LEDs)

Mode Buttons (Inputs):
- RX/TX Scan: D2
- Swap: D3
- Zero (0): D4

Status LEDs (Outputs):
- RX Mode LED: D5
- TX Mode LED: D6
- Swap Mode LED: D7
- Block LED: D8

PTT Input: D9 (HIGH = TX, LOW = RX)
I2C EEPROM (24C64): A4(SDA), A5(SCL) - DO NOT USE FOR BUTTONS!
ინსტრუქცია:
1. ნორმალური მოდა: RX და TX ანტენებია ორივე ერთია.
2. RX/TX Scan ღილაკზე ერთხელ დაჭერა - RX Scan მოდა
3. RX/TX Scan ღილაკზე ორჯერ  დაჭერა - TX Scan მოდა
4. SWAP ღილაკი - როცა აქტიურია RX ან TX Scan მოდა, RX და TX ანტენები ცვლიან როლებს.
5. SWAP ღილაკზე კიდევ ერთხელ დაჭერა - აუქმებს SWAP მოდას
6. 005 - ბლოკავს 6, 7 და 8 ანტენებს, როგორც გადაცემაზე, ისე მიღებაზე
7. 000 - აუქმებს დაბლოკილ ანტენებს (ან 008 კომბინაცია)
8. 1-7 დამატებითი ღილაკები ჩართავენ ან გამორთავენ 7 დამატებით რელეს. ჩართულს - გამორთავს, გამორთულს - ჩართავს.
*/

#include <Wire.h>

// I2C EEPROM device address (24C64)
#define EEPROM_ADDR 0x50
unsigned long bootTime = 0;

// EEPROM wear-leveling configuration for 24C64 (8192 bytes)
const uint16_t EEPROM_START_ADDR = 0;
const uint16_t SLOT_COUNT = 480;        // 480 * 17 = 8160 bytes
const uint8_t SLOT_SIZE = 17;           // bytes per slot
const uint8_t PAYLOAD_LEN = SLOT_SIZE - 4; // 13

// Commit debounce for EEPROM (milliseconds)
const unsigned long EEPROM_COMMIT_DELAY = 30000; // Saves states if change is detected after30 seconds

// Slot layout:
// [0] magic(1) = 0xA5
// [1..2] version (uint16_t little-endian)
// [3] checksum (xor of payload bytes)
// [4] maxAntennas (1)
// [5] activeRelay (1)
// [6] rxRelay (1)
// [7] txRelay (1)
// [8] flags (bits: bit0=rxScanMode, bit1=txScanMode, bit2=swapMode)
// [9..16] blockedAntennas[8] (1 byte each, 0/1)

const uint8_t SLOT_MAGIC = 0xA5; // A4-A5 Address 

// Pin Definitions
// 8 Antenna System
const int antennaButtons[8] = {22, 23, 24, 25, 26, 27, 28, 29};    // Input Buttons for 8-Antennas
const int antennaRelays[8] = {30, 31, 32, 33, 34, 35, 36, 37};     // Antenna relays + RX LEDs (in parallel)
const int txLeds[8] = {38, 39, 40, 41, 42, 43, 44, 45};           // Separate TX indicator LEDs

// 7 Additional Toggle System - CHANGED to avoid I2C conflict!
const int toggleButtons[7] = {10, 11, 12, 13, 14, 15, 16};        // Input Buttons for additional 7 relays (D10-D16)
const int toggleRelays[7] = {46, 47, 48, 49, 50, 51, 52};         // Toggle 7-relays + LEDs (in parallel)

// Mode Buttons
const int rxTxScanButton = 2;  // Button for RX - TX scan modes
const int swapButton = 3;      // Button for SWAP mode
const int zeroButton = 4;      // "0" Button 

// Status LEDs
const int rxModeLed = 5;      // RX Mode indicator
const int txModeLed = 6;      // TX Mode indicator
const int swapLed = 7;        // Swap Mode indicator
const int blockLed = 8;       // Block indicator which shows wheather thete is blocked/restrected antennas (blinks)

// PTT Input (HIGH = TX, LOW = RX) - CHANGED to D17 since D10 is now used
const int pttPin = 9;   // PTT button (HIGH = TX, LOW = RX)

// I2C EEPROM pins (A4, A5) - DO NOT USE FOR BUTTONS!

// State variables (runtime)
unsigned long pttReleaseTime = 0; // Start Time for TX->RX transition detection
bool pttInDelay = false;          // ptt delay mode
const unsigned long PTT_DELAY_MS = 100; //TX->RX transition time
bool pttActive = false; // Global PTT state (includes 100ms delay) show it is TX or RX mode

int activeRelay = 0;          // Current active antenna in normal mode
int txRelay = 0;              // TX antenna (in scan modes)
int rxRelay = 0;              // RX antenna (in scan modes)
bool rxScanMode = false;
bool txScanMode = false;
bool swapMode = false;
int maxAntennas = 8;
bool blockedAntennas[8] = {false, false, false, false, false, false, false, false};

// Toggle system states
bool toggleStates[7] = {false, false, false, false, false, false, false};

// Button debouncing variables for antenna buttons
unsigned long lastDebounceTime[8] = {0, 0, 0, 0, 0, 0, 0, 0};
const unsigned long debounceDelay = 5;
bool antennaButtonStates[8] = {LOW, LOW, LOW, LOW, LOW, LOW, LOW, LOW};
bool lastAntennaButtonStates[8] = {LOW, LOW, LOW, LOW, LOW, LOW, LOW, LOW};
unsigned long antennaButtonPressStart[8] = {0, 0, 0, 0, 0, 0, 0, 0};
bool longPressHandled[8] = {false, false, false, false, false, false, false, false};

// Mode button debouncing
bool lastRxTxScanState = HIGH;
bool lastSwapState = HIGH;
bool lastZeroState = HIGH;
unsigned long rxTxScanPressTime = 0;
unsigned long swapPressTime = 0;
unsigned long zeroPressTime = 0;

// Zero button state tracking
bool zeroButtonPressed = false;
bool zeroLongPressHandled = false;

// Toggle button debouncing
bool lastToggleStates[7] = {HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH};

// EEPROM cache
uint8_t cachedPayload[PAYLOAD_LEN];
uint16_t cachedVersion = 0;
uint16_t cachedSlotIdx = 0xFFFF;
bool eepromDirty = false;
unsigned long eepromLastDirtyMs = 0;

// Blinking variables
unsigned long lastBlinkTime = 0;
bool isBlinking = false;
int blinkStep = 0;
int antennaToBlink = -1;
const int BLINK_ON_TIME = 300;
const int BLINK_OFF_TIME = 150;
const int BLINK_COUNT = 3;

// Scan mode button timing
unsigned long lastRxTxScanPress = 0;
bool waitingForSecondPress = false;
const unsigned long DOUBLE_PRESS_MAX_INTERVAL = 800; // Increased to 800ms for better response

// NEW: Sequence detection for 0-0-X command
unsigned long sequenceStartTime = 0;
int zeroPressCount = 0;
const unsigned long SEQUENCE_TIMEOUT = 3000; // 3 seconds for sequence completion
bool waitingForAntennaInSequence = false;

// ---------- NEW: Sequence Handling Functions ----------
void handleZeroZeroSequence(int antennaNumber) {   // 00 detection function
  // antennaNumber is 0-7 (antenna 1-8)
  
  // Handle 0-0-X sequence where X is antenna number (1-8)
  // Block all antennas above X (X+1 to 8)
  // Note: antennaNumber is index (0-7), antenna position is (antennaNumber + 1)
  int maxAllowedAntenna = antennaNumber + 1; // Convert index to antenna number
  
  // Block antennas from (maxAllowedAntenna) to 7 (indices maxAllowedAntenna to 7)
  for (int i = maxAllowedAntenna; i < 8; i++) {
    blockedAntennas[i] = true;
  }
  
  // Ensure antennas 0 to (maxAllowedAntenna-1) are NOT blocked
  for (int i = 0; i < maxAllowedAntenna; i++) {
    blockedAntennas[i] = false;
  }
  
  // If current TX antenna is blocked, find a new one
  if (blockedAntennas[txRelay]) {
    findNewTxAntenna();
  }
  
  // If current RX antenna is blocked, find a new one
  if (blockedAntennas[rxRelay]) {
    for (int i = 0; i < maxAntennas; i++) {
      if (!blockedAntennas[i]) {
        rxRelay = i;
        break;
      }
    }
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
  
  // Visual feedback: blink blocked antennas
  for (int i = maxAllowedAntenna; i < 8; i++) {
    digitalWrite(txLeds[i], HIGH);
  }
  digitalWrite(blockLed, HIGH);
  delay(1000);
  for (int i = maxAllowedAntenna; i < 8; i++) {
    digitalWrite(txLeds[i], LOW);
  }
  digitalWrite(blockLed, LOW);
  delay(200);
  updateAntennaRelaysAndLEDs();
}

void removeAllRestrictions() {
  // Unblock all antennas
  for (int i = 0; i < 8; i++) {
    blockedAntennas[i] = false;
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
  
  // Visual feedback: blink all antennas to show all are unblocked
  for (int i = 0; i < maxAntennas; i++) {
    digitalWrite(txLeds[i], HIGH);
  }
  digitalWrite(blockLed, LOW);
  delay(1000);
  for (int i = 0; i < maxAntennas; i++) {
    digitalWrite(txLeds[i], LOW);
  }
  delay(200);
  updateAntennaRelaysAndLEDs();
}

void resetZeroSequence() {
  sequenceStartTime = 0;
  zeroPressCount = 0;
  waitingForAntennaInSequence = false;
}

// ---------- Utility Functions ----------
uint8_t packFlags() {
  uint8_t f = 0;
  if (rxScanMode) f |= 0x01;
  if (txScanMode) f |= 0x02;
  if (swapMode)   f |= 0x04;
  return f;
}

void unpackFlags(uint8_t f) {
  rxScanMode = (f & 0x01);
  txScanMode = (f & 0x02);
  swapMode   = (f & 0x04);
}

uint8_t calcPayloadChecksum(const uint8_t *payload, uint8_t len) {
  uint8_t c = 0;
  for (uint8_t i = 0; i < len; i++) c ^= payload[i];
  return c;
}

void loadDefaults() {
  maxAntennas = 8;
  activeRelay = 0;
  rxRelay = 0;
  txRelay = 0;
  rxScanMode = false;
  txScanMode = false;
  swapMode = false;
  
  for (int i = 0; i < 8; i++) blockedAntennas[i] = false;
  
  // Reset toggle states
  for (int i = 0; i < 7; i++) {
    toggleStates[i] = false;
    digitalWrite(toggleRelays[i], LOW);
  }
  
  // Reset sequence detection
  resetZeroSequence();
}

// ---------- EEPROM Functions ----------
const uint8_t EEPROM_PAGE = 32;

uint8_t extEEPROM_readByte(uint16_t addr) {
  Wire.beginTransmission((uint8_t)EEPROM_ADDR);
  Wire.write((uint8_t)(addr >> 8));
  Wire.write((uint8_t)(addr & 0xFF));
  uint8_t err = Wire.endTransmission();
  if (err != 0) return 0xFF;

  Wire.requestFrom((uint8_t)EEPROM_ADDR, (uint8_t)1);
  if (Wire.available()) return Wire.read();
  return 0xFF;
}

bool extEEPROM_waitReady(uint16_t timeoutMs = 200) {
  unsigned long start = millis();
  while (millis() - start < timeoutMs) {
    Wire.beginTransmission((uint8_t)EEPROM_ADDR);
    uint8_t e = Wire.endTransmission();
    if (e == 0) return true;
    delay(5);
  }
  return false;
}

bool extEEPROM_writeChunk(uint16_t addr, const uint8_t *data, uint8_t len) {
  Wire.beginTransmission((uint8_t)EEPROM_ADDR);
  Wire.write((uint8_t)(addr >> 8));
  Wire.write((uint8_t)(addr & 0xFF));
  for (uint8_t i = 0; i < len; i++) Wire.write(data[i]);
  uint8_t err = Wire.endTransmission();
  if (err != 0) {
    for (int r = 0; r < 3; r++) {
      delay(5);
      Wire.beginTransmission((uint8_t)EEPROM_ADDR);
      Wire.write((uint8_t)(addr >> 8));
      Wire.write((uint8_t)(addr & 0xFF));
      for (uint8_t i = 0; i < len; i++) Wire.write(data[i]);
      err = Wire.endTransmission();
      if (err == 0) break;
    }
  }
  extEEPROM_waitReady();
  return (err == 0);
}

bool extEEPROM_writePageSafe(uint16_t addr, const uint8_t *data, uint16_t len) {
  uint16_t cur = addr;
  uint16_t remaining = len;
  const uint8_t *p = data;
  while (remaining) {
    uint8_t pageOffset = cur % EEPROM_PAGE;
    uint8_t canWrite = EEPROM_PAGE - pageOffset;
    uint8_t toWrite = (remaining < canWrite) ? remaining : canWrite;
    if (!extEEPROM_writeChunk(cur, p, toWrite)) return false;
    cur += toWrite;
    p += toWrite;
    remaining -= toWrite;
  }
  return true;
}

void extEEPROM_readBuffer(uint16_t addr, uint8_t *buf, uint16_t len) {
  uint16_t remaining = len;
  uint16_t cur = addr;
  uint8_t *p = buf;
  while (remaining) {
    uint8_t toRead = (remaining > 32) ? 32 : (uint8_t)remaining;
    
    Wire.beginTransmission((uint8_t)EEPROM_ADDR);
    Wire.write((uint8_t)(cur >> 8));
    Wire.write((uint8_t)(cur & 0xFF));
    if (Wire.endTransmission() != 0) {
      for (uint16_t i = 0; i < remaining; i++) p[i] = 0xFF;
      return;
    }
    
    Wire.requestFrom((uint8_t)EEPROM_ADDR, toRead);
    uint8_t i = 0;
    while (Wire.available() && i < toRead) p[i++] = Wire.read();
    
    p += toRead;
    cur += toRead;
    remaining -= toRead;
  }
}

void clearEEPROM() {
  // Iterate through all slots and write 0xFF to the magic byte
  for (uint16_t s = 0; s < SLOT_COUNT; s++) {
    uint32_t base = (uint32_t)EEPROM_START_ADDR + (uint32_t)s * SLOT_SIZE;
    uint8_t currentMagic = extEEPROM_readByte((uint16_t)base);
    if (currentMagic != 0xFF) {
      uint8_t clearByte = 0xFF;
      extEEPROM_writeChunk((uint16_t)base, &clearByte, 1);
    }
  }
  
  // Reset cache
  cachedSlotIdx = 0xFFFF;
  cachedVersion = 0;
  eepromDirty = false;
  
  // Reset sequence detection
  resetZeroSequence();
  
  // Load defaults
  loadDefaults();
  
  // Immediately store default state in slot 0 (version 1)
  scheduleSaveState();
  saveStateToEEPROM();
  
  // Blink all antennas once to indicate EEPROM cleared
  for (int i = 0; i < maxAntennas; i++) {
    digitalWrite(txLeds[i], HIGH);
  }
  digitalWrite(blockLed, HIGH);
  delay(500);
  for (int i = 0; i < maxAntennas; i++) {
    digitalWrite(txLeds[i], LOW);
  }
  digitalWrite(blockLed, LOW);
}

bool readSlot(uint16_t slotIndex, uint8_t *slotBuf) {
  if (slotIndex >= SLOT_COUNT) return false;
  uint32_t base = (uint32_t)EEPROM_START_ADDR + (uint32_t)slotIndex * SLOT_SIZE;
  if (base + SLOT_SIZE - 1 > 8191u) return false;
  extEEPROM_readBuffer((uint16_t)base, slotBuf, SLOT_SIZE);
  return true;
}

bool writeSlotImmediate(uint16_t slotIndex, const uint8_t *slotBuf) {
  if (slotIndex >= SLOT_COUNT) return false;
  uint32_t base = (uint32_t)EEPROM_START_ADDR + (uint32_t)slotIndex * SLOT_SIZE;
  if (base + SLOT_SIZE - 1 > 8191u) return false;
  return extEEPROM_writePageSafe((uint16_t)base, slotBuf, SLOT_SIZE);
}

void loadStateFromEEPROM() {
  Wire.beginTransmission((uint8_t)EEPROM_ADDR);
  uint8_t err = Wire.endTransmission();
  if (err != 0) {
    loadDefaults();
    cachedSlotIdx = 0xFFFF;
    cachedVersion = 0;
    return;
  }

  uint16_t bestVersion = 0;
  bool found = false;
  uint8_t buf[SLOT_SIZE];
  uint16_t bestIdx = 0xFFFF;

  for (uint16_t s = 0; s < SLOT_COUNT; s++) {
    if (!readSlot(s, buf)) continue;
    if (buf[0] != SLOT_MAGIC) continue;
    uint16_t ver = (uint16_t)buf[1] | ((uint16_t)buf[2] << 8);
    if (ver == 0) continue;
    uint8_t checksum = buf[3];
    uint8_t comp = calcPayloadChecksum(&buf[4], PAYLOAD_LEN);
    if (checksum != comp) continue;
    if (!found || ver > bestVersion) {
      bestVersion = ver;
      bestIdx = s;
      memcpy(cachedPayload, &buf[4], PAYLOAD_LEN);
      found = true;
    }
  }

  if (!found) {
    loadDefaults();
    cachedSlotIdx = 0xFFFF;
    cachedVersion = 0;
    return;
  }

  uint8_t payloadIdx = 0;
  uint8_t ee_maxAntennas = cachedPayload[payloadIdx++];
  uint8_t ee_activeRelay = cachedPayload[payloadIdx++];
  uint8_t ee_rxRelay = cachedPayload[payloadIdx++];
  uint8_t ee_txRelay = cachedPayload[payloadIdx++];
  uint8_t ee_flags = cachedPayload[payloadIdx++];
  bool ee_blockedAnt[8];
  for (uint8_t i = 0; i < 8; i++) ee_blockedAnt[i] = (cachedPayload[payloadIdx++] == 1);

  bool dataValid = (ee_maxAntennas >= 1 && ee_maxAntennas <= 8 &&
                    ee_activeRelay < 8 && ee_rxRelay < 8 && ee_txRelay < 8);

  if (!dataValid) {
    loadDefaults();
    cachedSlotIdx = 0xFFFF;
    cachedVersion = 0;
    return;
  }

  maxAntennas = ee_maxAntennas;
  activeRelay = ee_activeRelay;
  rxRelay = ee_rxRelay;
  txRelay = ee_txRelay;
  unpackFlags(ee_flags);
  for (uint8_t i = 0; i < 8; i++) blockedAntennas[i] = ee_blockedAnt[i];

  cachedVersion = bestVersion;
  cachedSlotIdx = bestIdx;
}

void scheduleSaveState() {
  eepromDirty = true;
  eepromLastDirtyMs = millis();
}

void packPayload(uint8_t *outPayload) {
  uint8_t idx = 0;
  outPayload[idx++] = (uint8_t)maxAntennas;
  outPayload[idx++] = (uint8_t)activeRelay;
  outPayload[idx++] = (uint8_t)rxRelay;
  outPayload[idx++] = (uint8_t)txRelay;
  outPayload[idx++] = packFlags();
  for (uint8_t i = 0; i < 8; i++) outPayload[idx++] = blockedAntennas[i] ? 1 : 0;
}

void saveStateToEEPROM() {
  uint8_t payload[PAYLOAD_LEN];
  packPayload(payload);

  uint8_t checksum = calcPayloadChecksum(payload, PAYLOAD_LEN);

  if (cachedSlotIdx != 0xFFFF) {
    bool same = true;
    for (uint8_t i = 0; i < PAYLOAD_LEN; i++) {
      if (cachedPayload[i] != payload[i]) { same = false; break; }
    }
    if (same) {
      eepromDirty = false;
      return;
    }
  }

  uint16_t nextSlot = 0;
  uint16_t currentBestVersion = cachedVersion;
  if (cachedSlotIdx != 0xFFFF) {
    nextSlot = (cachedSlotIdx + 1) % SLOT_COUNT;
  } else {
    bool found = false;
    uint8_t buf[SLOT_SIZE];
    for (uint16_t s = 0; s < SLOT_COUNT; s++) {
      if (!readSlot(s, buf)) continue;
      if (buf[0] != SLOT_MAGIC) { nextSlot = s; found = true; break; }
    }
    if (!found) nextSlot = 0;
  }

  uint8_t slotBuf[SLOT_SIZE];
  memset(slotBuf, 0xFF, SLOT_SIZE);
  slotBuf[0] = SLOT_MAGIC;
  uint16_t newVersion = (uint16_t)(currentBestVersion + 1);
  if (newVersion == 0) newVersion = 1;
  slotBuf[1] = (uint8_t)(newVersion & 0xFF);
  slotBuf[2] = (uint8_t)((newVersion >> 8) & 0xFF);
  slotBuf[3] = checksum;
  for (uint8_t i = 0; i < PAYLOAD_LEN; i++) slotBuf[4 + i] = payload[i];

  bool ok = writeSlotImmediate(nextSlot, slotBuf);
  if (ok) {
    cachedSlotIdx = nextSlot;
    cachedVersion = newVersion;
    memcpy(cachedPayload, payload, PAYLOAD_LEN);
    eepromDirty = false;
  } else {
    eepromDirty = true;
  }
}

// ---------- Button Handling ----------
void checkAntennaButtons() {
  unsigned long currentTime = millis();
  
  for (int i = 0; i < 8; i++) {
    bool reading = digitalRead(antennaButtons[i]);
    
    // Simple debounce with state change detection
    if (reading != lastAntennaButtonStates[i]) {
      delay(50); // Simple debounce
      reading = digitalRead(antennaButtons[i]);
      
      if (reading == LOW && lastAntennaButtonStates[i] == HIGH) {
        // Button pressed (falling edge)
        
        // Check if we're in 0-0-X sequence mode
        if (waitingForAntennaInSequence) {
          // We're waiting for an antenna button press after 0-0
          if (currentTime - sequenceStartTime <= SEQUENCE_TIMEOUT) {
            // Process the sequence - pass antenna index (0-7)
            handleZeroZeroSequence(i);
            resetZeroSequence();
            return; // Skip normal processing
          } else {
            // Sequence timed out
            resetZeroSequence();
          }
        }
        
        // Normal button processing
        if (i < maxAntennas) {
          if (rxScanMode) {
            // In RX scan mode, change RX antenna
            setRxAntenna(i);
          } else if (txScanMode) {
            // In TX scan mode, change TX antenna
            setTxAntenna(i);
          } else {
            // Normal mode - change both RX and TX
            setActiveAntenna(i);
          }
        }
        // Record press start for long press detection
        antennaButtonPressStart[i] = currentTime;
        longPressHandled[i] = false;
      } else if (reading == HIGH && lastAntennaButtonStates[i] == LOW) {
        // Button released - just track the state
        longPressHandled[i] = false;
      }
      
      lastAntennaButtonStates[i] = reading;
    }
    
    // Check for long press while button is still pressed
    if (reading == LOW && !longPressHandled[i]) {
      unsigned long pressDuration = currentTime - antennaButtonPressStart[i];
      
      if (pressDuration >= 2000) {  // Long press (2+ seconds)
        toggleAntennaBlock(i);
        longPressHandled[i] = true;  // Mark as handled to prevent repeated toggles
        startBlink(i);  // Immediate feedback
      }
    }
  }
}

void checkModeButtons() {
  bool rxTxScanReading = digitalRead(rxTxScanButton);
  bool swapReading = digitalRead(swapButton);
  bool zeroReading = digitalRead(zeroButton);
  unsigned long currentTime = millis();
  
  // --- 1. RX/TX SCAN BUTTON LOGIC ---
  if (rxTxScanReading != lastRxTxScanState) {
    delay(50);
    rxTxScanReading = digitalRead(rxTxScanButton);
    if (rxTxScanReading == LOW && lastRxTxScanState == HIGH) {
      rxTxScanPressTime = currentTime;
    } else if (rxTxScanReading == HIGH && lastRxTxScanState == LOW) {
      unsigned long pressDuration = currentTime - rxTxScanPressTime;
      if (pressDuration < 3000) {
        if (waitingForSecondPress && (currentTime - lastRxTxScanPress < DOUBLE_PRESS_MAX_INTERVAL)) {
          toggleTxScanMode();
          waitingForSecondPress = false;
        } else {
          waitingForSecondPress = true;
          lastRxTxScanPress = currentTime;
        }
      }
    }
    lastRxTxScanState = rxTxScanReading;
  }
  if (waitingForSecondPress && (currentTime - lastRxTxScanPress > DOUBLE_PRESS_MAX_INTERVAL)) {
    toggleRxScanMode();
    waitingForSecondPress = false;
  }

  // --- 2. SWAP BUTTON LOGIC ---
  if (swapReading != lastSwapState) {
    delay(50);
    swapReading = digitalRead(swapButton);
    if (swapReading == LOW && lastSwapState == HIGH) {
      swapPressTime = currentTime;
    } else if (swapReading == HIGH && lastSwapState == LOW) {
      if (currentTime - swapPressTime < 3000) {
        toggleSwapMode();
      }
    }
    lastSwapState = swapReading;
  }

  // --- 3. ZERO BUTTON (0) LOGIC ---
  if (zeroReading != lastZeroState) {
    delay(50);
    zeroReading = digitalRead(zeroButton);
    
    if (zeroReading == LOW && lastZeroState == HIGH) {
      // Button Pressed
      zeroPressTime = currentTime;
      zeroButtonPressed = true;
      zeroLongPressHandled = false;

      // Check if previous sequence timed out (3 seconds)
      if (zeroPressCount > 0 && (currentTime - sequenceStartTime > SEQUENCE_TIMEOUT)) {
        resetZeroSequence();
      }

      // Start or continue sequence
      if (zeroPressCount == 0) {
        sequenceStartTime = currentTime;
      }
      zeroPressCount++;

      // Visual feedback: Short blink on Block LED for every '0' press
      digitalWrite(blockLed, HIGH);
      delay(100);
      digitalWrite(blockLed, LOW);

      // --- LOGIC FOR 0-0-0 (Triple Zero) ---
      if (zeroPressCount == 3) {
        removeAllRestrictions();
        resetZeroSequence();
      } 
      // --- LOGIC FOR 0-0 (Double Zero) ---
      // Ready the system to accept an antenna button press (1-8)
      else if (zeroPressCount == 2) {
        waitingForAntennaInSequence = true;
      }
    } 
    else if (zeroReading == HIGH && lastZeroState == LOW) {
      // Button Released
      zeroButtonPressed = false;
    }
    lastZeroState = zeroReading;
  }

  // --- 4. ZERO BUTTON LONG PRESS (3s Erase EEPROM) ---
  if (zeroButtonPressed && !zeroLongPressHandled) {
    if (currentTime - zeroPressTime >= 3000) {
      clearEEPROM();
      resetZeroSequence(); // Clear any partial sequence
      zeroLongPressHandled = true;
    }
  }

  // --- 5. GLOBAL SEQUENCE TIMEOUT ---
  // If the user waits too long between 0-0 and the antenna press, reset everything.
  if (waitingForAntennaInSequence && (currentTime - sequenceStartTime > SEQUENCE_TIMEOUT)) {
    resetZeroSequence();
  }
}

void checkToggleButtons() {
  for (int i = 0; i < 7; i++) {
    bool reading = digitalRead(toggleButtons[i]);
    
    if (reading != lastToggleStates[i]) {
      delay(50); // Simple debounce
      reading = digitalRead(toggleButtons[i]);
      
      if (reading == LOW && lastToggleStates[i] == HIGH) {
        // Toggle the relay and LED state
        toggleStates[i] = !toggleStates[i];
        digitalWrite(toggleRelays[i], toggleStates[i] ? HIGH : LOW);
      }
      lastToggleStates[i] = reading;
    }
  }
}

// ---------- Core Antenna Functions ----------
void toggleAntennaBlock(int antennaNumber) {
  if (antennaNumber < 0 || antennaNumber >= 8) return;
  
  blockedAntennas[antennaNumber] = !blockedAntennas[antennaNumber];
  
  // If we just blocked the TX antenna, find a new one
  if (blockedAntennas[antennaNumber] && txRelay == antennaNumber) {
    findNewTxAntenna();
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void findNewTxAntenna() {
  // Find first unblocked antenna
  for (int i = 0; i < maxAntennas; i++) {
    if (!blockedAntennas[i]) {
      txRelay = i;
      scheduleSaveState();
      updateAntennaRelaysAndLEDs();
      return;
    }
  }
  
  // If all are blocked, unblock antenna 0
  blockedAntennas[0] = false;
  txRelay = 0;
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

bool canUseAsTx(int antennaNumber) {
  if (antennaNumber < 0 || antennaNumber >= 8) return false;
  return !blockedAntennas[antennaNumber];
}

void toggleRxScanMode() {
  if (millis() - bootTime < 3000) return;
  
  if (txScanMode) {
    txScanMode = false;
    digitalWrite(txModeLed, LOW);
  }
  
  rxScanMode = !rxScanMode;
  digitalWrite(rxModeLed, rxScanMode ? HIGH : LOW);
  
  if (rxScanMode) {
    // Entering RX scan mode
    rxRelay = activeRelay;
    // Keep TX antenna as it was (for TX LED indication)
    // Don't change txRelay
    
    swapMode = false;
    digitalWrite(swapLed, LOW);
  } else {
    // Exiting RX scan mode
    activeRelay = rxRelay;
    txRelay = rxRelay;  // Both RX and TX on same antenna
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void toggleTxScanMode() {
  if (millis() - bootTime < 3000) return;
  
  if (rxScanMode) {
    rxScanMode = false;
    digitalWrite(rxModeLed, LOW);
  }
  
  txScanMode = !txScanMode;
  digitalWrite(txModeLed, txScanMode ? HIGH : LOW);
  
  if (txScanMode) {
    // Entering TX scan mode
    txRelay = activeRelay;
    // Keep RX antenna as it was
    // Don't change rxRelay
    
    // Ensure TX antenna is not blocked
    if (blockedAntennas[txRelay]) {
      findNewTxAntenna();
    }
    
    swapMode = false;
    digitalWrite(swapLed, LOW);
  } else {
    // Exiting TX scan mode
    activeRelay = txRelay;
    rxRelay = txRelay;  // Both RX and TX on same antenna
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void toggleSwapMode() {
  if (millis() - bootTime < 3000) return;
  if (!(rxScanMode || txScanMode)) return;
  
  // Can only swap if TX antenna is not blocked
  if (!canUseAsTx(rxRelay)) {
    startBlink(rxRelay);
    return;
  }
  
  // Swap RX and TX antennas
  int temp = rxRelay;
  rxRelay = txRelay;
  txRelay = temp;
  
  swapMode = !swapMode;
  digitalWrite(swapLed, swapMode ? HIGH : LOW);
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void setRxAntenna(int antennaNumber) {
  if (millis() - bootTime < 3000) return;
  if (antennaNumber < 0 || antennaNumber >= maxAntennas) return;
  
  rxRelay = antennaNumber;
  
  // If not in scan mode, also set TX antenna
  if (!rxScanMode && !txScanMode) {
    txRelay = antennaNumber;
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void setTxAntenna(int antennaNumber) {
  if (millis() - bootTime < 3000) return;
  if (antennaNumber < 0 || antennaNumber >= maxAntennas) return;
  
  if (blockedAntennas[antennaNumber]) {
    startBlink(antennaNumber);
    return;
  }
  
  txRelay = antennaNumber;
  
  // If not in scan mode, also set RX antenna
  if (!rxScanMode && !txScanMode) {
    rxRelay = antennaNumber;
  }
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

void setActiveAntenna(int antennaNumber) {
  if (millis() - bootTime < 3000) return;
  if (antennaNumber < 0 || antennaNumber >= maxAntennas) return;
  
  if (blockedAntennas[antennaNumber]) {
    startBlink(antennaNumber);
    return;
  }
  
  activeRelay = antennaNumber;
  rxRelay = antennaNumber;
  txRelay = antennaNumber;
  
  scheduleSaveState();
  updateAntennaRelaysAndLEDs();
}

// ---------- Update Functions ----------
void updateAntennaRelaysAndLEDs() {
  // Use global pttActive (includes 300ms delay)
  
  // Determine which relay to activate
  int relayToActivate;
  
  if (pttActive) {
    // Transmitting - use TX antenna
    // Make sure TX antenna is not blocked
    if (!canUseAsTx(txRelay)) {
      // Find an unblocked antenna
      for (int i = 0; i < maxAntennas; i++) {
        if (canUseAsTx(i)) {
          relayToActivate = i;
          break;
        }
      }
    } else {
      relayToActivate = txRelay;
    }
  } else {
    // Receiving - use RX antenna
    relayToActivate = rxRelay;
  }
  
  // Turn off all antenna relays first (only one can be active)
  for (int i = 0; i < 8; i++) {
    digitalWrite(antennaRelays[i], LOW);
  }
  
  // Turn on the selected relay (and its parallel LED)
  if (relayToActivate >= 0 && relayToActivate < 8) {
    digitalWrite(antennaRelays[relayToActivate], HIGH);
  }
  
  // Update TX indicator LEDs (show which antenna is selected for TX)
  for (int i = 0; i < 8; i++) {
    // Don't update if we're blinking this antenna
    if (isBlinking && antennaToBlink == i) continue;
    
    if (i == txRelay && canUseAsTx(i)) {
      digitalWrite(txLeds[i], HIGH);
    } else {
      digitalWrite(txLeds[i], LOW);
    }
  }
}

void updateStatusLEDs() {
  digitalWrite(rxModeLed, rxScanMode ? HIGH : LOW);
  digitalWrite(txModeLed, txScanMode ? HIGH : LOW);
  digitalWrite(swapLed, swapMode ? HIGH : LOW);
  // Block LED is handled in blink function
}

// ---------- Blink Functions ----------
void startBlink(int antenna) {
  if (antenna < 0 || antenna >= 8) return;
  isBlinking = true;
  blinkStep = 0;
  antennaToBlink = antenna;
  lastBlinkTime = millis();
}

void handleBlink() {
  if (!isBlinking) return;
  
  unsigned long now = millis();
  bool isOnStep = (blinkStep % 2 == 0);
  int interval = isOnStep ? BLINK_ON_TIME : BLINK_OFF_TIME;
  
  if (now - lastBlinkTime < interval) return;
  lastBlinkTime = now;
  
  if (isOnStep) {
    digitalWrite(txLeds[antennaToBlink], HIGH);
    digitalWrite(blockLed, HIGH);
  } else {
    digitalWrite(txLeds[antennaToBlink], LOW);
    digitalWrite(blockLed, LOW);
  }
  
  blinkStep++;
  if (blinkStep >= BLINK_COUNT * 2) {
    isBlinking = false;
    antennaToBlink = -1;
    // Restore normal TX LED state
    updateAntennaRelaysAndLEDs();
  }
}

// ---------- PTT Check ----------
void updatePttState() {
  bool physicalPtt = (digitalRead(pttPin) == HIGH);
  static bool lastPhysicalPtt = LOW;

  if (physicalPtt) {
    pttInDelay = false;
    pttActive = true;
  } else {
    if (lastPhysicalPtt == HIGH) {
      pttReleaseTime = millis();
      pttInDelay = true;
    }

    if (pttInDelay) {
      if (millis() - pttReleaseTime < PTT_DELAY_MS) {
        pttActive = true; 
      } else {
        pttInDelay = false;
        pttActive = false;
      }
    } else {
      pttActive = false;
    }
  }
  lastPhysicalPtt = physicalPtt;
}

void checkPTT() {
  static bool lastPttState = LOW;
  bool currentPttState = pttActive;  // Use global state with delay
  
  if (currentPttState != lastPttState) {
    lastPttState = currentPttState;
    
    if (currentPttState == HIGH) {
      // PTT pressed - transmitting
      // Check if TX antenna is blocked
      if (!canUseAsTx(txRelay)) {
        startBlink(txRelay);
        findNewTxAntenna();
      }
    }
    
    updateAntennaRelaysAndLEDs();
  }
}

// ---------- Setup ----------
void setup() {
  bootTime = millis();
  
  // Initialize I2C for EEPROM
  Wire.begin();
  
  // Setup antenna button pins
  for (int i = 0; i < 8; i++) {
    pinMode(antennaButtons[i], INPUT_PULLUP);
  }
  
  // Setup antenna relay pins (with parallel RX LEDs)
  for (int i = 0; i < 8; i++) {
    pinMode(antennaRelays[i], OUTPUT);
    digitalWrite(antennaRelays[i], LOW);
  }
  
  // Setup TX indicator LED pins
  for (int i = 0; i < 8; i++) {
    pinMode(txLeds[i], OUTPUT);
    digitalWrite(txLeds[i], LOW);
  }
  
  // Setup toggle button pins
  for (int i = 0; i < 7; i++) {
    pinMode(toggleButtons[i], INPUT_PULLUP);
  }
  
  // Setup toggle relay pins (with parallel LEDs)
  for (int i = 0; i < 7; i++) {
    pinMode(toggleRelays[i], OUTPUT);
    digitalWrite(toggleRelays[i], LOW);
  }
  
  // Setup mode buttons
  pinMode(rxTxScanButton, INPUT_PULLUP);
  pinMode(swapButton, INPUT_PULLUP);
  pinMode(zeroButton, INPUT_PULLUP);
  
  // Setup status LEDs
  pinMode(rxModeLed, OUTPUT);
  pinMode(txModeLed, OUTPUT);
  pinMode(swapLed, OUTPUT);
  pinMode(blockLed, OUTPUT);
  
  // Setup PTT
  pinMode(pttPin, INPUT);
  
  // Load state from EEPROM
  delay(100);
  loadStateFromEEPROM();
  
  // Initialize all outputs
  updateAntennaRelaysAndLEDs();
  updateStatusLEDs();
}

// ---------- Main Loop ----------
void loop() {
  updatePttState();
  checkAntennaButtons();
  checkModeButtons();
  checkToggleButtons();
  checkPTT();
  handleBlink();
  
  // Commit to EEPROM if needed
  if (eepromDirty && (millis() - eepromLastDirtyMs >= EEPROM_COMMIT_DELAY)) {
    saveStateToEEPROM();
  }
}