
#include "luma-stiks.h"
#include "WiFi.h"

static const char* tag = "stiks";

int PeerCount = 0;
PEER_ADDR peerList[LUMA_MAX_PEERS];

/*
  -30 is absolute min, -90 is max
*/
// TODO default rssi
LUMA_NODE* Luma_ScanAPMaching(String startName, float minRssi, int &countFound){
  // Initialize count
  countFound = 0;
  
  // Start WiFi scan
  int n = WiFi.scanNetworks();
  if (n == 0) {
    Serial.println("No networks found");
    return nullptr;
  } else {
    // Dynamically allocate memory for lumaStik array
    LUMA_NODE *node = new LUMA_NODE[n];
    
    Serial.println("Networks found:");
    for (int i = 0; i < n; ++i) {
      //if (WiFi.SSID(i).startsWith(nameFilter) && WiFi.RSSI(i) >= minRssi) {
	    if ( WiFi.SSID(i).startsWith(startName) ) {
        // Update lumaStik array with details
        node[countFound].ssid = WiFi.SSID(i);
		    memccpy(&node[countFound].peer.peer_addr, WiFi.BSSID(i), 6, sizeof(uint8_t));
        countFound++;

		    ESP_LOGD(tag, "SSID: %s, MAC: %s, RSSi: %d", WiFi.SSID(i), WiFi.BSSIDstr(i), WiFi.RSSI(i));
      }
      vTaskDelay(10);  // Short delay for responsiveness
    }
    
    WiFi.scanDelete();
    // If count is 0, free the allocated memory and return nullptr
    if (countFound == 0) {
      delete[] node;
      return nullptr;
    }

    return node;
  }
}

//bool Luma_Find_Node(const char* master_ssid, PEER_ADDR &addr, int &ch){
bool Luma_Find_Node(const char* master_ssid, esp_now_peer_info_t &node){
	int n = WiFi.scanNetworks();
  bool ret = false;

	for(int i = 0; i < n; ++i){
    if(strcmp(WiFi.SSID(i).c_str(), master_ssid) == 0){
      uint8_t* mac = WiFi.BSSID(i);
      memcpy(node.peer_addr, mac, 6);
      node.channel = WiFi.channel(i);
      String macStr = Luma_MacToStr(mac);
      ESP_LOGD(tag, "Found: %s, mac: %s, ch: %d", WiFi.SSID(i).c_str(), macStr.c_str(), node.channel);
			ret = true;
      break;
		}
	}

  WiFi.scanDelete();
	return ret;
}


#define MAC_LENGTH 6
int Luma_ConvertMACStringToMACAddress(const char* mChar, uint8_t* macAddr) {
	// Check for null pointer or empty string
    if (mChar == nullptr || strlen(mChar) < 17) {
        return 1;
    }

    uint8_t cnt = 0;

    for (uint8_t i = 0, j = 0; i < 17; i += 3, j++) {
        int highNibble, lowNibble;

        highNibble = mChar[i];
        lowNibble = mChar[i + 1];

        // Convert hexadecimal digit to decimal
        highNibble = isdigit(highNibble) ? highNibble - '0' : toupper(highNibble) - 'A' + 10;
        lowNibble = isdigit(lowNibble) ? lowNibble - '0' : toupper(lowNibble) - 'A' + 10;

        // Construct the byte and save it
        macAddr[j] = (highNibble << 4) | lowNibble;
    }

	return 0;
}

esp_err_t Luma_Send_Packet(PEER_ADDR &peerAddr, LUMA_PACKET &packet){
  ESP_LOGD(tag, "Sending to Mac: %s", Luma_MacToStr(peerAddr).c_str());

	esp_err_t result = esp_now_send(peerAddr, (const uint8_t*) &packet, sizeof(LUMA_PACKET));

	if (result == ESP_OK) {
		Serial.println("Success");
	} else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
		Serial.println("ESPNOW not Init.");
	} else if (result == ESP_ERR_ESPNOW_ARG) {
		Serial.println("Invalid Argument");
	} else if (result == ESP_ERR_ESPNOW_INTERNAL) {
		Serial.println("Internal Error");
	} else if (result == ESP_ERR_ESPNOW_NO_MEM) {
		Serial.println("ESP_ERR_ESPNOW_NO_MEM");
	} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
		Serial.println("Peer not found.");
	} else {
		Serial.println("Not sure what happened");
	}

	return result;
}


