const int MAX_FOLLOWERS = 10;  // Maximum number of follower devices

typedef struct struct_message {
  byte devCode;
  Vec3 ESPInBase[6];
  byte cmd;
  char message[50];
} struct_message;

struct_message espNowMessage;
struct_message espNowMegsRecv;
esp_now_peer_info_t peerInfo;

#define ESP_NOW_CHANNEL 0
#define ESP_NOW_ENCRYPT false

uint8_t thisDevMac[6];
uint8_t singleFollowerDev[6];


// input a mac[6]:{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}
// return String: "FF:FF:FF:FF:FF:FF"
String macToString(uint8_t mac[6]) {
  char macStr[18]; // 6 pairs of 2 characters + null terminator
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X", 
           mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  return String(macStr);
}


void getThisDevMacAddress() {
  // WiFi.macAddress(thisDevMac);

  esp_err_t ret = esp_wifi_get_mac(WIFI_IF_STA, thisDevMac);

  String getInfoJsonString = macToString(thisDevMac);
  thisMacStr=getInfoJsonString;
  jsonInfoHttp.clear();
  jsonInfoHttp["mac"] = getInfoJsonString;

  Serial.println(getInfoJsonString);
}


void changeEspNowMode(byte inputMode) {
  switch(inputMode) {
  case 0: espNowMode = inputMode;
    if (InfoPrint == 1) {Serial.println("esp-now mode: none");}
    screenLine_3 = "ESP-NOW: NONE";
    oled_update();

    jsonInfoHttp.clear();
    jsonInfoHttp["info"] = "esp-now mode: none";

    break;
  case 1: espNowMode = inputMode;
    espNowMessage.cmd = 0;
    if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(group)");}
    screenLine_3 = "ESP-NOW: F-LEADER-B";
    oled_update();

    jsonInfoHttp.clear();
    jsonInfoHttp["info"] = "esp-now mode: flow-leader(group)";

    break;
  case 2: espNowMode = inputMode;
    espNowMessage.cmd = 0;
    if (InfoPrint == 1) {Serial.println("esp-now mode: flow-leader(single)");}
    screenLine_3 = "ESP-NOW: F-LEADER-S";
    oled_update();

    jsonInfoHttp.clear();
    jsonInfoHttp["info"] = "esp-now mode: flow-leader(single)";

    break;
  case 3: espNowMode = inputMode;
    if (InfoPrint == 1) {Serial.println("esp-now mode: follower");}
    screenLine_3 = "ESP-NOW: > FOLLOWER <";
    oled_update();

    jsonInfoHttp.clear();
    jsonInfoHttp["info"] = "esp-now mode: follower";

    break;
  }
}


// callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  Serial.print("Packet to: ");
  // Copies the sender mac address to a string
  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(macStr);
  Serial.print(" send status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}


void macStringToByteArray(const String& macString, uint8_t* byteArray) {
  for (int i = 0; i < 6; i++) {
    byteArray[i] = strtol(macString.substring(i * 3, i * 3 + 2).c_str(), NULL, 16);
  }
  return;
}


void OnDataRecv(const esp_now_recv_info_t *info, const unsigned char* incomingData, int len) {
// void OnDataRecv(const unsigned char* mac, const unsigned char* incomingData, int len) {
  const uint8_t *mac = info->src_addr;
  if (espNowMode != 3){
    return;
  }

  if (!ctrlByBroadcast) {
    if (memcmp(mac, mac_whitelist_broadcast, 6) != 0) {
      return;
    }
  }

  memcpy(&espNowMegsRecv, incomingData, sizeof(espNowMegsRecv));
  if (InfoPrint == 1) {
    Serial.print("Bytes received: ");Serial.println(len);
  }

  DeserializationError err = deserializeJson(jsonCmdReceive, espNowMegsRecv.message);
  switch(espNowMegsRecv.cmd) {
  case 0:
          for(int i=0;i<6;i++)
            {
              BasesetXYZ[i].x= espNowMegsRecv.ESPInBase[i].x;
              BasesetXYZ[i].y= espNowMegsRecv.ESPInBase[i].y;
              BasesetXYZ[i].z= espNowMegsRecv.ESPInBase[i].z;
            }
            Claws_moveServoPosByXYZ();
                            break;
  case 1:
    if (err == DeserializationError::Ok) {
      jsonCmdReceiveHandler();
    };break;
  case 2:
    if (err == DeserializationError::Ok) {
      runNewJsonCmd = true;
    };break;
  case 3:
    Serial.println(espNowMegsRecv.message);
  }
}


void initEspNow() {
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  // register esp-now sending call back function.
  esp_now_register_send_cb(OnDataSent);

  // register esp-now receving call back function.
  esp_now_register_recv_cb(OnDataRecv);

  // register peer
  peerInfo.channel = ESP_NOW_CHANNEL;  
  peerInfo.encrypt = ESP_NOW_ENCRYPT;
  //peerInfo.ifidx = WIFI_IF_STA; 
}


void registerNewFollowerToPeer(String inputMac) {
  if (inputMac.length() != 17) {
    Serial.println("invalid MAC address format.");
    return;
  }

  uint8_t macArray[6];
  macStringToByteArray(inputMac, macArray);
  for (int i = 0; i < 6; i++) {
    singleFollowerDev[i] = macArray[i];
  }
  memcpy(peerInfo.peer_addr, macArray, 6);
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer: " + inputMac);

    jsonInfoHttp.clear();
    jsonInfoHttp["info"] = "Failed to add peer";
    jsonInfoHttp["mac"] = inputMac;

    return;
  }
  Serial.println("add peer: " + inputMac);

  jsonInfoHttp.clear();
  jsonInfoHttp["info"] = "add peer";
  jsonInfoHttp["mac"]  = inputMac;
}


void deleteFollower(String inputMac) {
  if (inputMac.length() != 17) {
    Serial.println("invalid MAC address format.");
    return;
  }

  uint8_t macArray[6];
  macStringToByteArray(inputMac, macArray);
  esp_now_del_peer(macArray);

  jsonInfoHttp.clear();
  jsonInfoHttp["info"] = "delete peer";
  jsonInfoHttp["mac"]  = inputMac;

  if (InfoPrint == 1) {
    Serial.println("delete peer: " + inputMac);
  }
}


void espNowGroupSend(byte devCodeIn, double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3, double x4, double y4, double z4,double x5, double y5, double z5, double x6, double y6, double z6,byte cmdIn, String messageIn) {
  espNowMessage.devCode  = devCodeIn;
  espNowMessage.ESPInBase[0].x     = x1;
  espNowMessage.ESPInBase[0].y     = y1;
  espNowMessage.ESPInBase[0].z     = z1;
  espNowMessage.ESPInBase[1].x     = x2;
  espNowMessage.ESPInBase[1].y     = y2;
  espNowMessage.ESPInBase[1].z     = z2;
  espNowMessage.ESPInBase[2].x     = x3;
  espNowMessage.ESPInBase[2].y     = y3;
  espNowMessage.ESPInBase[2].z     = z3;
  espNowMessage.ESPInBase[3].x     = x4;
  espNowMessage.ESPInBase[3].y     = y4;
  espNowMessage.ESPInBase[3].z     = z4;
  espNowMessage.ESPInBase[4].x     = x5;
  espNowMessage.ESPInBase[4].y     = y5;
  espNowMessage.ESPInBase[4].z     = z5;
  espNowMessage.ESPInBase[5].x     = x6;
  espNowMessage.ESPInBase[5].y     = y6;
  espNowMessage.ESPInBase[5].z     = z6;
  espNowMessage.cmd      = cmdIn;
  // espNowMessage.message  = messageIn;
  strcpy(espNowMessage.message, messageIn.c_str());

  esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
     //Serial.printf("Error sending the data: %d\n", result);
  }
}


