/*
  Forum:https://forum.arduino.cc/t/esp32-board-freezes-randomly-when-rapid-serial-printing/1199915
  Wokwi: https://wokwi.com/projects/384078474682031105

   ESP32Time library (example sketch)
   https://github.com/fbiego/ESP32Time/blob/main/examples/esp32_time/esp32_time.ino


*/

#include "WiFi.h"
#include "time.h"
#include "Wire.h"
#include <Adafruit_MPU6050.h>
#include "ESP32Time.h"

#define WOKWI

#ifdef WOKWI
// Wokwi specific data for SDA/SCL interface
// and access to simulated WiFi
constexpr byte SDA_1 {15};
constexpr byte SCL_1 {16};
constexpr unsigned long I2C_FREQ {400000};
TwoWire I2C_1 = TwoWire(0);
const char* ssid       = "Wokwi-GUEST";
const char* password   = "";
#else
// Usual declaration
const char* ssid       = "SSID";
const char* password   = "PASSWORD";
#endif

Adafruit_MPU6050 mpu;

const char* ntpServer = "pool.ntp.org";
const long  gmtOffset_sec = 3600;
const int   daylightOffset_sec = 3600;

ESP32Time rtc(gmtOffset_sec);  // offset in seconds GMT+1

#include "Freenove_WS2812_Lib_for_ESP32.h"
#define LED_PIN 1
Freenove_ESP32_WS2812 led = Freenove_ESP32_WS2812(1, LED_PIN, 0, TYPE_GRB);


long timer = 0;
struct tm timeinfo;
time_t now;

sensors_event_t a, g, temp;

void setLED(uint8_t r, uint8_t g, uint8_t b) {
  led.setLedColorData(0, r, g, b);
  led.show();
}

void setup() {
  // ******** LED for debugging *********
  led.begin();
  led.setBrightness(10);
  setLED(60, 60, 0); // yellow
  // **************************

  Serial.begin(115200);

  Serial.println("Serial started");

  // ********* Sync time *********
  //connect to WiFi
  WiFi.begin(ssid, password, 6);
  Serial.println("WiFi begin done");
  while (WiFi.status() != WL_CONNECTED);
  //init and get the time
  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
  if (!getLocalTime(&timeinfo)) {
    Serial.println("Failed to obtain time");
    return;
  }
  //disconnect WiFi as it's no longer needed
  WiFi.disconnect(true);
  WiFi.mode(WIFI_OFF);

  Serial.println("WiFi off, time received");
  // **************************

  // ********* mpu6050 *********
#ifdef WOKWI
  I2C_1.begin(SDA_1, SCL_1, I2C_FREQ);
  if (!mpu.begin(0x68, &I2C_1)) {
#else
  Wire1.begin();
  if (!mpu.begin(0x68, &Wire1)) {
#endif
    Serial.println("MPU6050 Chip wurde nicht gefunden");
    return;
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  // **************************

  setLED(0, 60, 0); // green
  refreshLocalTimeEverySeconds(0);  // Get the NTP time immediately
}

void loop() {
  refreshLocalTimeEverySeconds(10);  // Get the NTP time every 10 seconds only
  measureEverymSec(10);              // Measure every 10 ms
  heartBeat();                       //
}

void refreshLocalTimeEverySeconds(int seconds) {
  unsigned long mSeconds = seconds * 1000UL;
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= mSeconds) {
    lastTime = millis();
    if (!getLocalTime(&timeinfo)) {
      Serial.println("Failed to obtain time");
    } else {
      Serial.println("************ Local Time refreshed! **************");
      rtc.setTimeStruct(timeinfo);
    }
  }
}

void measureEverymSec(unsigned long msecs) {
  static unsigned long lastMeasurement = 0;
  constexpr int msgLen {120};
  char message[msgLen];
  if (millis() - lastMeasurement >= msecs) {
    lastMeasurement = millis();
    mpu.getEvent(&a, &g, &temp);
    int cnt = snprintf(message, msgLen, "%20d, %20d, %2.6f, %2.6f, %2.6f\n",
                       rtc.getEpoch(),
                       lastMeasurement,
                       a.acceleration.x,
                       a.acceleration.y,
                       a.acceleration.z);
    // This skips sending if not enough space in TX buffer
    if (Serial.availableForWrite() >= cnt) {
      Serial.print(message);
    }
  }
}

void heartBeat() {
  static byte state = HIGH;
  static unsigned long lastChange = 0;
  if (millis() - lastChange > 500) {
    lastChange = millis();
    state = !state;
    if (state) {
      setLED(255, 0, 0);
    } else {
      setLED(0, 255, 0);
    }
  }
}