WIP: TimeManager

host/host_central_mast/lib/TimeManager/NTPException.hpp

+//
+// Created by zoe on 12/19/22.
+//
+
+#ifndef HOST_CENTRAL_MAST_NTPEXCEPTION_HPP
+#define HOST_CENTRAL_MAST_NTPEXCEPTION_HPP
+
+#include <exception>
+#include <string>
+
+class NTPException : public std::exception {
+public:
+    explicit NTPException(const std::string &message) : message(message) {}
+
+    const char *what() const noexcept override {
+        return message.c_str();
+    }
+
+private:
+    std::string message;
+};
+
+#endif //HOST_CENTRAL_MAST_NTPEXCEPTION_HPP

host/host_central_mast/lib/TimeManager/StringToTimeConversionException.hpp

+//
+// Created by zoe on 12/19/22.
+//
+
+#ifndef HOST_CENTRAL_MAST_STRINGTOTIMECONVERSIONEXCEPTION_HPP
+#define HOST_CENTRAL_MAST_STRINGTOTIMECONVERSIONEXCEPTION_HPP
+
+#include <exception>
+#include <string>
+#include "TimeManager.h"
+
+class StringToTimeConversionException : public std::exception {
+public:
+    explicit StringToTimeConversionException(const std::string &message) : message(message) {}
+
+    const char *what() const noexcept override {
+        esp_log_write(ESP_LOG_ERROR, "TimeManager", "Error converting time to epoch: %s",
+                      message.c_str());
+        return message.c_str();
+    }
+
+private:
+    std::string message;
+};
+
+#endif //HOST_CENTRAL_MAST_STRINGTOTIMECONVERSIONEXCEPTION_HPP

host/host_central_mast/lib/TimeManager/TimeManager.cpp

+//
+// Created by zoe on 12/19/22.
+//
+
+#include "TimeManager.h"
+
+
+TimeManager::TimeManager(TinyGsmSim7000 &modem) : modem(modem) {
+
+}
+
+void TimeManager::syncNTP(const std::string &ntpServer) {
+    esp_log_write(ESP_LOG_DEBUG, TAG, "NTP Server Syncing...\n");
+
+    auto error = modem.NTPServerSync(ntpServer.c_str(), timeZone);
+
+    /**
+      According to TinGsmNTP.tpp, the error codes are:
+      case 1: "Network time synchronization is successful";
+      case 61: "Network error";
+      case 62: "DNS resolution error";
+      case 63: "Connection error";
+      case 64: "Service response error";
+      case 65: "Service response timeout";
+      default: "Unknown error: " + String(error);
+     */
+    if (error != 1) {
+        throw NTPException(modem.ShowNTPError(error).c_str());
+    } else {
+        esp_log_write(ESP_LOG_DEBUG, TAG, "NTP: %s", modem.ShowNTPError(error).c_str());
+    }
+}
+
+time_t TimeManager::timeToUnixEpochSeconds(const std::string &time) {
+    //	22/10/27,10:16:20+00
+    struct tm tm{};
+    time_t dateInEpoch = 0;
+
+    std::stringstream stringStream(time);
+
+    try {
+        stringStream >> std::get_time(&tm, "%y/%m/%d,%H:%M:%S%z");
+    } catch (std::exception &e) {
+        throw StringToTimeConversionException(("Error converting time to epoch %s", e.what()));
+    }
+
+    return dateInEpoch;
+}
+
+void TimeManager::writeModemTimeToRTC() {
+    auto gsmDateTimeString = modem.getGSMDateTime(DATE_FULL);
+    esp_log_write(ESP_LOG_DEBUG, TAG, "GSM DateTime: %s\n", gsmDateTimeString.c_str());
+    time_t time = timeToUnixEpochSeconds(gsmDateTimeString.c_str());
+
+    rtc.setTime(time);
+    esp_log_write(ESP_LOG_INFO, TAG, "Time set to EPOCH: %s\n",
+                  String(rtc.getEpoch()).c_str());
+}

host/host_central_mast/lib/TimeManager/TimeManager.h

+//
+// Created by zoe on 12/19/22.
+//
+
+#ifndef HOST_CENTRAL_MAST_TIMEMANAGER_H
+#define HOST_CENTRAL_MAST_TIMEMANAGER_H
+
+#define TINY_GSM_MODEM_SIM7000
+
+#include <TinyGsmClientSIM7000.h>
+#include <string>
+#include "NTPException.hpp"
+#include "StringToTimeConversionException.hpp"
+#include "ESP32Time.h"
+#include <iomanip>
+
+class TimeManager {
+public:
+    explicit TimeManager(TinyGsmSim7000 &modem);
+
+private:
+    constexpr static char *TAG = "GSM";
+    ESP32Time rtc;
+    int timeZone = 0;
+
+    // I would like this to be a const reference but the functions used by the modem are not const
+    TinyGsmSim7000 &modem;
+
+    // sync ntp
+    void syncNTP(const std::string &ntpServer = "time1.uibk.ac.at");
+
+    // convert time to unix epoch seconds
+    time_t timeToUnixEpochSeconds(const std::string &time);
+
+    // write modem time to rtc
+    void writeModemTimeToRTC();
+};
+
+
+#endif //HOST_CENTRAL_MAST_TIMEMANAGER_H