void espNowSingleDevSend(String inputMac, byte devCodeIn,double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3, double x4, double y4, double z4,double x5, double y5, double z5, double x6, double y6, double z6, byte cmdIn, String messageIn){
  if (inputMac.length() != 17) {
    Serial.println("invalid MAC address format.");
    return;
  }

  espNowMessage.devCode  = devCodeIn;
  espNowMessage.ESPInBase[0].x     = x1;
  espNowMessage.ESPInBase[0].y     = y1;
  espNowMessage.ESPInBase[0].z     = z1;
  espNowMessage.ESPInBase[1].x     = x2;
  espNowMessage.ESPInBase[1].y     = y2;
  espNowMessage.ESPInBase[1].z     = z2;
  espNowMessage.ESPInBase[2].x     = x3;
  espNowMessage.ESPInBase[2].y     = y3;
  espNowMessage.ESPInBase[2].z     = z3;
  espNowMessage.ESPInBase[3].x     = x4;
  espNowMessage.ESPInBase[3].y     = y4;
  espNowMessage.ESPInBase[3].z     = z4;
  espNowMessage.ESPInBase[4].x     = x5;
  espNowMessage.ESPInBase[4].y     = y5;
  espNowMessage.ESPInBase[4].z     = z5;
  espNowMessage.ESPInBase[5].x     = x6;
  espNowMessage.ESPInBase[5].y     = y6;
  espNowMessage.ESPInBase[5].z     = z6;
  espNowMessage.cmd      = cmdIn;
  // espNowMessage.message  = messageIn;
  strcpy(espNowMessage.message, messageIn.c_str());

  uint8_t macArray[6];
  macStringToByteArray(inputMac, macArray);
  for (int i = 0; i < 6; i++) {
    singleFollowerDev[i] = macArray[i];
  }
 
  esp_err_t result = esp_now_send(
    macArray, 
    (uint8_t *) &espNowMessage,
    sizeof(struct_message));

  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
    //Serial.printf("Error sending the data: %d\n", result);
  }
}


void espNowSingleDevFlowCtrl() {
  for(int i=0;i<6;i++)
  {
    espNowMessage.ESPInBase[i].x= BasesetXYZ[i].x;
    espNowMessage.ESPInBase[i].y= BasesetXYZ[i].y;
    espNowMessage.ESPInBase[i].z= BasesetXYZ[i].z;
  }
  esp_err_t result = esp_now_send(singleFollowerDev, 
                                  (uint8_t *) &espNowMessage,
                                  sizeof(struct_message));

  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }
}


void espNowGroupDevsFlowCtrl() {
  for(int i=0;i<6;i++)
  {
    espNowMessage.ESPInBase[i].x= BasesetXYZ[i].x;
    espNowMessage.ESPInBase[i].y= BasesetXYZ[i].y;
    espNowMessage.ESPInBase[i].z= BasesetXYZ[i].z;
  }

  esp_err_t result = esp_now_send(0, (uint8_t *) &espNowMessage, sizeof(struct_message));
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }
}


void changeBroadcastMode(bool inputMode, String inputMac) {
  ctrlByBroadcast = inputMode;

  jsonInfoHttp.clear();
  jsonInfoHttp["mode"]  = inputMode;

  uint8_t macArray[6];
  macStringToByteArray(inputMac, macArray);
  for (int i = 0; i < 6; i++) {
    mac_whitelist_broadcast[i] = macArray[i];
  }

  if (InfoPrint == 1) {
    if (ctrlByBroadcast) {
      Serial.println("it can be ctrl by esp-now broadcast cmd.");
      jsonInfoHttp["info"] = "it can be ctrl by esp-now broadcast cmd.";
      jsonInfoHttp["leader mac"] = inputMac;
    } else {
      Serial.println("it won't be ctrl by esp-now broadcast cmd.");
      jsonInfoHttp["info"] = "it won't be ctrl by esp-now broadcast cmd, leader mac: "+inputMac;
    }
  }
}