#include <esp_now.h>
#include <WiFi.h>

#define CHANNEL 1


void InitESPNow() {     // Init ESP Now with fallback
  WiFi.disconnect();    //Ensures STA mode -> disconnects from existing wifi connections
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");    // Works as intended
   }
  else {
    Serial.println("ESPNow Init Failed");
    // Retry InitESPNow, add a counte and then restart?
    // InitESPNow();
    // or Simply Restart
    ESP.restart();
  }
}

void configDeviceAP() {     // Config AP SSID
  String Prefix = "Slave:";   // Slave will be used as the prefix AP SSID
  String Mac = WiFi.macAddress();   // Creates a sting variable of the devices MAC address
  String SSID = Prefix + Mac;   // Combines prefix + mac addr to make the final SSID
  String Password = "123456789";    // Password for the AP
  bool result = WiFi.softAP(SSID.c_str(), Password.c_str(), CHANNEL, 0);    // AP mode using "softAP" from WiFi library
  if (!result) {    // If opposite result
    Serial.println("AP Config failed.");    // Print the following
  } else {
    Serial.println("AP Config Success. Broadcasting with AP: " + String(SSID));   // Print the following
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("ESPNow/Basic/Slave");
  
  WiFi.mode(WIFI_AP);   //Set device in AP mode to begin with
  
  configDeviceAP();   // configure device AP mode
  
  Serial.print("AP MAC: "); Serial.println(WiFi.softAPmacAddress());    // Print MAC Address
  
  InitESPNow(); // Init ESPNow with a fallback logic
  
  esp_now_register_recv_cb(OnDataRecv);   // Once ESPNow is successfully Init, we will register for recv CB to
  // get recv packer info.
}

// callback when data is recv from Master
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Recv from: "); Serial.println(macStr);
  Serial.print("Last Packet Recv Data: "); Serial.println(*data);
  Serial.println("");
}

void loop() {
  // Chill
}