host/host_central_mast/platformio.ini

@@ -13,6 +13,7 @@ platform = espressif32
 board = esp-wrover-kit
 framework = arduino
 monitor_speed = 115200
+lib_ldf_mode = deep
 monitor_port = /dev/ttyACM0
 upload_port = /dev/ttyACM0
 build_flags =

host/host_central_mast/src/main.cpp

+#define TINY_GSM_MODEM_SIM7000
+
+
 #include "FS.h"
 #include "SD.h"
 #include "SPI.h"
@@ -21,7 +24,6 @@ static const std::string TAG_GSM = "GSM";
   PURPOSE: Test functionality
 */
 
-#define TINY_GSM_MODEM_SIM7000
 #define TINY_GSM_RX_BUFFER 1024 // Set RX buffer to 1Kb
 #define SerialAT Serial1
 
@@ -39,7 +41,9 @@ const char gprsPass[] = "";
 #include <TinyGsmClient.h>
 
 #ifdef DUMP_AT_COMMANDS // if enabled it requires the streamDebugger lib
+
 #include <StreamDebugger.h>
+
 StreamDebugger debugger(SerialAT, Serial);
 TinyGsm modem(debugger);
 #else
@@ -61,17 +65,19 @@ TinyGsm modem(SerialAT);
 #define SD_CS 13
 #define LED_PIN 12
 
-enum MessageType{
+enum MessageType {
     dataAck,
     hostChange
 };
-typedef struct response{
-	MessageType type;
-	uint8_t mac[6];
-	long time;
-}response;
+typedef struct response {
+    MessageType type;
+    uint8_t mac[6];
+    long time;
+} response;
+
+#include <TimeManager.h>
 
-uint8_t BROADCAST_MAC[6] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
+uint8_t BROADCAST_MAC[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 esp_now_peer_info_t broadcast = {};
 response announce = {};
 
@@ -83,475 +89,480 @@ TaskHandle_t ESPNOWTask;
 
 static std::queue<String> queue;
 
-void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status)
-{
-	// go to sleep
+void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+    // go to sleep
 }
 
 static char log_print_buffer[512];
 
-int vprintf_into_sd(const char *szFormat, va_list args)
-{
-	String logstring = "[" + rtc.getDateTime() + "] ";
-	logstring += szFormat;
-	// write evaluated format string into buffer
-	int ret = vsnprintf(log_print_buffer, sizeof(log_print_buffer), logstring.c_str(), args);
-
-	String date = rtc.getDate();
-	String filename = "/log_" + date + ".txt";
-
-	// output is now in buffer. write to file.
-	if (ret >= 0) {
-		if (!SD.exists(filename)) {
-			File writeLog = SD.open(filename, FILE_WRITE);
-			if (!writeLog)
-				Serial.println("Couldn't open " + filename + " for writing");
-			delay(50);
-			writeLog.close();
-		}
-
-		File logFile = SD.open(filename, FILE_APPEND);
-
-		// debug output
-		vprintf(logstring.c_str(), args);
-		logFile.write((uint8_t *)log_print_buffer, (size_t)ret);
-		// to be safe in case of crashes: flush the output
-		logFile.flush();
-		logFile.close();
-	}
-	return ret;
+int vprintf_into_sd(const char *szFormat, va_list args) {
+    String logstring = "[" + rtc.getDateTime() + "] ";
+    logstring += szFormat;
+    // write evaluated format string into buffer
+    int ret = vsnprintf(log_print_buffer, sizeof(log_print_buffer), logstring.c_str(), args);
+
+    String date = rtc.getDate();
+    String filename = "/log_" + date + ".txt";
+
+    // output is now in buffer. write to file.
+    if (ret >= 0) {
+        if (!SD.exists(filename)) {
+            File writeLog = SD.open(filename, FILE_WRITE);
+            if (!writeLog)
+                Serial.println("Couldn't open " + filename + " for writing");
+            delay(50);
+            writeLog.close();
+        }
+
+        File logFile = SD.open(filename, FILE_APPEND);
+
+        // debug output
+        vprintf(logstring.c_str(), args);
+        logFile.write((uint8_t *) log_print_buffer, (size_t) ret);
+        // to be safe in case of crashes: flush the output
+        logFile.flush();
+        logFile.close();
+    }
+    return ret;
 }
 
 String getMacAddressAsString(const uint8_t *mac);
+
 DynamicJsonDocument parseReceivedJsonData(char *data);
+
 void saveStringToSDCard(const std::string &dataString);
+
 String documentToLineProtocolString(const DynamicJsonDocument &doc);
+
 void turnOffLEDs();
+
 void setupSDCard();
+
 void syncUTCTimeToRTC();
-void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len)
-{
-	esp_log_write(ESP_LOG_INFO, TAG_ESPNOW.c_str(), "Message recieved\n");
-	// copy received data to a char array
-	char data[len];
-	memcpy(data, incomingData, len);
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Raw received Data: %s\n", data);
-
-	if(!esp_now_is_peer_exist(mac)){
-		esp_now_peer_info_t client = {};
-		memcpy(client.peer_addr, mac, sizeof(uint8_t) * 6);
-		client.encrypt = false;
-		client.channel = 0;
-
-		esp_err_t status = esp_now_add_peer(&client);
-		if(status != ESP_OK){
-			esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Failed to add new Peer: %d", status);
-		}
-	}
-
-	DynamicJsonDocument doc = parseReceivedJsonData(data);
-
-	String macAddress = getMacAddressAsString(mac);
-
-	// add timestamp and mac address
-	// doc["timestamp"] = rtc.getEpoch();
-	doc["clientMac"] = macAddress;
-
-	// serialize json document again
-	std::string dataString{};
-	serializeJson(doc, dataString);
-
-	saveStringToSDCard(dataString);
-
-	String lineData = documentToLineProtocolString(doc);
-
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Line protocol data: %s\n", lineData.c_str());
-
-	xSemaphoreTake(xMutex, portMAX_DELAY);
-	queue.push(lineData);
-	xSemaphoreGive(xMutex);
-
-	response response = {};
-	response.type = dataAck;
-	esp_read_mac(response.mac, ESP_MAC_WIFI_STA);
-	response.time = rtc.getEpoch();
-	esp_err_t success = esp_now_send(mac, (uint8_t*) &response, sizeof(response));
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), 
-		(success == ESP_OK) ? "Response sent\n" : "Failed to respond\n");
+
+void on_data_recv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+    esp_log_write(ESP_LOG_INFO, TAG_ESPNOW.c_str(), "Message recieved\n");
+    // copy received data to a char array
+    char data[len];
+    memcpy(data, incomingData, len);
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Raw received Data: %s\n", data);
+
+    if (!esp_now_is_peer_exist(mac)) {
+        esp_now_peer_info_t client = {};
+        memcpy(client.peer_addr, mac, sizeof(uint8_t) * 6);
+        client.encrypt = false;
+        client.channel = 0;
+
+        esp_err_t status = esp_now_add_peer(&client);
+        if (status != ESP_OK) {
+            esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Failed to add new Peer: %d", status);
+        }
+    }
+
+    DynamicJsonDocument doc = parseReceivedJsonData(data);
+
+    String macAddress = getMacAddressAsString(mac);
+
+    // add timestamp and mac address
+    // doc["timestamp"] = rtc.getEpoch();
+    doc["clientMac"] = macAddress;
+
+    // serialize json document again
+    std::string dataString{};
+    serializeJson(doc, dataString);
+
+    saveStringToSDCard(dataString);
+
+    String lineData = documentToLineProtocolString(doc);
+
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Line protocol data: %s\n", lineData.c_str());
+
+    xSemaphoreTake(xMutex, portMAX_DELAY);
+    queue.push(lineData);
+    xSemaphoreGive(xMutex);
+
+    response response = {};
+    response.type = dataAck;
+    esp_read_mac(response.mac, ESP_MAC_WIFI_STA);
+    response.time = rtc.getEpoch();
+    esp_err_t success = esp_now_send(mac, (uint8_t *) &response, sizeof(response));
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(),
+                  (success == ESP_OK) ? "Response sent\n" : "Failed to respond\n");
 }
 
