#include <WiFi.h>
#include <esp_now.h>
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"

#define PIN_TRIG 4
#define PIN_ECHO 2
#define uS_TO_S_FACTOR 1000000
#define TIME_TO_SLEEP  30 //Slep for 10 seconds


RTC_DATA_ATTR int bootCount = 0;

uint8_t broadcastAddress[] = {0x8C, 0xAA, 0xB5, 0x84, 0xFB, 0x90};

esp_now_peer_info_t peerInfo;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status){
  // funzione di callback per quando invio al sink
  // Stampa esito dell'invio e vai a nanna
  //Serial.print("Send status: ");
}

void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len){
  // funzione di callback per quando ricevo dal sink (niente)
  // Non ricevo mai niente in teoria
  unsigned long timestamp = micros() / uS_TO_S_FACTOR; // Ottieni il timestamp in millisecondi
  Serial.print("Timestamp: ");
  Serial.println(timestamp);
  Serial.print("Msg recvd: ");
  char rs[len];
  memcpy(rs, data, len);
  Serial.println(String(rs));
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("SVEGLIO");
  //WiFi stuff
  WiFi.mode(WIFI_STA);
  esp_now_init();
  //send callback
  esp_now_register_send_cb(OnDataSent);
  //receive callback
  esp_now_register_recv_cb(OnDataRecv);

  // Peer Registration
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  // Add peer (Is possible to register multiple peers)
  esp_now_add_peer(&peerInfo);

  //Proximity stuff
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);



  String message;

  // Start a new measurement:
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Read the result:
  int duration = pulseIn(PIN_ECHO, HIGH);
  int distanza = duration / 58;
  if(distanza < 50){
    //OCCUPIED
    message = "OCCUPIED";
  } else {
    //FREE
    message = "FREE";
  }
  WiFi.setTxPower(WIFI_POWER_19_5dBm);
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);
  delay(1000);
  WiFi.mode(WIFI_OFF);

  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);
  //Serial.flush(); 
  Serial.println("ora dormo");
  esp_deep_sleep_start();
  Serial.println("sto dormendo");
}


void loop() {
}