// ROFLS+ Arena Controller

#include <WiFi.h>
#include <esp_now.h> // automatically installed for ESP32 boards, I think?
#include <Preferences.h> // automatically installed for ESP32 boards
#include <FastLED.h> // https://fastled.io/
#include <avdweb_Switch.h> // https://github.com/avdwebLibraries/avdweb_Switch
#include <CountDown.h> // https://github.com/RobTillaart/CountDown
#include <StopWatch.h> // https://github.com/RobTillaart/StopWatch_RT

// Hardware connections
// Buttons:
#define START_BTN_PIN 1
#define PAUSE_BTN_PIN 3
#define PIT_BTN_PIN 5
#define RESET_BTN_PIN 7
// Switches:
#define RUMBLE_SWITCH_PIN 9
#define PIT_ENABLE_SWITCH_PIN 11
#define LIGHT_SWITCH_PIN 12
#define TESTmode_SWITCH_PIN 4
// LEDs:
#define LIGHT_STATUS_LED 10
#define AUTOPIT_STATUS_LED 13
#define MODE_STATUS_LED 14
// Relays:
#define PIT_RELEASE_PIN 37
#define LIGHT_PIN 39
#define UNUSED_RELAY3_PIN 35
#define UNUSED_RELAY4_PIN 33

const byte relayOnState = LOW;
const byte relayOffState = HIGH;

// define buttons and switches
//
// constructor Switch
// Parameters: byte _pin, byte PinMode = 5, bool polarity = 0, unsigned long debouncePeriod = 50, unsigned long longPressPeriod = 300, unsigned long doubleClickPeriod = 250, unsigned long deglitchPeriod = 10
Switch buttonSTART = Switch(START_BTN_PIN, INPUT_PULLUP, LOW, 50, 1000);
Switch buttonPAUSE = Switch(PAUSE_BTN_PIN);
Switch buttonPIT = Switch(PIT_BTN_PIN, INPUT_PULLUP, LOW, 50, 1000);
Switch buttonRESET = Switch(RESET_BTN_PIN, INPUT_PULLUP, LOW, 50, 1000);
Switch switchRUMBLE = Switch(RUMBLE_SWITCH_PIN);
Switch switchPIT = Switch(PIT_ENABLE_SWITCH_PIN);
Switch switchLIGHT = Switch(LIGHT_SWITCH_PIN);
Switch switchTESTmode = Switch(TESTmode_SWITCH_PIN);

bool buttonSTARTvar = false;
bool buttonSTARTforced = false;
bool buttonPAUSEvar = false;
bool buttonPITvar = false;
bool buttonPIThold = false;
bool buttonRESETvar = false;
bool buttonREDTEAMvar = false;
bool buttonREDTEAMtapout = false;
bool buttonBLUETEAMvar = false;
bool buttonBLUETEAMtapout = false;
unsigned long PITopenTimestamp = 0;
bool PITreleased = false;

const long PITopenTime = 500; // activate solenoid for 500ms
const int countdownTIME = 180; // countdown timer length in seconds, actual countdown for the fight
const int countdownToFightTIME = 3; // countdown timer length in seconds, "ready" countdown
const int PITreleaseTime = 90; // automatic pit release time in seconds until end of countdown
bool countdownPAUSED = false;
CountDown FightCountDown(CountDown::SECONDS);
CountDown ReadyCountDown(CountDown::SECONDS);

// Rumble stopwatch
StopWatch rumbleTIME(StopWatch::SECONDS);

int CLOCK_LED_BRIGHTNESS = 32; // 64 is okay

int BLINK_COUNTER_REDTEAM = 0;
int BLINK_COUNTER_BLUETEAM = 0;
int BLINK_INTERVAL = 200;


bool ARENA_READY = false;
bool REDTEAM_READY = false;
bool BLUETEAM_READY = false;

//------------------------------------------------------------------------------------
// ESP-NOW config
// send config, Clock:
uint8_t broadcastAddressClock[] = {0x48, 0x27, 0xE2, 0x5D, 0xB6, 0x84};
// struct for clock data
typedef struct struct_message_Clock {
  int sendMinutes;
  int sendSeconds;
  int sendREDchannel;
  int sendGREENchannel;
  int sendBLUEchannel;
  int sendBrightness;
} struct_message_Clock;


struct_message_Clock sendClockDATA;