-String documentToLineProtocolString(const DynamicJsonDocument &doc)
-{
-	String measurementType = doc["measurementType"].as<String>();
-	String sensorName = doc["sensorName"].as<String>();
-	String timestamp = doc["timestamp"].as<String>();
-	String protocol = doc["protocol"].as<String>();
-	String value = doc["value"].as<String>();
-	String channel = doc["channel"].as<String>();
-	String clientMac = doc["clientMac"].as<String>();
-
-	String lineData = sensorName + ",clientMac=" + clientMac + ",protocol=" + protocol + ",channel=" + channel + " "
-	                  + measurementType + "=" + value + " " + timestamp;
-	return lineData;
+String documentToLineProtocolString(const DynamicJsonDocument &doc) {
+    String measurementType = doc["measurementType"].as<String>();
+    String sensorName = doc["sensorName"].as<String>();
+    String timestamp = doc["timestamp"].as<String>();
+    String protocol = doc["protocol"].as<String>();
+    String value = doc["value"].as<String>();
+    String channel = doc["channel"].as<String>();
+    String clientMac = doc["clientMac"].as<String>();
+
+    String lineData =
+            sensorName + ",clientMac=" + clientMac + ",protocol=" + protocol + ",channel=" +
+            channel + " "
+            + measurementType + "=" + value + " " + timestamp;
+    return lineData;
 }
-void saveStringToSDCard(const std::string &dataString)
-{
-	File dataFile = SD.open("/datalog.txt", FILE_APPEND);
-
-	// if the file is available, write to it:
-	if (dataFile) {
-		if (dataString.length() > 0) {
-			dataFile.println(dataString.c_str());
-		}
-		dataFile.close();
-	}
-	// if the file isn't open, pop up an error:
-	else {
-		esp_log_write(ESP_LOG_ERROR, TAG.c_str(), "error opening datalog.txt\n");
-		// TODO: Error handling
-	}
+
+void saveStringToSDCard(const std::string &dataString) {
+    File dataFile = SD.open("/datalog.txt", FILE_APPEND);
+
+    // if the file is available, write to it:
+    if (dataFile) {
+        if (dataString.length() > 0) {
+            dataFile.println(dataString.c_str());
+        }
+        dataFile.close();
+    }
+        // if the file isn't open, pop up an error:
+    else {
+        esp_log_write(ESP_LOG_ERROR, TAG.c_str(), "error opening datalog.txt\n");
+        // TODO: Error handling
+    }
 }
-DynamicJsonDocument parseReceivedJsonData(char *data)
-{
-	DynamicJsonDocument doc(250);
-	auto error = deserializeJson(doc, data);
-	if (error) {
-		esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW.c_str(), "Error while parsing json: %s\n", error.f_str());
-		// TODO error handling
-	}
-	return doc;
+
+DynamicJsonDocument parseReceivedJsonData(char *data) {
+    DynamicJsonDocument doc(250);
+    auto error = deserializeJson(doc, data);
+    if (error) {
+        esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW.c_str(), "Error while parsing json: %s\n",
+                      error.f_str());
+        // TODO error handling
+    }
+    return doc;
 }
-String getMacAddressAsString(const uint8_t *mac)
-{
-	String macAddress;
-	for (int i = 0; i < 6; i++) {
-		macAddress += String(mac[i], HEX);
-	}
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "MAC: %s\n", macAddress.c_str());
-	return macAddress;
+
+String getMacAddressAsString(const uint8_t *mac) {
+    String macAddress;
+    for (int i = 0; i < 6; i++) {
+        macAddress += String(mac[i], HEX);
+    }
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "MAC: %s\n", macAddress.c_str());
+    return macAddress;
 }
 
