/*
BOTTI LUCA - codice persona: 10687292, matricola: 226358
Politecnico di Milano - Internet of Things - Challenge 1
*/

#include <WiFi.h>
#include <esp_now.h>
#include <esp_sleep.h>
#include <esp_err.h>

// debug active or not, if it is active print some information, otherwise only for error there will be printed something (must to be used with FAST_TIMES = 0)
// 1 active, 0 inactive
#define DEBUG 0

// used to only print times and have a more fair result (must to be used with DEBUG = 0)
// 1 print only times, 0 don't print
#define FAST_TIMES 0

// distance in cm
#define PRESENCE_DISTANCE 50

// if distance is < PRESENCE_DISTANCE
#define OCCUPIED_STRING "OCCUPIED"
// if distance is >= PRESENCE_DISTANCE
#define FREE_STRING "FREE"

// person code 10687292: 92 mod 50 + 5 = 47 sec
#define PERIOD_X (92 % 50 + 5)
#define uS_TO_S_FACTOR 1000000ULL  /* Conversion factor for micro seconds to seconds */
#define TIME_TO_SLEEP_DEFAULT (PERIOD_X * uS_TO_S_FACTOR)

//define half the sound speed in cm/uS. distance = time * sound speed / 2, sound speed used at 20 degree
#define HALF_SOUND_SPEED (0.0343 / 2)

#define PIN_TRIG 18
#define PIN_ECHO 5


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

String data_to_send = "";

// RTC_DATA_ATTR int bootCount = 0; // not working on wokwi so not used

unsigned long start_time = -1;
unsigned long sensor_time = -1;
unsigned long wifi_time_start = -1;


void setupDistanceSensor () {
  pinMode(PIN_TRIG, OUTPUT); // Sets the trigPin as an Output
  pinMode(PIN_ECHO, INPUT); // Sets the echoPin as an Input
  return;
}

float getDistanceCM () {
  // Clears the trigPin
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(PIN_ECHO, HIGH);
  // float distanceCM = duration * HALF_SOUND_SPEED;
  // return distanceCM;
  return duration * HALF_SOUND_SPEED;
}

int setupESPNow () {
  esp_err_t res;

  WiFi.mode(WIFI_STA);

  // Init ESP-NOW 
  res = esp_now_init();
  if (res != ESP_OK) { 
    Serial.println("Error initializing ESP-NOW: " + String(esp_err_to_name(res))); 
    return 1; 
  }
  else if (DEBUG) Serial.println("Initialized ESP-NOW"); 


  // register peer
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  // adding peer 
  res = esp_now_add_peer(&peerInfo);
  if (res != ESP_OK){ 
    Serial.println("Failed to add peer: " + String(esp_err_to_name(res))); 
    return 1; 
  }
  else if (DEBUG) Serial.println("Added peer"); 

  // send callback
  res = esp_now_register_send_cb(OnDataSent);
  if (res != ESP_OK){ 
    Serial.println("Failed to add send callback" + String(esp_err_to_name(res))); 
    return 1; 
  }
  else if (DEBUG) Serial.println("Added send callback"); 
  
  // receive callback
  if (DEBUG) {
    res = esp_now_register_recv_cb(OnDataRecv);
    if (res != ESP_OK){ 
      Serial.println("Failed to add receive callback" + String(esp_err_to_name(res))); 
      return 1; 
    }
    Serial.println("Added receive callback");
  }

  return 0;
}

void sendMessage() {
  wifi_time_start = micros();
  WiFi.mode(WIFI_STA); // activating wifi in station mode
  // WiFi.setTxPower(WIFI_POWER_19_5dBm); not setted because it is the default value and we will save a little bit of time
  // WiFi.setTxPower(WIFI_POWER_2dBm);
  esp_err_t res = esp_now_send(broadcastAddress, (uint8_t*)data_to_send.c_str(), data_to_send.length() + 1);
  // if not in debug mode from here any other 
  if (res != ESP_OK) {
    Serial.println("Failed to send message " + String(esp_err_to_name(res))); 
    ESP.restart();
    return; // not executed
  }
  else if (DEBUG) Serial.println("Sended message: " + data_to_send);
  if (! DEBUG) WiFi.mode(WIFI_OFF); // turning off wifi, needed if because otherwise the receive callback will never be triggered
}