// send config, pilot buttons:
uint8_t broadcastAddressREDTEAMbutton[] = {0x84, 0xFC, 0xE6, 0xC7, 0x23, 0x14};
uint8_t broadcastAddressBLUETEAMbutton[] = {0x84, 0xFC, 0xE6, 0xC7, 0x1A, 0x02};

// Structure for sending data
typedef struct struct_message_send {
  bool TEAMLED; // LED state
} struct_message_send;

// Create struct_message instances for sending to both receivers
struct_message_send sendToREDTEAMbutton;
struct_message_send sendToBLUETEAMbutton;

// ESP-Now stuff for the Pit controller
uint8_t broadcastAddressPitController[] = {0x84, 0xFC, 0xE6, 0xC7, 0x19, 0xDE};
// Structure for sending data
typedef struct struct_message_pit {
  bool PIT; // LED state
} struct_message_pit;
struct_message_pit sendToPitController;

esp_now_peer_info_t peerInfo;

// callback when data is sent

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}
//------------------------------------------------------------------------------------
// receive config
// Structure example to send data
// Must match the receiver structure
typedef struct struct_message_receive {
  int boardID;
  bool buttonSTART;
  bool buttonSTARTforced;
  bool buttonPAUSE;
  bool buttonPIT;
  bool buttonPIThold;
  bool buttonRESET;
  bool buttonREDTEAM;
  bool buttonREDTEAMtapout;
  bool buttonBLUETEAM;
  bool buttonBLUETEAMtapout;
} struct_message_receive;

// Create a struct_message called receiveDATA
struct_message_receive receiveDATA;

// callback function that will be executed when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&receiveDATA, incomingData, sizeof(receiveDATA));
  // only fill the data to the right vars
  switch (receiveDATA.boardID) {
    case 0:
      // referee remote
      buttonSTARTvar = receiveDATA.buttonSTART;
      buttonSTARTforced = receiveDATA.buttonSTARTforced;
      buttonPAUSEvar = receiveDATA.buttonPAUSE;
      buttonPITvar = receiveDATA.buttonPIT;
      buttonPIThold = receiveDATA.buttonPIThold;
      buttonRESETvar = receiveDATA.buttonRESET;
      break;
    case 1:
      // RED team button
      buttonREDTEAMvar = receiveDATA.buttonREDTEAM;
      buttonREDTEAMtapout = receiveDATA.buttonREDTEAMtapout;
      break;
    case 2:
      // BLUE team button
      buttonBLUETEAMvar = receiveDATA.buttonBLUETEAM;
      buttonBLUETEAMtapout = receiveDATA.buttonBLUETEAMtapout;
      break;
  }
}

// send data to clock:
void sendTimeDisplay(int MINUTES, int SECONDS, int RED, int GREEN, int BLUE, int BRIGHTNESS) {
  // only send data if there was a change
  if ((sendClockDATA.sendMinutes != MINUTES) || (sendClockDATA.sendSeconds != SECONDS) || (sendClockDATA.sendREDchannel != RED) || (sendClockDATA.sendGREENchannel != GREEN) || (sendClockDATA.sendBLUEchannel != BLUE) || (sendClockDATA.sendBrightness != BRIGHTNESS)) {
    sendClockDATA.sendMinutes = MINUTES;
    sendClockDATA.sendSeconds = SECONDS;
    sendClockDATA.sendREDchannel = RED;
    sendClockDATA.sendGREENchannel = GREEN;
    sendClockDATA.sendBLUEchannel = BLUE;
    sendClockDATA.sendBrightness = BRIGHTNESS;
    // actually send it
    esp_err_t result = esp_now_send(broadcastAddressClock, (uint8_t *) &sendClockDATA, sizeof(sendClockDATA));
  }
}


//------------------------------------------------------------------------------------