-[[noreturn]] void ESPNOWReceiveTask(void *parameter)
-{
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "ESPNOWReceiveTask started on core %d\n", xPortGetCoreID());
+[[noreturn]] void ESPNOWReceiveTask(void *parameter) {
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "ESPNOWReceiveTask started on core %d\n",
+                  xPortGetCoreID());
 
-	WiFi.mode(WIFI_STA);
+    WiFi.mode(WIFI_STA);
 
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Initialising ESPNow...\n");
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Initialising ESPNow...\n");
 
-	if (esp_now_init() != ESP_OK) {
-		// initialization failed
-		esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW.c_str(), "Initialising ESPNow FAILED\n");
-		exit(ESP_FAIL);
-	}
+    if (esp_now_init() != ESP_OK) {
+        // initialization failed
+        esp_log_write(ESP_LOG_ERROR, TAG_ESPNOW.c_str(), "Initialising ESPNow FAILED\n");
+        exit(ESP_FAIL);
+    }
 
-	esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Initialising ESPNow SUCCESS\n");
+    esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Initialising ESPNow SUCCESS\n");
 
-	esp_now_register_recv_cb(on_data_recv);
+    esp_now_register_recv_cb(on_data_recv);
 
-	while (true) {
-	}
+    while (true) {
+    }
 }
 
