#include <SPI.h>
#include <TFT_eSPI.h>
#include "ESP32_NOW.h"
#include "WiFi.h"
#include <esp_mac.h>  // For the MAC2STR and MACSTR macros
#include <vector>

TFT_eSPI tft = TFT_eSPI();

/* Definitions */
#define SCREEN_WIDTH 320
#define SCREEN_HEIGHT 240
#define FONT_SIZE 4

#define ESPNOW_WIFI_CHANNEL 6

/* Classes */

// Creating a new class that inherits from the ESP_NOW_Peer class is required.

class ESP_NOW_Peer_Class : public ESP_NOW_Peer {
public:
  // Constructor of the class
  ESP_NOW_Peer_Class(const uint8_t *mac_addr,
                     uint8_t channel,
                     wifi_interface_t iface,
                     const uint8_t *lmk)
    : ESP_NOW_Peer(mac_addr, channel, iface, lmk) {}

  // Destructor of the class
  ~ESP_NOW_Peer_Class() {}

  // Function to register the control peer
  bool add_peer() {
    if (!add()) {
      log_e("Failed to register the broadcast peer");
      return false;
    }
    return true;
  }

  // Function to print the received messages from the control
  void onReceive(const uint8_t *data, size_t len, bool broadcast) {
    Serial.printf("Received a message from control " MACSTR " (%s)\n", MAC2STR(addr()), broadcast ? "broadcast" : "unicast");
    Serial.printf("  Message: %s\n", (char *)data);

    tft.init();          // Start the tft display
    tft.setRotation(1);  // Set the TFT display rotation in landscape mode

    // Clear the screen before writing to it
    tft.fillScreen(TFT_WHITE);
    tft.setTextColor(TFT_BLACK, TFT_WHITE);

    // Set X and Y coordinates for center of display
    int centerX = SCREEN_WIDTH / 2;
    int centerY = SCREEN_HEIGHT / 2;

    tft.drawCentreString("Communication test!", centerX, centerY - 100, FONT_SIZE);
    tft.drawCentreString("Received a message from controller :", centerX, centerY - 40, 2);
    tft.setCursor(70, 120, 2);
    tft.printf(MACSTR " (%s)\n", MAC2STR(addr()), broadcast ? "broadcast" : "unicast");
    tft.setCursor(70, 140, 2);
    tft.printf("Message: %s\n", (char *)data);

    tft.drawCentreString("From Alexis", 280, 220, 2);
  }
};

/* Global Variables */

// List of all the controls. It will be populated when a new control is registered
std::vector<ESP_NOW_Peer_Class> masters;

/* Callbacks */

// Callback called when an unknown peer sends a message
void register_new_master(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
  if (memcmp(info->des_addr, ESP_NOW.BROADCAST_ADDR, 6) == 0) {
    Serial.printf("Unknown peer " MACSTR " sent a broadcast message\n", MAC2STR(info->src_addr));
    Serial.println("Registering the peer as a controller");

    ESP_NOW_Peer_Class new_master(info->src_addr, ESPNOW_WIFI_CHANNEL, WIFI_IF_STA, NULL);

    masters.push_back(new_master);
    if (!masters.back().add_peer()) {
      Serial.println("Failed to register the new controller");
      return;
    }
  } else {
    // The peripherals will only receive broadcast messages
    log_v("Received a unicast message from " MACSTR, MAC2STR(info->src_addr));
    log_v("Ignoring the message");
  }
}


void setup() {
  Serial.begin(115200);
  
  // Initialize the Wi-Fi module
  WiFi.mode(WIFI_STA);
  WiFi.setChannel(ESPNOW_WIFI_CHANNEL);
  while (!WiFi.STA.started()) delay(100);

  Serial.println("ESP-NOW Example - Broadcast Peripherals");
  Serial.println("Wi-Fi parameters:");
  Serial.println("  Mode: STA");
  Serial.println("  MAC Address: " + WiFi.macAddress());
  Serial.printf("  Channel: %d\n", ESPNOW_WIFI_CHANNEL);

  // Initialize the ESP-NOW protocol
  if (!ESP_NOW.begin()) {
    Serial.println("Failed to initialize ESP-NOW");
    Serial.println("Reeboting in 5 seconds...");
    delay(5000);
    ESP.restart();
  }

  // Register the new peer callback
  ESP_NOW.onNewPeer(register_new_master, NULL);

  Serial.println("Setup complete. Waiting for a control to broadcast a message...");

  tft.init();          // Start the tft display
  tft.setRotation(1);  // Set the TFT display rotation in landscape mode

  // Clear the screen before writing to it
  tft.fillScreen(TFT_WHITE);
  tft.setTextColor(TFT_BLACK, TFT_WHITE);

  // Set X and Y coordinates for center of display
  int centerX = SCREEN_WIDTH / 2;
  int centerY = SCREEN_HEIGHT / 2;

  tft.drawCentreString("Communication test!", centerX, centerY - 100, FONT_SIZE);

  tft.drawCentreString("Communication will start soon", centerX, centerY, 2);

  tft.drawCentreString("From Alexis", 280, 220, 2);
}

void loop() {

  delay(1000);
}