// 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
#define START_BTN_PIN 1
#define PAUSE_BTN_PIN 2
#define PIT_BTN_PIN 3
#define RESET_BTN_PIN 4
#define RUMBLE_SWITCH_PIN 5
#define PIT_ENABLE_SWITCH_PIN 6
#define PIT_RELEASE_PIN 39

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

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;
const long PITopenTime = 1000; // activate solenoid for 1000ms
bool PITreleased = false;

const int countdownTIME = 3; // countdown timer length in minutes
const int countdownToFight = 3; // countdown timer length in seconds
const int PITreleaseTime = 90; // automatic pit release time in seconds until end of countdown
bool countdownPAUSED = false;
CountDown FightCountDown[1];

// Rumble stopwatch
StopWatch rumbleTIME;

int CLOCK_LED_BRIGHTNESS = 16; // 64 is okay

int BLINK_COUNTER_REDTEAM = 0;
int BLINK_COUNTER_BLUETEAM = 0;

bool ARENA_READY = true;
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[] = {0x48, 0x27, 0xE2, 0x5D, 0xB6, 0x84};
uint8_t broadcastAddressBLUETEAMbutton[] = {0x48, 0x27, 0xE2, 0x5D, 0xB6, 0x84};
// struct for pilot button data:
typedef struct struct_message_TEAMButton {
  bool REDTEAMButtonLED;
  bool BLUETEAMButtonLED;
} struct_message_TEAMButton;

struct_message_TEAMButton sendTEAMButtonDATA;

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 outputs
  pinMode(PIT_RELEASE_PIN, OUTPUT);

  digitalWrite(PIT_RELEASE_PIN, 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 Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);

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

  // set rumble stopwatch resolution to seconds

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


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


  // set rumble stopwatch resolution to seconds
}

int XDAS = 255;
bool toggle = false;
void loop() {
  // poll all the switch/button inputs
  pollInput();
  // deactivate solenoids if needed
  checkPIT();

  // Normal fight routine
  while (!switchRUMBLE.on()) {
    // poll all the switch/button inputs
    pollInput();
    // deactivate solenoids if needed
    checkPIT();
    // start button logic
    if (buttonSTARTvar) {
      buttonSTARTvar = false;
      if (!FightCountDown[0].isRunning() && countdownPAUSED == false) {
        if (buttonREDTEAMvar && buttonBLUETEAMvar) {
          buttonREDTEAMvar = false;
          buttonBLUETEAMvar = false;
          FightCountDown[0].start(0,0,countdownTIME,0);
        }
      }
      else {
        if (buttonREDTEAMvar && buttonBLUETEAMvar) {
          buttonREDTEAMvar = false;
          buttonBLUETEAMvar = false;
          countdownPAUSED = false;
          FightCountDown[0].cont();
        }
      }
    }
    // forced start of countdown
    if (buttonSTARTforced) {
      buttonSTARTforced = false;
      if (!FightCountDown[0].isRunning() && countdownPAUSED == false) {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        FightCountDown[0].start(0,0,countdownTIME,0);
      }
      else {
        buttonREDTEAMvar = false;
        buttonBLUETEAMvar = false;
        countdownPAUSED = false;
        FightCountDown[0].cont();
      }
    }
    // pause button logic
    if (buttonPAUSEvar) {
      buttonPAUSEvar = false;
      if (FightCountDown[0].isRunning()) {
        countdownPAUSED = true;
        FightCountDown[0].stop();
      }
    }
    // pit button logic
    if (buttonPITvar) {
      buttonPITvar = false;
      openPIT();
    }
    // automatic pit release
    if (FightCountDown[0].remaining() <= PITreleaseTime && FightCountDown[0].remaining() > 0 && switchPIT.on()) {
      openPIT();
    }
    // reset button logic
    if (buttonRESETvar) {
      buttonRESETvar = false;
      PITreleased = false;
      ESP.restart();
    }
    //blink_LED_RedTeam(200);
    //blink_LED_BlueTeam(200);
    // update the LED Display
    sendTimeDisplay((FightCountDown[0].remaining()/60%10), (FightCountDown[0].remaining()%60), 0, XDAS, 0, CLOCK_LED_BRIGHTNESS);
  }


  // Rumble fightroutine
  while (switchRUMBLE.on()) {
    // poll all the switch/button inputs
    pollInput();
    // deactivate solenoids if needed
    checkPIT();
    // update the LED Display
    sendTimeDisplay(12, 34, XDAS, 0, XDAS, CLOCK_LED_BRIGHTNESS);
  }
}