-time_t timeToUnixEpochSeconds(const std::string &time)
-{
-	//	22/10/27,10:16:20+00
-	struct tm tm {};
-	time_t dateInEpoch = 0;
-
-	if (strptime(time.c_str(), "%y/%m/%d,%T+00", &tm)) {
-		time_t curTime;
-		struct tm *timeinfo;
-
-		timeinfo = localtime(&curTime);
-
-		timeinfo->tm_year = tm.tm_year;
-		timeinfo->tm_mon = tm.tm_mon;
-		timeinfo->tm_mday = tm.tm_mday;
-		timeinfo->tm_hour = tm.tm_hour;
-		timeinfo->tm_min = tm.tm_min;
-		timeinfo->tm_sec = tm.tm_sec;
-		timeinfo->tm_isdst = -1;
-
-		dateInEpoch = mktime(timeinfo);
-	}
-	return dateInEpoch;
+time_t timeToUnixEpochSeconds(const std::string &time) {
+    //	22/10/27,10:16:20+00
+    struct tm tm{};
+    time_t dateInEpoch = 0;
+
+    if (strptime(time.c_str(), "%y/%m/%d,%T+00", &tm)) {
+        time_t curTime;
+        struct tm *timeinfo;
+
+        timeinfo = localtime(&curTime);
+
+        timeinfo->tm_year = tm.tm_year;
+        timeinfo->tm_mon = tm.tm_mon;
+        timeinfo->tm_mday = tm.tm_mday;
+        timeinfo->tm_hour = tm.tm_hour;
+        timeinfo->tm_min = tm.tm_min;
+        timeinfo->tm_sec = tm.tm_sec;
+        timeinfo->tm_isdst = -1;
+
+        dateInEpoch = mktime(timeinfo);
+    }
+    return dateInEpoch;
 }
 
-void setup()
-{
-	// Set console baud rate
-	Serial.begin(115200);
-	delay(10);
-	setupSDCard();
-
-	//	https://stackoverflow.com/questions/60442350/arduinos-esp-log-set-vprintf-does-not-work-on-esp32
-	esp_log_set_vprintf(&vprintf_into_sd);
-	esp_log_level_set("*", ESP_LOG_VERBOSE);
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s", WiFi.macAddress().c_str());
-
-	turnOffLEDs();
-
-	xMutex = xSemaphoreCreateMutex();
-
-	delay(1000);
-
-	// create ESPNOWReceiveTask. TODO: Until the UTC time is not synced, this will not add the correct time. If we
-	// TODO: create the task after the time is synced, no messages will be received until synchronization is done
-	xTaskCreatePinnedToCore(ESPNOWReceiveTask,   /* Function to implement the task */
-	                        "ESPNOWReceiveTask", /* Name of the task */
-	                        10000,               /* Stack size in words */
-	                        nullptr,             /* Task input parameter */
-	                        0,                   /* Priority of the task */
-	                        &ESPNOWTask,         /* Task handle. */
-	                        0);                  /* Core where the task should run */
-
-	SerialAT.begin(UART_BAUD, SERIAL_8N1, PIN_RX, PIN_TX);
-
-	// Restart takes quite some time
-	// To skip it, call init() instead of restart()
-	esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "Initializing modem...\n");
-	if (!modem.restart()) {
-		esp_log_write(ESP_LOG_WARN, TAG_GSM.c_str(),
-		              "Failed to restart modem, attempting to continue without restarting\n");
-	}
-
-	syncUTCTimeToRTC();
-
-	broadcast.channel = 0;
-	broadcast.encrypt = false;
-	memcpy(&broadcast.peer_addr, &BROADCAST_MAC, sizeof(BROADCAST_MAC));
-	if(esp_now_add_peer(&broadcast) != ESP_OK){
-		esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to add Broadcast Host");
-	}
-
-	announce.type = hostChange;
-	esp_read_mac(announce.mac, ESP_MAC_WIFI_STA);
-	announce.time = rtc.getEpoch();
-	if(esp_now_send(BROADCAST_MAC, (uint8_t *) &announce, sizeof(announce)) != ESP_OK){
-		esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to announce mac");
-	}
-	else{
-		esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Mac announced!");
-	}
+void setup() {
+    // Set console baud rate
+    Serial.begin(115200);
+    delay(10);
+    setupSDCard();
+
+    //	https://stackoverflow.com/questions/60442350/arduinos-esp-log-set-vprintf-does-not-work-on-esp32
+    esp_log_set_vprintf(&vprintf_into_sd);
+    esp_log_level_set("*", ESP_LOG_VERBOSE);
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s", WiFi.macAddress().c_str());
+
+    turnOffLEDs();
+
+    xMutex = xSemaphoreCreateMutex();
+
+    delay(1000);
+
+    // create ESPNOWReceiveTask. TODO: Until the UTC time is not synced, this will not add the correct time. If we
+    // TODO: create the task after the time is synced, no messages will be received until synchronization is done
+    xTaskCreatePinnedToCore(ESPNOWReceiveTask,   /* Function to implement the task */
+                            "ESPNOWReceiveTask", /* Name of the task */
+                            10000,               /* Stack size in words */
+                            nullptr,             /* Task input parameter */
+                            0,                   /* Priority of the task */
+                            &ESPNOWTask,         /* Task handle. */
+                            0);                  /* Core where the task should run */
+
+    SerialAT.begin(UART_BAUD, SERIAL_8N1, PIN_RX, PIN_TX);
+
+    // Restart takes quite some time
+    // To skip it, call init() instead of restart()
+    esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "Initializing modem...\n");
+    if (!modem.restart()) {
+        esp_log_write(ESP_LOG_WARN, TAG_GSM.c_str(),
+                      "Failed to restart modem, attempting to continue without restarting\n");
+    }
+
+    syncUTCTimeToRTC();
+
+    broadcast.channel = 0;
+    broadcast.encrypt = false;
+    memcpy(&broadcast.peer_addr, &BROADCAST_MAC, sizeof(BROADCAST_MAC));
+    if (esp_now_add_peer(&broadcast) != ESP_OK) {
+        esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to add Broadcast Host");
+    }
+
+    announce.type = hostChange;
+    esp_read_mac(announce.mac, ESP_MAC_WIFI_STA);
+    announce.time = rtc.getEpoch();
+    if (esp_now_send(BROADCAST_MAC, (uint8_t *) &announce, sizeof(announce)) != ESP_OK) {
+        esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to announce mac");
+    } else {
+        esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Mac announced!");
+    }
 }
