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


#define NUMSLAVES 20    // Global copy of slave
esp_now_peer_info_t slaves[NUMSLAVES] = {};   // Array to store mac addresses of the slaves
int SlaveCnt = 0;   // Counter for number of slaves

#define CHANNEL 1
#define PRINTSCANRESULTS 0


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 ScanForSlave() {
  int8_t scanResults = WiFi.scanNetworks();   // Scan for slaves in AP mode
  
  memset(slaves, 0, sizeof(slaves));    //reset slaves
  SlaveCnt = 0;
  Serial.println("");
  if (scanResults == 0) {
    Serial.println("No WiFi devices in AP Mode found");   // No devices found
  } else {
    Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");   // Device/Devices found
    for (int i = 0; i < scanResults; ++i) {   // Increase number of slaves by one for every device found
      
      String SSID = WiFi.SSID(i);   // Gets the SSID of the i:th device found
      int32_t RSSI = WiFi.RSSI(i);  // Gets the recieved signal strength indicator of the i:th device  
      String BSSIDstr = WiFi.BSSIDstr(i);   // Gets the MAC address (BSSID) of the i:th device found and converts it to a string

      if (PRINTSCANRESULTS) {
        Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
      }
      delay(10);
      if (SSID.indexOf("Slave") == 0) {   // Check if the current device starts with `Slave` -> SSID of interest -> print the following
        Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
        
        int mac[6];   // Get BSSID -> Mac Address of the Slave

        if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x",  &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] ) ) {
          for (int ii = 0; ii < 6; ++ii ) {
            slaves[SlaveCnt].peer_addr[ii] = (uint8_t) mac[ii];
          }
        }
        slaves[SlaveCnt].channel = CHANNEL; // pick a channel
        slaves[SlaveCnt].encrypt = 0; // no encryption
        SlaveCnt++;
      }
    }
  }

  if (SlaveCnt > 0) {   // If more than 0 slaves
    Serial.print(SlaveCnt); Serial.println(" Slave(s) found, processing..");    // Print the following
  } else {
    Serial.println("No Slave Found, trying again.");    // No slaves found -> trying again
  }

  
  WiFi.scanDelete();    // clean up ram
}

// Check if the slave is already paired with the master.
// If not, pair the slave with master

void manageSlave() {
  if (SlaveCnt > 0) {
    for (int i = 0; i < SlaveCnt; i++) {
      Serial.print("Processing: ");
      for (int ii = 0; ii < 6; ++ii ) {
        Serial.print((uint8_t) slaves[i].peer_addr[ii], HEX);
        if (ii != 5) Serial.print(":");
      }
      Serial.print(" Status: ");
      // check if the peer exists
      bool exists = esp_now_is_peer_exist(slaves[i].peer_addr);
      if (exists) {
        // Slave already paired.
        Serial.println("Already Paired");
      } else {
        // Slave not paired, attempt pair
        esp_err_t addStatus = esp_now_add_peer(&slaves[i]);
        if (addStatus == ESP_OK) {
          
          Serial.println("Pair success");   // Pair success
        } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
          Serial.println("ESPNOW Not Init");    // ESPNow not done
        } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
          Serial.println("Add Peer - Invalid Argument");    // Invalid argument
        } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
          Serial.println("Peer list full");   // Peer list full -> max number of slaves on list
        } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
          Serial.println("Out of memory");    // Out of memory
        } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
          Serial.println("Peer Exists");      // Peer already exist -> not need to pair again
        } else {
          Serial.println("Not sure what happened");   // Joku muu män viduiks
        }
        delay(100);
      }
    }
  } else {
    
    Serial.println("No Slave found to process");    // No slave found to process
  }
}


uint8_t data = 0;   // Variable "data" stores the data that is sent to slaves

void sendData() {   // send data
  data++;   // "data" +1
  for (int i = 0; i < SlaveCnt; i++) {    // Cycling the found slaves in sequense
    const uint8_t *peer_addr = slaves[i].peer_addr;     // Retrieves the MAC address of the i:th slave. "peer_addr" is a pointer to the array.
    if (i == 0) {   // print only for first slave not all of them
      Serial.print("Sending: ");
      Serial.println(data);

    /*
    esp_err_t result = esp_now_send(NULL, &data, sizeof(data));   
    "NULL" in the place of the "peer_addr" enables group sending the data instead of seding it to each slave sequentially
    The loop for sending data sequentially must be removed as well
    */   
    
    }
    esp_err_t result = esp_now_send(peer_addr, &data, sizeof(data));    // Sends the data to the current slave
    Serial.print("Send Status: ");
    if (result == ESP_OK) {
      Serial.println("Success");    // Worked
    } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
      Serial.println("ESPNOW not Init.");   // ESPNow not done
    } else if (result == ESP_ERR_ESPNOW_ARG) {
      Serial.println("Invalid Argument");    // Invalid argument
    } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
      Serial.println("Internal Error");     // Internal error
    } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
      Serial.println("ESP_ERR_ESPNOW_NO_MEM");    // Out of memory
    } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
      Serial.println("Peer not found.");    // Did not find a peer
    } else {
      Serial.println("Not sure what happened");   // Joku muu män viduiks
    }
    delay(100);
  }
}

// callback when data is sent from Master to Slave
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];    // Declares a character array for storing the MAC address string
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",   // Formats the MAC address bytes into a string representation
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Sent to: "); Serial.println(macStr);    // Prints the last sent packages destination MAC address in the string format
  Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");   // Prints Success/Fail 
}

void setup() {
  Serial.begin(115200);
  
  WiFi.mode(WIFI_STA);    //Set device in STA mode

  Serial.println("ESPNow/Multi-Slave/Master");
  
  Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());   // MAC address of the Master in Station Mode
  
  InitESPNow();   // Init ESPNow with a fallback logic

  /*
  esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
  esp_now_add_peer(&slaves[0]); 
  Initializes ESPnow for group communication instead of the sequential
  */

  // Once ESPNow is successfully Init, we will register for Send CB to
  
  esp_now_register_send_cb(OnDataSent);   // get the status of Trasnmitted packet
}

void loop() {
  // In the loop we scan for slave
  ScanForSlave();
  // If Slave is found, it would be populate in `slave` variable
  // We will check if `slave` is defined and then we proceed further
  if (SlaveCnt > 0) { // check if slave channel is defined
    // `slave` is defined
    // Add slave as peer if it has not been added already
    manageSlave();
    // pair success or already paired
    // Send data to device
    sendData();
  } else {
    // No slave found to process
  }

  // wait for 3seconds to run the logic again
  delay(1000);
}