//Included library
#include <esp_now.h>
#include <WiFi.h>
#include "pitches.h"
#include <Wire.h>


//Define The Pin To Which the buttons on the controller are connected
#define redButton 13
#define blueButton 14
#define greenButton 27
#define orangeButton 12
#define resetButton 26

//Define The Pin To Which the LEDs on the controller are connected
#define redLed 25
#define blueLed 32
#define greenLed 17
#define orangeLed 33
#define whiteLed 16

//Define The Pin To Which the Buzzer on the controller is connected
#define buzzerPin 4


unsigned long lastPressTime = 0;
const unsigned long debounceDelay = 2000;  //  debounce delay


//Check for the state(current step) and perform the required action
//State 0 :Wait for Quizz button to be pressed, when pressed send your board number to the master
//State 1 :Blink 5x and then change to State 0
//State 2 :Blink 1x then change to State 4
//State 3 :Turn OFF led
//State 4 :Turn ON led



// Define the notes in the melody played by the Buzzer:
int melody[] = {
  NOTE_C4, NOTE_C5, NOTE_C6, NOTE_C6
};
// Define the note durations: 4 = quarter note, 8 = eighth note,...
int noteDurations[] = {
  8,
  6,
  4,
  4,
};


//Define The RECEIVER boards MAC address (NUMBER WRITTEN ON THE BOARD)
//RED Quizz button ESP01 (02)
uint8_t broadcastAddress1[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
//BLUE Quizz button ESP01 (03)
uint8_t broadcastAddress2[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
//GREEN Quizz button ESP01 (04)
uint8_t broadcastAddress3[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
//ORANGE Quizz button ESP01 (05)
uint8_t broadcastAddress4[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};


// Define variables to storing the coming data
int buttonpressed;
// Variable storing if sending data was successful
String success;


//Structure_message type of the data to send, THE DATA MUST MATCH THE RECEIVER STRUCTURE
typedef struct struct_message {
  int value;
} struct_message;
// Create a struct_message to hold incoming Data
struct_message incomingButton;
// Create a struct_message to hold outgoing Data
struct_message sendToButton;


// It a must have line I think??
esp_now_peer_info_t peerInfo;


// Define the function called when Data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  /* 
  //Debug print
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  if (status == 0) {
    success = "Delivery Success :)";
  } else {
    success = "Delivery Fail :(";
  }
  */
}
// Define the function called when Data is received
void OnDataRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len) {
  // Extract the MAC address from recv_info if needed
  const uint8_t *mac_addr = recv_info->src_addr;

  // Copy incoming data to your structure
  memcpy(&incomingButton, incomingData, sizeof(incomingButton));
  /*
    // Debug Print
    Serial.print("Message received from: ");
    for (int i = 0; i < 6; i++) {
        Serial.print(mac_addr[i], HEX);
        if (i < 5) Serial.print(":");
    }
    Serial.println();
*/
  // Copy the received data to its variable and directly call the Check function with it
  unsigned long currentTime2 = millis();
  if (currentTime2 - lastPressTime < debounceDelay) {
    return;
  }
  lastPressTime = currentTime2;
  checkAndSend(buttonpressed = incomingButton.value);
}




void setup() {
  // Init Serial Monitor
  Serial.begin(115200);

  //Init buttons
  pinMode(redButton, INPUT_PULLUP);
  pinMode(blueButton, INPUT_PULLUP);
  pinMode(greenButton, INPUT_PULLUP);
  pinMode(orangeButton, INPUT_PULLUP);
  pinMode(resetButton, INPUT_PULLUP);

  //Init led
  pinMode(redLed, OUTPUT);
  pinMode(blueLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  pinMode(orangeLed, OUTPUT);
  pinMode(whiteLed, OUTPUT);

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

  // Once ESPNow is successfully Init
  // Register for a callback function that will be called when sending data
  esp_now_register_send_cb(OnDataSent);
  // Register for a callback function that will be called when receiving data
  esp_now_register_recv_cb(OnDataRecv);


  //Define peer info
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  // register first peer
  memcpy(peerInfo.peer_addr, broadcastAddress1, 6);
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  // register second peer
  memcpy(peerInfo.peer_addr, broadcastAddress2, 6);
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  // register third peer
  memcpy(peerInfo.peer_addr, broadcastAddress3, 6);
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  // register fourth peer
  memcpy(peerInfo.peer_addr, broadcastAddress4, 6);
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }


  //Blink all led to signal the end of setup
  blink(2);

  /*
  //Debug print
  Serial.println("Setup Done");
  */
}

void blink(int i) {
  while (i > 0) {
    digitalWrite(whiteLed, HIGH);
    digitalWrite(redLed, HIGH);
    digitalWrite(blueLed, HIGH);
    digitalWrite(orangeLed, HIGH);
    digitalWrite(greenLed, HIGH);
    delay(250);
    digitalWrite(whiteLed, LOW);
    digitalWrite(redLed, LOW);
    digitalWrite(blueLed, LOW);
    digitalWrite(orangeLed, LOW);
    digitalWrite(greenLed, LOW);
    delay(250);
    --i;
  }
}


void loop() {
  // If the reset button (white button) is pressed turn on the white led send reset command to all the Quiz push buttons then turn of the LED
  if (digitalRead(resetButton) == 0) {
    digitalWrite(whiteLed, HIGH);
    reset();
    delay(500);
    digitalWrite(whiteLed, LOW);
  }
  // If the red button is pressed, remove the red Quizz push button from the players and send a silent reset to the other buttons
  if (digitalRead(redButton) == 0) {
    removeButton(1);
    //delay(5);
    silentReset();
    //delay(500);
  }
  // If the blue button is pressed, remove the blue Quizz push button from the players and send a silent reset to the other buttons
  if (digitalRead(blueButton) == 0) {
    removeButton(2);
    //delay(5);
    silentReset();
    //delay(500);
  }
  // If the green button is pressed, remove the green Quizz push button from the players and send a silent reset to the other buttons
  if (digitalRead(greenButton) == 0) {
    removeButton(3);
    //delay(5);
    silentReset();
    //delay(500);
  }
  // If the orange button is pressed, remove the orange Quizz push button from the players and send a silent reset to the other buttons
  if (digitalRead(orangeButton) == 0) {
    removeButton(4);
    //delay(5);
    silentReset();
    //delay(500);
  }
}

// Function to make the buzzer sound
void sing() {
  // iterate over the notes of the melody:
  for (int thisNote = 0; thisNote < 4; thisNote++) {
    // to calculate the note duration, take one second divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000 / noteDurations[thisNote];
    tone(buzzerPin, melody[thisNote], noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(buzzerPin);
  }
  return;
}


// The function called when a signal from a Quizz push button is pressed to give the focus to the first pressed and send standby command to the others
void checkAndSend(int i) {

  switch (i) {
    // If The Red Quizz Pushbutton is pressed
    case 1:
      {
        // Send standby command to other Quizz button
        sendToButton.value = 3;
        esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
        // Send Blink to the Red Quizz pushbutton and turn the RED onboard LED
        sendToButton.value = 2;

        esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
        digitalWrite(redLed, HIGH);
        sing();
        break;
      }
    // If The Blue Quizz Pushbutton is pressed
    case 2:
      {
        // Send standby command to other Quizz button
        sendToButton.value = 3;
        esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
        // Send Blink to the Blue Quizz pushbutton and turn the BLUE onboard LED
        sendToButton.value = 2;

        esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
        digitalWrite(blueLed, HIGH);
        sing();
        break;
      }
    // If The Green Quizz Pushbutton is pressed
    case 3:
      {
        // Send standby command to other Quizz button
        sendToButton.value = 3;
        esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
        // Send Blink to the Green Quizz pushbutton and turn the GREEN onboard LED
        sendToButton.value = 2;

        esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
        digitalWrite(greenLed, HIGH);
        sing();

        break;
      }
    // If The Orange Quizz Pushbutton is pressed
    case 4:
      {
        // Send standby command to other Quizz button
        sendToButton.value = 3;
        esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
        esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
        // Send Blink to the Orange Quizz pushbutton and turn the ORANGE onboard LED
        sendToButton.value = 2;

        esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
        digitalWrite(orangeLed, HIGH);
        sing();
        break;
      }

    default:
      //Serial.println("Bug");
      break;
  }
}


// Reset the broadcast address of the pushbutton and send wait command for Quizz pushbutton input to all Quizz pushbutton
void reset() {
  sendToButton.value = 1;
  broadcastAddress1[5] = { 0x9C };
  digitalWrite(redLed, LOW);
  esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  broadcastAddress2[5] = { 0xA4 };
  digitalWrite(blueLed, LOW);
  esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  broadcastAddress3[5] = { 0x9C };
  digitalWrite(greenLed, LOW);
  esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  broadcastAddress4[5] = { 0x97 };
  digitalWrite(orangeLed, LOW);
  esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  blink(5);
}


// Send command to re enable Quizz push button to send data
void silentReset() {
  sendToButton.value = 0;
  esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
  esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
  delay(5);
}


// Remove Quizz button from the player list
void removeButton(int i) {
  // Send the standby command to the quizz button then change his broadcastAddress to remove him from future broadcast
  sendToButton.value = 3;
  switch (i) {
    case 1:
      {
        broadcastAddress1[5] = { 0x9C };
        digitalWrite(redLed, LOW);
        esp_now_send(broadcastAddress1, (uint8_t *)&sendToButton, sizeof(sendToButton));
        broadcastAddress1[5] = { 0x00 };
        break;
      }
    case 2:
      {
        broadcastAddress2[5] = { 0xA4 };
        digitalWrite(blueLed, LOW);
        esp_now_send(broadcastAddress2, (uint8_t *)&sendToButton, sizeof(sendToButton));
        broadcastAddress2[5] = { 0x00 };
        break;
      }
    case 3:
      {
        broadcastAddress3[5] = { 0x9C };
        digitalWrite(greenLed, LOW);
        esp_now_send(broadcastAddress3, (uint8_t *)&sendToButton, sizeof(sendToButton));
        broadcastAddress3[5] = { 0x00 };
        break;
      }
    case 4:
      {
        broadcastAddress4[5] = { 0x97 };
        digitalWrite(orangeLed, LOW);
        esp_now_send(broadcastAddress4, (uint8_t *)&sendToButton, sizeof(sendToButton));
        broadcastAddress4[5] = { 0x00 };
        break;
      }

    default:
      Serial.println("Bug");
      break;
  }
}