-void syncUTCTimeToRTC()
-{
-	esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "NTP Server Syncing...\n");
-	modem.NTPServerSync("pool.ntp.org", 0);
-	auto gsmDateTimeString = modem.getGSMDateTime(DATE_FULL);
-	esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "GSM DateTime: %s\n", gsmDateTimeString.c_str());
-	time_t time = timeToUnixEpochSeconds(gsmDateTimeString.c_str());
-	rtc.setTime(time);
-	esp_log_write(ESP_LOG_INFO, TAG_GSM.c_str(), "Time set to EPOCH: %s\n", String(rtc.getEpoch()).c_str());
+
+// waitResponse default is 1000ms
+
+void syncUTCTimeToRTC() {
+    esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "NTP Server Syncing...\n");
+    auto error = modem.NTPServerSync("pool.ntp.org", 0);
+    esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "NTP: %s", modem.ShowNTPError(error).c_str());
+
+    auto gsmDateTimeString = modem.getGSMDateTime(DATE_FULL);
+    esp_log_write(ESP_LOG_DEBUG, TAG_GSM.c_str(), "GSM DateTime: %s\n", gsmDateTimeString.c_str());
+    time_t time = timeToUnixEpochSeconds(gsmDateTimeString.c_str());
+
+    rtc.setTime(time);
+    esp_log_write(ESP_LOG_INFO, TAG_GSM.c_str(), "Time set to EPOCH: %s\n",
+                  String(rtc.getEpoch()).c_str());
 }