void setup() {
  Serial.begin(115200);
  // set relay outputs:
  pinMode(PIT_RELEASE_PIN, OUTPUT);
  digitalWrite(PIT_RELEASE_PIN, relayOffState);
  pinMode(LIGHT_PIN, OUTPUT);
  digitalWrite(LIGHT_PIN, LOW); // have it by default on, to prevent flickering, needs a better fix tho
  pinMode(UNUSED_RELAY3_PIN, OUTPUT);
  digitalWrite(UNUSED_RELAY3_PIN, relayOffState);
  pinMode(UNUSED_RELAY4_PIN, OUTPUT);
  digitalWrite(UNUSED_RELAY4_PIN, relayOffState);
  // set status LED outputs:
  pinMode(LIGHT_STATUS_LED, OUTPUT);
  digitalWrite(LIGHT_PIN, HIGH); // have it by default on, to prevent flickering, needs a better fix tho
  pinMode(AUTOPIT_STATUS_LED, OUTPUT);
  digitalWrite(AUTOPIT_STATUS_LED, LOW);
  pinMode(MODE_STATUS_LED, OUTPUT);
  digitalWrite(MODE_STATUS_LED, LOW);

  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  //------------------------------------------------------------------------------------
  // ESP Now send part:
  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Transmitted packet
  esp_now_register_send_cb(OnDataSent);

  // Register clock peer
  memcpy(peerInfo.peer_addr, broadcastAddressClock, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  // Add peer        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }

  esp_err_t result = esp_now_send(broadcastAddressClock, (uint8_t *) &sendClockDATA, sizeof(sendClockDATA));

  // Register Red Team Button peer
  memcpy(peerInfo.peer_addr, broadcastAddressREDTEAMbutton, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add receiver 1");
    return;
  }

  // Register Blue Team Button peer
  memcpy(peerInfo.peer_addr, broadcastAddressBLUETEAMbutton, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add receiver 2");
    return;
  }

  // Register Pit controller peer
  memcpy(peerInfo.peer_addr, broadcastAddressPitController, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add receiver 3");
    return;
  }

  // Initialize both Team button LED states and pit controller state
  sendToREDTEAMbutton.TEAMLED = false;
  sendToBLUETEAMbutton.TEAMLED = false;
  sendToPitController.PIT = false;
  // reset remote button LEDs and pit
  esp_now_send(broadcastAddressREDTEAMbutton, (uint8_t *)&sendToREDTEAMbutton, sizeof(sendToREDTEAMbutton));
  esp_now_send(broadcastAddressBLUETEAMbutton, (uint8_t *)&sendToBLUETEAMbutton, sizeof(sendToBLUETEAMbutton));
  esp_now_send(broadcastAddressPitController, (uint8_t *)&sendToPitController, sizeof(sendToPitController));

  //------------------------------------------------------------------------------------
  // ESP Now receive part:  
  // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info
  esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv));
}

