#include <string>

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an ESP32 C3:       8(SDA), 9(SCL)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#include <ezButton.h>

ezButton button1(0);
ezButton button2(1);
ezButton button3(2);
ezButton button4(3);
ezButton button5(4);

#include "ESP32_NOW.h"
#include "WiFi.h"
#include "esp_mac.h"

#include <vector>

/* Definitions */

// Wi-Fi interface to be used by the ESP-NOW protocol
#define ESPNOW_WIFI_IFACE WIFI_IF_STA

// Channel to be used by the ESP-NOW protocol
#define ESPNOW_WIFI_CHANNEL 4

// Delay between sending messages
#define ESPNOW_SEND_INTERVAL_MS 5000

// Number of peers to wait for (excluding this device)
#define ESPNOW_PEER_COUNT 1

// Report to other devices every 5 messages
#define REPORT_INTERVAL 5


// Primary Master Key (PMK) and Local Master Key (LMK)
#define ESPNOW_DEVICE_PMK "TWEDpmk20250527"
#define ESPNOW_DEVICE_LMK "TWEDlmk20250527"

typedef struct {
  uint32_t msgType;               // 0 - command, 1 - callback
  char msgData[20];
} __attribute__((packed)) esp_now_data_t;

esp_now_data_t new_msg;


void setupLCDDisplay(){
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  showTextMessage("LCD Ready", true, 2);
}

void showTextMessage(std::string _message, bool _clearDisplay, int _textSize){
  if(_clearDisplay){
    display.clearDisplay();
  }

  display.setTextSize(_textSize);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  if(_clearDisplay){
    display.setCursor(0,0);             // Start at top-left corner
  }

  const char *msgchar = _message.c_str();

  display.println(msgchar);
  
  display.display();
}


class ESP_NOW_Network_Peer : public ESP_NOW_Peer {
public:

  ESP_NOW_Network_Peer(const uint8_t *mac_addr, const uint8_t *lmk = (const uint8_t *)ESPNOW_DEVICE_LMK)
    : ESP_NOW_Peer(mac_addr, ESPNOW_WIFI_CHANNEL, ESPNOW_WIFI_IFACE, lmk) {}

  ~ESP_NOW_Network_Peer() {}

  bool begin() {
    // In this example the ESP-NOW protocol will already be initialized as we require it to receive broadcast messages.
    if (!add()) {
      log_e("Failed to initialize ESP-NOW or register the peer");
      return false;
    }
    return true;
  }

  bool send_message(const uint8_t *data, size_t len) {
    if (data == NULL || len == 0) {
      log_e("Data to be sent is NULL or has a length of 0");
      return false;
    }

    showTextMessage("Sending Message", true, 1);

    // Call the parent class method to send the data
    return send(data, len);
  }

  void onReceive(const uint8_t *data, size_t len, bool broadcast) {
    esp_now_data_t *msg = (esp_now_data_t *)data;

    std::string strMsg(msg->msgData);
    // std::string strCount(msg->count);

    // Display message to LCD
    showTextMessage(strMsg, true, 2);
    // showTextMessage(strCount,false);
  }

  void onSent(bool success) {
    bool broadcast = memcmp(addr(), ESP_NOW.BROADCAST_ADDR, ESP_NOW_ETH_ALEN) == 0;


    if (broadcast) {
      log_i("Broadcast message reported as sent %s", success ? "successfully" : "unsuccessfully");
    } else {
      log_i("Unicast message reported as sent %s to peer " MACSTR, success ? "successfully" : "unsuccessfully", MAC2STR(addr()));
    }
  }
};

/* Peers */

std::vector<ESP_NOW_Network_Peer *> peers;                             // Create a vector to store the peer pointers
ESP_NOW_Network_Peer broadcast_peer(ESP_NOW.BROADCAST_ADDR, NULL);     // Register the broadcast peer (no encryption support for the broadcast address)
ESP_NOW_Network_Peer *master_peer = nullptr;                           // Pointer to peer that is the master