void goToDeepSleep () {
  unsigned long now = micros();
  unsigned long wifi_time = now - wifi_time_start;
  if (DEBUG) Serial.println("Used wireless connection for: " + String(wifi_time) + " us, or " + String((float)wifi_time / uS_TO_S_FACTOR) + " sec.");
  if (FAST_TIMES) Serial.println(wifi_time);
  unsigned long worked = now - start_time;
  if (DEBUG) Serial.println("Worked for: " + String(worked) + " us, or " + String((float)worked / uS_TO_S_FACTOR) + " sec. - DUTY-CYCLE = " + String((float)worked / TIME_TO_SLEEP_DEFAULT * 100) + "%.");
  unsigned long idle_time = worked - wifi_time - sensor_time;
  if (DEBUG) Serial.println("Idle time is: " + String(idle_time) + " us, or " + String((float)idle_time / uS_TO_S_FACTOR) + " sec.");
  if (FAST_TIMES) Serial.println(idle_time);
  unsigned long sleep_time = TIME_TO_SLEEP_DEFAULT - worked;
  esp_sleep_enable_timer_wakeup((uint64_t)sleep_time);
  if (DEBUG) Serial.println("The ESP will go to sleep for " + String(sleep_time) + " us, or " + String((float)sleep_time / uS_TO_S_FACTOR) + " sec.");
  if (FAST_TIMES) Serial.println(sleep_time);
  Serial.flush(); 
  esp_deep_sleep_start();
}

// Sending callback when ack received, or timeout
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  if (status != ESP_NOW_SEND_SUCCESS){
    Serial.println("Send message failed. - Retring");
    sendMessage();
  }
  else {
    if (! FAST_TIMES) Serial.println("Sent message " + data_to_send + " successfully.");
    if (! DEBUG) goToDeepSleep();
  }
}

bool recv_callback_done = false;
unsigned long timeout_recv = -1;
// Receiving callback used only for debug
void OnDataRecv(const uint8_t * mac_addr, const uint8_t *data, int len) {
  char msg[len];
  memcpy (msg, data, len);
  Serial.println("[OTHER MACHINE] - Received message: " + String(msg));
  recv_callback_done = true;
}

void setup() {

  if (DEBUG && FAST_TIMES) {
    Serial.println("Error both DEBUG and FAST_TIMES setted to 1"); 
    ESP.restart();
  }
  

  start_time = micros();

  Serial.begin(115200);

  // SETUP

  // ++bootCount; // not working on wokwi so not used

  // if (bootCount == 1) { //first boot, needed test to see if the setup will remain after the deep sleep in a real device
  setupDistanceSensor();
  if (DEBUG) Serial.println("Setted up sensor"); 

  if (setupESPNow()) {
    Serial.println("Error setting up ESPNOW"); 
    ESP.restart();
  }
  else if (DEBUG) Serial.println("ESP-NOW setted up");
  // }

  // SENSOR

  unsigned long temp = micros();
  if (getDistanceCM() < PRESENCE_DISTANCE)
      data_to_send = OCCUPIED_STRING;
  else
      data_to_send = FREE_STRING;
  sensor_time = micros() - temp;
  if (DEBUG) Serial.println("Used sensor for: " + String(sensor_time) + " us, or " + String((float)sensor_time / uS_TO_S_FACTOR) + " sec.");
  if (FAST_TIMES) Serial.println(sensor_time);
  
  // WIFI
  sendMessage();
  

  // going to deep sleep only if the message is sent correctly - see OnDataSent function

  // used for debug timeout
  if (DEBUG) timeout_recv = micros();
}

void loop() {
  if (! DEBUG || timeout_recv == -1) return;
  if (recv_callback_done || ((micros() - timeout_recv) > 1000)) 
    goToDeepSleep();
  delay (10);
}

/*
Used documentation
esp-now api: https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_now.html
esp-now arduino-esp32: https://espressif-docs.readthedocs-hosted.com/projects/arduino-esp32/en/latest/api/espnow.html
wifi api: https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_wifi.html - used default arduino library for esp32, as seen at lab
wifi arduino-esp32: https://espressif-docs.readthedocs-hosted.com/projects/arduino-esp32/en/latest/api/wifi.html
deep-sleep arduino-esp32: https://espressif-docs.readthedocs-hosted.com/projects/arduino-esp32/en/latest/api/deepsleep.html
dee-sleep api: https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/system/index.html

USEFULL TO KNOW
https://www.youtube.com/watch?v=oz0a7Ur7nko

esp-now distance up to 320m, throughput up to 0.6 Mbps
esp-now LONG-RANGE mode distance up to 500m, throughput up to 0.2 Mbps for activate see https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/network/esp_wifi.html#_CPPv421esp_wifi_set_protocol16wifi_interface_t7uint8_t LR mode protocol_bitmap. Snippet: https://esp32.com/viewtopic.php?t=33291
*/