void loop() {
  // poll all the switch/button inputs
  pollInput();
  // deactivate solenoids if needed
  checkPIT();
  // Arena Light
  arenaLIGHT();
  // update status LEDs
  statusLEDs();
  // update Teambutton LEDs
  updateTEAMLEDs();

  // start button logic
  if (buttonSTARTvar) {
    buttonSTARTvar = false;
    // start logic
    if (!FightCountDown.isRunning() && !rumbleTIME.isRunning() && countdownPAUSED == false) {
      if (switchRUMBLE.on()) {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        //rumbleTIME.start();
        ARENA_READY = true;
        ReadyCountDown.start(countdownToFightTIME);
      } else if (buttonREDTEAMvar && buttonBLUETEAMvar && !switchRUMBLE.on()) {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        //FightCountDown.start(countdownTIME);
        ARENA_READY = true;
        ReadyCountDown.start(countdownToFightTIME);
      }
    }
    else {
      // resume logic
      if (buttonREDTEAMvar && buttonBLUETEAMvar && !switchRUMBLE.on()) {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        countdownPAUSED = false;
        FightCountDown.resume();
      } else if (switchRUMBLE.on()) {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        countdownPAUSED = false;
        rumbleTIME.start();
      }
    }
  }
  // forced start of countdown
  if (buttonSTARTforced) {
    buttonSTARTforced = false;
    if (!FightCountDown.isRunning() && !rumbleTIME.isRunning() && countdownPAUSED == false) {
      buttonREDTEAMvar = false;
      buttonBLUETEAMvar = false;
      if (!switchRUMBLE.on()) {
        //FightCountDown.start(countdownTIME);
        ARENA_READY = true;
        ReadyCountDown.start(countdownToFightTIME);
      } else if (switchRUMBLE.on()) {
        //rumbleTIME.start();
        ARENA_READY = true;
        ReadyCountDown.start(countdownToFightTIME);
      }
    }
    else {
      // forced resume logic
      buttonREDTEAMvar = false;
      buttonBLUETEAMvar = false;
      countdownPAUSED = false;
      if (!switchRUMBLE.on()) {
        FightCountDown.resume();
      } else if (switchRUMBLE.on()) {
        rumbleTIME.start();
      }
    }
  }
  // pause button logic
  if (buttonPAUSEvar) {
    buttonPAUSEvar = false;
    if (FightCountDown.isRunning()) {
      countdownPAUSED = true;
      FightCountDown.stop();
    }
    if (rumbleTIME.isRunning()) {
      countdownPAUSED = true;
      rumbleTIME.stop();
    }
  }
  // pit button logic
  if (buttonPITvar) {
    buttonPITvar = false;
    buttonPIThold = false;
    openPITmanually();
  }
  // automatic pit release
  if (FightCountDown.remaining() <= PITreleaseTime && FightCountDown.remaining() != 0 && !switchRUMBLE.on() && switchPIT.on() && buttonPIThold == false) {
    openPIT();
  }
  if (rumbleTIME.elapsed() >= PITreleaseTime && switchRUMBLE.on() && switchPIT.on() && buttonPIThold == false) {
    openPIT();
  }

  // tap out logic
  if (buttonREDTEAMtapout && !switchRUMBLE.on()) {
    countdownPAUSED = true;
    FightCountDown.stop();
    REDTEAM_LED(true);
    sendTimeDisplay(99, 0, 255, 0, 0, CLOCK_LED_BRIGHTNESS);
    delay(1500);
    sendTimeDisplay(0, 99, 255, 0, 0, CLOCK_LED_BRIGHTNESS);
    delay(1500);
    sendTimeDisplay((FightCountDown.remaining()/60%60), (FightCountDown.remaining()%60), 255, 0, 0, CLOCK_LED_BRIGHTNESS);
    delay(7000);
  }
  if (buttonBLUETEAMtapout && !switchRUMBLE.on()) {
    countdownPAUSED = true;
    FightCountDown.stop();
    BLUETEAM_LED(true);
    sendTimeDisplay(99, 0, 0, 0, 255, CLOCK_LED_BRIGHTNESS);
    delay(1500);
    sendTimeDisplay(0, 99, 0, 0, 255, CLOCK_LED_BRIGHTNESS);
    delay(1500);
    sendTimeDisplay((FightCountDown.remaining()/60%60), (FightCountDown.remaining()%60), 0, 0, 255, CLOCK_LED_BRIGHTNESS);
    delay(1500);
  }

  // reset button logic
  if (buttonRESETvar) {
    buttonRESETvar = false;
    PITreleased = false;
    ESP.restart();
  }

  // Ready Countdown to Fight Countdown transition
  if (ReadyCountDown.remaining() == 0 && ARENA_READY) {
    ARENA_READY = false;
    REDTEAM_LED(false);
    BLUETEAM_LED(false);
    if (!switchRUMBLE.on()) {
      FightCountDown.start(countdownTIME);
    } else if (switchRUMBLE.on()) {
      rumbleTIME.start();
    }
  }

  // If either team has tapped out, skip updating the display.
  if (buttonREDTEAMtapout || buttonBLUETEAMtapout) {
      return;
  }

  // Display the Ready CountDown timer if it is running.
  if (ReadyCountDown.isRunning()) {
      sendTimeDisplay((ReadyCountDown.remaining()/60%60), (ReadyCountDown.remaining()%60), 255, 165, 0, CLOCK_LED_BRIGHTNESS);
      return;
  }

  // When the RUMBLE switch is active, always show the rumble timer.
  if (switchRUMBLE.on()) {
      sendTimeDisplay((rumbleTIME.elapsed()/60%60), (rumbleTIME.elapsed()%60), 0, 255, 255, CLOCK_LED_BRIGHTNESS);
      return;
  }

  // If the fight countdown isn't running and isn't paused, determine which countdown color to use.
  if (!FightCountDown.isRunning() && !countdownPAUSED) {
    // Choose green if both buttons are active; otherwise, choose magenta.
    if (buttonREDTEAMvar && buttonBLUETEAMvar) {
      sendTimeDisplay((countdownTIME/60%60), (countdownTIME%60), 0, 255, 0, CLOCK_LED_BRIGHTNESS);
    } else {
      sendTimeDisplay((countdownTIME/60%60), (countdownTIME%60), 255, 0, 255, CLOCK_LED_BRIGHTNESS);
    }
  } else {
    // Otherwise, show the fight countdown timer.
    sendTimeDisplay((FightCountDown.remaining()/60%60), (FightCountDown.remaining()%60), 0, 255, 0, CLOCK_LED_BRIGHTNESS);
  }
}