void setupESPNOW(){
  uint8_t self_mac[6];

  // Initialize the Wi-Fi module
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(ESPNOW_WIFI_CHANNEL);
  while (!WiFi.STA.started()) {
    delay(100);
  }

  
  // Initialize the ESP-NOW protocol
  if (!ESP_NOW.begin((const uint8_t *)ESPNOW_DEVICE_PMK)) {
    // Serial.println("Failed to initialize ESP-NOW");
    delay(5000);
    ESP.restart();
  }

  if (!broadcast_peer.begin()) {
    // Serial.println("Failed to initialize broadcast peer");
    delay(5000);
    ESP.restart();
  }
  
  // Register the callback to be called when a new peer is found
  ESP_NOW.onNewPeer(register_new_peer, NULL);
  
  memset(&new_msg, 0, sizeof(new_msg));

  showTextMessage("ESP_NOW Ready", false, 2);

}

/* Callbacks */

// Callback called when a new peer is found
void register_new_peer(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
  esp_now_data_t *msg = (esp_now_data_t *)data;

  ESP_NOW_Network_Peer *new_peer = new ESP_NOW_Network_Peer(info->src_addr);
  if (new_peer == nullptr || !new_peer->begin()) {
    Serial.println("Failed to create or register the new peer");
    delete new_peer;
    return;
  }
  peers.push_back(new_peer);
}



void setup() {
  Serial.begin(115200);
  setupLCDDisplay();
  setupESPNOW();
  button1.setDebounceTime(50);
  button2.setDebounceTime(50);
  button3.setDebounceTime(50);
  button4.setDebounceTime(50);
  button5.setDebounceTime(50);
}

int msgCount = 0;
bool buttonPressed = false;
std::string cmdsent = "INTRO";

void loop() {
  button1.loop();
  button2.loop();
  button3.loop();
  button4.loop();
  button5.loop();
  
  cmdsent = readButtons();

  if(buttonPressed){
    Serial.println("A button is pressed.");
    strncpy(new_msg.msgData, cmdsent.c_str(), sizeof(new_msg.msgData));

    bool sendResult = broadcast_peer.send_message((const uint8_t *)&new_msg, sizeof(new_msg));

    if (sendResult) {
      showTextMessage(cmdsent,true,2);
      showTextMessage("msgData Sent",false,1);
    } else {
      showTextMessage("Sending Failed",false,2);
    }
  }
}

std::string getDefaultMacAddress() {

  std::string mac = "";

  unsigned char mac_base[6] = {0};

  if (esp_efuse_mac_get_default(mac_base) == ESP_OK) {
    char buffer[18];  // 6*2 characters for hex + 5 characters for colons + 1 character for null terminator
    sprintf(buffer, "%02X:%02X:%02X:%02X:%02X:%02X", mac_base[0], mac_base[1], mac_base[2], mac_base[3], mac_base[4], mac_base[5]);
    mac = buffer;
  }

  return mac;
}

std::string getInterfaceMacAddress(esp_mac_type_t interface) {

  std::string mac = "";

  unsigned char mac_base[6] = {0};

  if (esp_read_mac(mac_base, interface) == ESP_OK) {
    char buffer[18];  // 6*2 characters for hex + 5 characters for colons + 1 character for null terminator
    sprintf(buffer, "%02X:%02X:%02X:%02X:%02X:%02X", mac_base[0], mac_base[1], mac_base[2], mac_base[3], mac_base[4], mac_base[5]);
    mac = buffer;
  }

  return mac;
}

std::string readButtons(){
  std::string msg = "";
  buttonPressed = 0;
  if(button1.isPressed()){
    msg = "INTRO";
    buttonPressed = 1;
  }
  if(button2.isPressed()){
    msg = "VERSE";
    buttonPressed = 1;
  }
  if(button3.isPressed()){
    msg = "PRECHORUS";
    buttonPressed = 1;
  }
  if(button4.isPressed()){
    msg = "CHORUS";
    buttonPressed = 1;
  }
  if(button5.isPressed()){
    msg = "BRIDGE";
    buttonPressed = 1;
  }

  return msg;
}