#include <WifiEspNow.h>
#if defined(ESP8266)
#include <ESP8266WiFi.h>
#elif defined(ESP32)
#include <WiFi.h>
#endif

// RTC_DATA_ATTR unsigned long addtomillis = 0;
RTC_DATA_ATTR unsigned long rstcnt = 0;

#define ONE  
//#define TWO  


#ifdef ONE 
  static uint8_t PEER[] {0xB4, 0xE6, 0x2D, 0x94, 0x1D, 0xF6};
  const char msg[] = "Hello from ONE" ;
#endif

#ifdef TWO  
  static uint8_t PEER[] {0xB4, 0xE6, 0x2D, 0x97, 0xFE, 0x22};
  const char msg[] = "Hello from TWO" ;
#endif

volatile boolean flgMsgRecd = false ;

void printReceivedMessage(const uint8_t mac[6], const uint8_t* buf, size_t count, void* cbarg) {
  Serial.printf("Message from %02X:%02X:%02X:%02X:%02X:%02X\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  for (int i = 0; i < count; ++i) {
    Serial.print(static_cast<char>(buf[i]));
  }
  Serial.println();
  delay(20) ;
  flgMsgRecd = true ;
}


void setup() {
/*  if(addtomillis ==0) { addtomillis = 1 ; }
  else { addtomillis = 300 ; }
  // This adds 300 ms to my RTC RAM variable addtomillis so that the clock can be restored  on wakeup
*/
  
  Serial.begin(115200);
  Serial.println();
#ifdef ONE 
  Serial.printf("I am One %lu \n",rstcnt) ;
#endif
#ifdef TWO
  Serial.printf("I am Two %lu \n",rstcnt) ;
#endif

  unsigned long t ;
  t = millis() ;

//Init ESP Now
  WiFi.persistent(false);
  WiFi.mode(WIFI_AP);

  WiFi.softAP("ESPNOW", nullptr, 3);
  WiFi.softAPdisconnect(false);
  bool ok = WifiEspNow.begin();
  if (!ok) {
    Serial.println("WifiEspNow.begin() failed");
    ESP.restart();
  }

  WifiEspNow.onReceive(printReceivedMessage, nullptr);

  ok = WifiEspNow.addPeer(PEER);
  if (!ok) {
    Serial.println("WifiEspNow.addPeer() failed");
    ESP.restart();
  }

// Main sketch
  char s[60];

  Serial.println(millis()-t) ;
  unsigned long tim = millis() ;
  while((!flgMsgRecd) && ((tim-millis())<250)) ;
  flgMsgRecd = false ;
  int len = snprintf(s, sizeof(s), "%s with Mac Id %s at %lu %d",msg, WiFi.softAPmacAddress().c_str(), millis(),rstcnt++);
  WifiEspNow.send(PEER, reinterpret_cast<const uint8_t*>(s), sizeof(s));
  Serial.printf("Been awake for %lu \n",20+millis()-t) ;
  delay(20) ;
  esp_sleep_enable_timer_wakeup(300000);
  esp_deep_sleep_start();
}

void InitESPNow(void) {
  WiFi.persistent(false);
  WiFi.mode(WIFI_AP);

  WiFi.softAP("ESPNOW", nullptr, 3);
  WiFi.softAPdisconnect(false);
  bool ok = WifiEspNow.begin();
  if (!ok) {
    Serial.println("WifiEspNow.begin() failed");
    ESP.restart();
  }

  WifiEspNow.onReceive(printReceivedMessage, nullptr);

  ok = WifiEspNow.addPeer(PEER);
  if (!ok) {
    Serial.println("WifiEspNow.addPeer() failed");
    ESP.restart();
  }
}

void loop() {
  char s[60];

  unsigned long t ;
  t = millis() ;
  InitESPNow() ;
  Serial.println(millis()-t) ;
  while(!flgMsgRecd) ;
  Serial.println("Reached") ;
  flgMsgRecd = false ;
  int len = snprintf(s, sizeof(s), "%s with Mac Id %s at %lu ",msg, WiFi.softAPmacAddress().c_str(), millis());
  WifiEspNow.send(PEER, reinterpret_cast<const uint8_t*>(s), sizeof(s));
  delay(10) ;
  lightsleep() ; 
  Serial.println("Out of sleep") ;
}

void lightsleep() {
 //   Serial.println("light_sleep_enter");
    esp_sleep_enable_timer_wakeup(500000); 
    int ret = esp_light_sleep_start();
 //   Serial.printf("light_sleep: %d\n", ret);
//    delay(400) ;
}