-void setupSDCard()
-{
-	SPI.begin(SD_SCLK, SD_MISO, SD_MOSI, SD_CS);
-	if (!SD.begin(SD_CS)) {
-		esp_log_write(ESP_LOG_ERROR, TAG.c_str(), "Card MOUNT FAIL\n");
-		// TODO: Error handling
-	} else {
-		uint32_t cardSize = SD.cardSize() / (1024 * 1024);
-		String sdcardSizeString = "SDCard Size: " + String(cardSize) + "MB";
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", sdcardSizeString.c_str());
-	}
+
+void setupSDCard() {
+    SPI.begin(SD_SCLK, SD_MISO, SD_MOSI, SD_CS);
+    if (!SD.begin(SD_CS)) {
+        esp_log_write(ESP_LOG_ERROR, TAG.c_str(), "Card MOUNT FAIL\n");
+        // TODO: Error handling
+    } else {
+        uint32_t cardSize = SD.cardSize() / (1024 * 1024);
+        String sdcardSizeString = "SDCard Size: " + String(cardSize) + "MB";
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", sdcardSizeString.c_str());
+    }
 }
 
 // I don't think this does anything. Copied from the example
-void turnOffLEDs()
-{ // Set LED OFF
-	pinMode(LED_PIN, OUTPUT);
-	digitalWrite(LED_PIN, HIGH);
-
-	pinMode(PWR_PIN, OUTPUT);
-	digitalWrite(PWR_PIN, HIGH);
-	delay(300);
-	digitalWrite(PWR_PIN, LOW);
+void turnOffLEDs() { // Set LED OFF
+    pinMode(LED_PIN, OUTPUT);
+    digitalWrite(LED_PIN, HIGH);
+
+    pinMode(PWR_PIN, OUTPUT);
+    digitalWrite(PWR_PIN, HIGH);
+    delay(300);
+    digitalWrite(PWR_PIN, LOW);
 }
 
 const String INFLUXDB_TOKEN =
-    "dUh2gbVLv7e3egqocxriDsJQNUacA9qZ5YXsYtdnVAglnHgy4nx-jDVO7nGlSF34BosfnuwnUDaviC7dQeC5RQ==";
+        "dUh2gbVLv7e3egqocxriDsJQNUacA9qZ5YXsYtdnVAglnHgy4nx-jDVO7nGlSF34BosfnuwnUDaviC7dQeC5RQ==";
 
 struct RequestInformation {
-	String method;
-	String host;
-	String path;
-	String body;
+    String method;
+    String host;
+    String path;
+    String body;
 };
 
-String buildRequest(const RequestInformation &requestInformation)
-{
-	String request = "";
-	request += requestInformation.method + " " + requestInformation.path + " HTTP/1.1\r\n";
-	request += "Host: " + requestInformation.host + "\r\n";
-	request += "Authorization: Token " + INFLUXDB_TOKEN + "\r\n";
-	request += "User-Agent: ESP32\r\n";
-	request += "Content-Type: text/plain\r\n";
-	request += "Content-Length: " + String(requestInformation.body.length()) + "\r\n";
-	request += "\r\n";
-	request += requestInformation.body;
-	return request;
+String buildRequest(const RequestInformation &requestInformation) {
+    String request = "";
+    request += requestInformation.method + " " + requestInformation.path + " HTTP/1.1\r\n";
+    request += "Host: " + requestInformation.host + "\r\n";
+    request += "Authorization: Token " + INFLUXDB_TOKEN + "\r\n";
+    request += "User-Agent: ESP32\r\n";
+    request += "Content-Type: text/plain\r\n";
+    request += "Content-Length: " + String(requestInformation.body.length()) + "\r\n";
+    request += "\r\n";
+    request += requestInformation.body;
+    return request;
 }
 
-void loop()
-{
-
-	// Restart takes quite some time
-	// To skip it, call init() instead of restart()
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Initializing modem...\n");
-	if (!modem.init()) {
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(),
-		              "Failed to restart modem, attempting to continue without restarting\n");
-	}
-
-	announce.time = rtc.getEpoch();
-	if(esp_now_send(BROADCAST_MAC, (uint8_t *) &announce, sizeof(announce)) != ESP_OK){
-		esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to announce mac\n");
-	}
-	else{
-		esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Mac announced!\n");
-	}
-
-	String name = modem.getModemName();
-	delay(500);
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Modem Name %s\n", name.c_str());
-
-	String modemInfo = modem.getModemInfo();
-	delay(500);
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Modem Info: %s\n", modemInfo.c_str());
-
-	// Set SIM7000G GPIO4 LOW ,turn off GPS power
-	// CMD:AT+SGPIO=0,4,1,0
-	// Only in version 20200415 is there a function to control GPS power
-	modem.sendAT("+SGPIO=0,4,1,0");
-	if (modem.waitResponse(10000L) != 1) {
-		DBG(" SGPIO=0,4,1,0 false ");
-	}
-
-	modem.sendAT("+CFUN=0 ");
-	if (modem.waitResponse(10000L) != 1) {
-		DBG(" +CFUN=0  false ");
-	}
-	delay(200);
-
-	/*
-	  2 Automatic
-	  13 GSM only
-	  38 LTE only
-	  51 GSM and LTE only
-	* * * */
-	String res;
-	res = modem.setNetworkMode(38);
-	if (res != "1") {
-		DBG("setNetworkMode  false ");
-		return;
-	}
-	delay(200);
-
-	/*
-	  1 CAT-M
-	  2 NB-Iot
-	  3 CAT-M and NB-IoT
-	* * */
-	//	res = modem.setPreferredMode(1);
-	//	if (res != "1") {
-	//
-	//		DBG("setPreferredMode  false ");
-	//		return;
-	//	}
-	delay(200);
-
-	/*AT+CBANDCFG=<mode>,<band>[,<band>…]
-	 * <mode> "CAT-M"   "NB-IOT"
-	 * <band>  The value of <band> must is in the band list of getting from  AT+CBANDCFG=?
-	 * For example, my SIM card carrier "NB-iot" supports B8.  I will configure +CBANDCFG= "Nb-iot ",8
-	 */
-	modem.sendAT("+CBANDCFG=\"CAT-M\",8 ");
-	if (modem.waitResponse(10000L) != 1) {
-		DBG(" +CBANDCFG=\"NB-IOT\" ");
-	}
-	delay(200);
-
-	modem.sendAT("+CFUN=1 ");
-	if (modem.waitResponse(10000L) != 1) {
-		DBG(" +CFUN=1  false ");
-	}
-	delay(200);
-
-	//	modem.disableGPS();
-	delay(200);
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", String(modem.getSignalQuality()).c_str());
-	delay(200);
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Trying to connect to network\n");
-	modem.gprsConnect(apn, gprsUser, gprsPass);
-	delay(200);
-	syncUTCTimeToRTC();
-
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Waiting for network...\n");
-	if (!modem.isNetworkConnected()) {
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Network not connected\n");
-		return;
-	} else {
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Network connected\n");
-		delay(200);
-
-		// quality
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", String(modem.getSignalQuality()).c_str());
-		// make a http post request
-		String url = "influxdb.qe-forte.uibk.ac.at";
-		String path = "/api/v2/write?org=QE&bucket=esp32test&precision=s";
-		Serial.print("Connecting to ");
-		esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", url.c_str());
-		// Use WiFiClient class to create TCP connections
-
-		while (!queue.empty()) {
-
-			xSemaphoreTake(xMutex, portMAX_DELAY);
-			String lineData = queue.front();
-			queue.pop();
-			xSemaphoreGive(xMutex);
-
-			RequestInformation requestInformation{.method = "POST", .host = url, .path = path, .body = lineData};
-
-			//"sensorName":"DRS26","timestamp":1666872216,"protocol":"I2C","value":0,"channel":0,"measurementType":"CIRCUMFERENCE_INCREMENT"
-
-			String request = buildRequest(requestInformation);
-			esp_log_write(ESP_LOG_VERBOSE, TAG.c_str(), "request: %s\n", request.c_str());
-
-			TinyGsmClient client{modem};
-			const int httpPort = 80;
-			if (!client.connect(url.c_str(), httpPort)) {
-				esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "connection failed\n");
-				return;
-			}
-
-			client.print(request);
-
-			// print response
-			while (client.connected()) {
-				String line = client.readStringUntil('\n');
-				if (line == "\r") {
-					esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "headers received\n");
-					break;
-				}
-			}
-			client.stop();
-			delay(1000);
-		}
-		DBG("Network connected");
-	}
+void loop() {
+
+    // Restart takes quite some time
+    // To skip it, call init() instead of restart()
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Initializing modem...\n");
+    if (!modem.init()) {
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(),
+                      "Failed to restart modem, attempting to continue without restarting\n");
+    }
+
+    announce.time = rtc.getEpoch();
+    if (esp_now_send(BROADCAST_MAC, (uint8_t *) &announce, sizeof(announce)) != ESP_OK) {
+        esp_log_write(ESP_LOG_WARN, TAG_ESPNOW.c_str(), "Failed to announce mac\n");
+    } else {
+        esp_log_write(ESP_LOG_DEBUG, TAG_ESPNOW.c_str(), "Mac announced!\n");
+    }
+
+    String name = modem.getModemName();
+    delay(500);
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Modem Name %s\n", name.c_str());
+
+    String modemInfo = modem.getModemInfo();
+    delay(500);
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Modem Info: %s\n", modemInfo.c_str());
+
+    // Set SIM7000G GPIO4 LOW ,turn off GPS power
+    // CMD:AT+SGPIO=0,4,1,0
+    // Only in version 20200415 is there a function to control GPS power
+    modem.sendAT("+SGPIO=0,4,1,0");
+    if (modem.waitResponse(10000L) != 1) {
+        DBG(" SGPIO=0,4,1,0 false ");
+    }
+
+    modem.sendAT("+CFUN=0 ");
+    if (modem.waitResponse(10000L) != 1) {
+        DBG(" +CFUN=0  false ");
+    }
+    delay(200);
+
+    /*
+      2 Automatic
+      13 GSM only
+      38 LTE only
+      51 GSM and LTE only
+    * * * */
+    String res;
+    res = modem.setNetworkMode(38);
+    if (res != "1") {
+        DBG("setNetworkMode  false ");
+        return;
+    }
+    delay(200);
+
+    /*
+      1 CAT-M
+      2 NB-Iot
+      3 CAT-M and NB-IoT
+    * * */
+    //	res = modem.setPreferredMode(1);
+    //	if (res != "1") {
+    //
+    //		DBG("setPreferredMode  false ");
+    //		return;
+    //	}
+    delay(200);
+
+    /*AT+CBANDCFG=<mode>,<band>[,<band>…]
+     * <mode> "CAT-M"   "NB-IOT"
+     * <band>  The value of <band> must is in the band list of getting from  AT+CBANDCFG=?
+     * For example, my SIM card carrier "NB-iot" supports B8.  I will configure +CBANDCFG= "Nb-iot ",8
+     */
+    modem.sendAT("+CBANDCFG=\"CAT-M\",8 ");
+    if (modem.waitResponse(10000L) != 1) {
+        DBG(" +CBANDCFG=\"NB-IOT\" ");
+    }
+    delay(200);
+
+    modem.sendAT("+CFUN=1 ");
+    if (modem.waitResponse(10000L) != 1) {
+        DBG(" +CFUN=1  false ");
+    }
+    delay(200);
+
+    //	modem.disableGPS();
+    delay(200);
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", String(modem.getSignalQuality()).c_str());
+    delay(200);
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Trying to connect to network\n");
+    modem.gprsConnect(apn, gprsUser, gprsPass);
+    delay(200);
+    syncUTCTimeToRTC();
+
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Waiting for network...\n");
+    if (!modem.isNetworkConnected()) {
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Network not connected\n");
+        return;
+    } else {
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Network connected\n");
+        delay(200);
+
+        // quality
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", String(modem.getSignalQuality()).c_str());
+        // make a http post request
+        String url = "influxdb.qe-forte.uibk.ac.at";
+        String path = "/api/v2/write?org=QE&bucket=esp32test&precision=s";
+        Serial.print("Connecting to ");
+        esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "%s\n", url.c_str());
+        // Use WiFiClient class to create TCP connections
+
+        while (!queue.empty()) {
+
+            xSemaphoreTake(xMutex, portMAX_DELAY);
+            String lineData = queue.front();
+            queue.pop();
+            xSemaphoreGive(xMutex);
+
+            RequestInformation requestInformation{.method = "POST", .host = url, .path = path, .body = lineData};
+
+            //"sensorName":"DRS26","timestamp":1666872216,"protocol":"I2C","value":0,"channel":0,"measurementType":"CIRCUMFERENCE_INCREMENT"
+
+            String request = buildRequest(requestInformation);
+            esp_log_write(ESP_LOG_VERBOSE, TAG.c_str(), "request: %s\n", request.c_str());
+
+            TinyGsmClient client{modem};
+            const int httpPort = 80;
+            if (!client.connect(url.c_str(), httpPort)) {
+                esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "connection failed\n");
+                return;
+            }
+
+            client.print(request);
+
+            // print response
+            while (client.connected()) {
+                String line = client.readStringUntil('\n');
+                if (line == "\r") {
+                    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "headers received\n");
+                    break;
+                }
+            }
+            client.stop();
+            delay(1000);
+        }
+        DBG("Network connected");
+    }
 
 #if TINY_GSM_POWERDOWN
-	// Try to power-off (modem may decide to restart automatically)
-	// To turn off modem completely, please use Reset/Enable pins
-	//	modem.sendAT("+CPOWD=1");
-	//	if (modem.waitResponse(10000L) != 1) {
-	//		DBG("+CPOWD=1");
-	//	}
-	//	modem.poweroff();
-	esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Poweroff.");
+    // Try to power-off (modem may decide to restart automatically)
+    // To turn off modem completely, please use Reset/Enable pins
+    //	modem.sendAT("+CPOWD=1");
+    //	if (modem.waitResponse(10000L) != 1) {
+    //		DBG("+CPOWD=1");
+    //	}
+    //	modem.poweroff();
+    esp_log_write(ESP_LOG_DEBUG, TAG.c_str(), "Poweroff.");
 #endif
 
-	delay(1000);
+    delay(1000);
 }