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



uint8_t broadcastAddress1[] = {0xFC, 0xF5, 0xC4, 0x2F, 0x71, 0x24}; //ALPHA
//uint8_t broadcastAddress1[] = {0xFC, 0xF5, 0xC4, 0x2F, 0xE2, 0xF0}; //BETA


unsigned long cnt, currentus, prevus, interval, tmp1, tmp0, loss;

// Variable to store if sending data was successful
String success;

// Callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
//  Serial.print("\r\nLast Packet Send Status:\t");
//  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
//  if (status ==0){
//    success = "Delivery Success :)";
//  }
//  else{
//    success = "Delivery Fail :(";
//  }
}

// Callback when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  memcpy(&tmp1, incomingData, sizeof(tmp1));
  if (tmp1 - tmp0 != 1)
  {
    Serial.println("packet loss"); 
	loss++;
	Serial.println(loss, DEC); 
    Serial.println(cnt, DEC); 
  }
  tmp0 = tmp1;
}
 
void setup() {
  // Init Serial Monitor
  Serial.begin(115200);

  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_register_send_cb(OnDataSent);

  // Register for a callback function that will be called when data is received
  esp_now_register_recv_cb(OnDataRecv);
  
  // Register peer
  esp_now_peer_info_t peerInfo;
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  
  // Add peer 1
  memcpy(peerInfo.peer_addr, broadcastAddress1, 6);       
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }

  xTaskCreate(
                    sendMsg,          /* Task function. */
                    "sendMsg",        /* String with name of task. */
                    10000,            /* Stack size in bytes. */
                    NULL,             /* Parameter passed as input of the task */
                    1,                /* Priority of the task. */
                    NULL);            /* Task handle. */
}


void sendMsg( void * parameter )
{
  while(1)
  {
	  prevus = micros();	  
	  esp_err_t result = esp_now_send(0, (uint8_t *) &cnt, sizeof(cnt));
	  currentus = micros;
	  interval = currentus - prevus;
	  Serial.println(interval, DEC);
	  if (result == ESP_OK) {
	  Serial.println("Sent with success");
	  }
	  else {
	  Serial.println("Error sending the data");
	  }
	  cnt++;
	  vTaskDelay( 1000 ); 
  }
}

void loop() {}
