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

void readMacAddress(){
  uint8_t baseMac[6];
  esp_err_t ret = esp_wifi_get_mac(WIFI_IF_STA, baseMac);
  if (ret == ESP_OK) {
    Serial.printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
                  baseMac[0], baseMac[1], baseMac[2],
                  baseMac[3], baseMac[4], baseMac[5]);
  } else {
    Serial.println("Failed to read MAC address");
  }
}



esp_now_peer_info_t peerInfo;
// String send_data = "";

// Callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  if (status != ESP_NOW_SEND_SUCCESS) {
    Serial.println("SENDMSGERR");
  } else {
    Serial.println("SENDMSGDONE");
  }
}

// Callback when data is received
void OnDataRecv(uint8_t * mac, uint8_t *incomingData, int len) {
  String message = String((char *)incomingData).substring(0, len);
  if (message == "PING") {
    AutoSendMessageToPeer("PONG", mac);
  } else {
    //Serial.printf("INMSG %02x:%02x:%02x:%02x:%02x:%02x",
    //                mac[0], mac[1], mac[2],
    //                mac[3], mac[4], mac[5]);
    Serial.print("MI");
    for (int i = 0; i < 6; i++) {
      if (i > 0) Serial.print(":");
      if (mac[i] < 16) Serial.print("0");
      Serial.print(mac[i], HEX);
    }
    Serial.print("D");
    Serial.println(message);
  }
}

void setup(){
  Serial.begin(115200);

  WiFi.mode(WIFI_STA);
  WiFi.STA.begin();

  Serial.print("C");
  readMacAddress();

  if (esp_now_init() != ESP_OK) {
    Serial.println("ELC1500");
    return;
  }

  esp_now_register_send_cb(esp_now_send_cb_t(OnDataSent));
  esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv));

}

String waitForInput() {
  while(Serial.available() == 0) {}
  String input = Serial.readString();
  return input;
}
 
void loop() {
  String input = waitForInput();
  if (input == "C" || input == "C\n") {
    Serial.print("C");
    readMacAddress();
  } else if (input.startsWith("S")) {
    if (input.startsWith("ST")) {
      String receiverMacStr = input.substring(2,19);
      uint8_t receiverMac[6];

      macStringToBytes(receiverMacStr, receiverMac);
      if (input.charAt(19) == 'D') {
        String msgtxt = input.substring(20);
        AutoSendMessageToPeer(msgtxt, receiverMac);

      }
    }
  }
}