void Luma_SendToAll_Packet(LUMA_PACKET &packet){
  for(int i = 0; i < PeerCount; i++){
    Luma_Send_Packet(peerList[i], packet);
  }
}


esp_err_t Luma_Node_Check(PEER_ADDR &addr){
  uint8_t a = 100;
	return esp_now_send(addr, (const uint8_t*) &a, 1);
}


LUMA_NODE* Luma_Scan_For_Stations(String nameFilter, float minRssi, int &countFound) {
  countFound = 0;
  
  // Start WiFi scan
  int n = WiFi.scanNetworks();
  if (n == 0) {
    Serial.println("No networks found");
    return nullptr;
  } 
  else {
    // Dynamically allocate memory for lumaStik array
    LUMA_NODE *node = new LUMA_NODE[n];
    
    Serial.println("Networks found:");
    for (int i = 0; i < n; ++i) {
      //if (WiFi.SSID(i).startsWith(nameFilter) && WiFi.RSSI(i) >= minRssi) {
      if ( WiFi.SSID(i).startsWith(nameFilter) ) {
        // Update lumaStik array with details
        node[countFound].ssid = WiFi.SSID(i);
        memccpy(&node[countFound].peer.peer_addr, WiFi.BSSID(i), 6, sizeof(uint8_t));
        countFound++;

        ESP_LOGD(tag, "SSID: %s, MAC: %s, RSSi: %d", WiFi.SSID(i), WiFi.BSSIDstr(i), WiFi.RSSI(i));
      }
      vTaskDelay(10);  // Short delay for responsiveness
    }
      
    // If count is 0, free the allocated memory and return nullptr
    if (countFound == 0) {
      delete[] node;
      return nullptr;
    }
      
    return node;
  }
}

esp_err_t Luma_Register_with_Master( esp_now_peer_info_t &masterNode, const char* nodeName){
  LUMA_PACKET packet;
  packet.type = LPT_REG;
  LUMA_REGISTER_PACKET *reg = (LUMA_REGISTER_PACKET *)packet.data;
  strncpy(reg->ssid, nodeName, strlen(reg->ssid) + 1);
	return Luma_Send_Packet(masterNode.peer_addr, packet);
}

String Luma_MacToStr(uint8_t* mac){
  String macStr;

  for (int i = 0; i < 6; ++i) {
    // Convert each byte to a hexadecimal string
    // and append it to the String object
    String section = String(mac[i], HEX);
    if (section.length() == 1) {
      section = "0" + section;
    }
    macStr += section;

    // Add colon between sections, but not after the last one
    if (i < 5) {
      macStr += ":";
    }
  }

  // Convert the String to uppercase
  macStr.toUpperCase();

  return macStr;
}

int Luma_PeerCount(){
  return PeerCount;
}

bool Luma_Add_Peer(esp_now_peer_info_t &node) {
    // check if the peer exists
    bool exists = esp_now_is_peer_exist(node.peer_addr);
    if ( exists) {
      // Slave already paired.
      ESP_LOGD(tag, "Already Paired");
      return true;
    } else {
      // Slave not paired, attempt pair
      esp_err_t addStatus = esp_now_add_peer(&node);
      if (addStatus == ESP_OK) {
        // Pair success
        memcpy(&peerList[PeerCount][0], node.peer_addr, 6);
        PeerCount++;
        ESP_LOGD(tag, "Pair success");
        return true;
      } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
        // How did we get so far!!
        ESP_LOGD(tag, "ESPNOW Not Init");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
        ESP_LOGD(tag, "Invalid Argument");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
        ESP_LOGD(tag, "Peer list full");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
        ESP_LOGD(tag, "Out of memory");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
        ESP_LOGD(tag, "Peer Exists");
        return true;
      } else {
        ESP_LOGD(tag, "Not sure what happened");
        return false;
      }
    }

}