#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PubSubClient.h>
#include <uRTCLib.h>
#include <WiFi.h>

const char *WIFI_SSID = "Wokwi-GUEST";
const char *WIFI_PWD = "";

const char *MQTT_SERVER = "broker.hivemq.com";
const uint16_t MQTT_PORT = 1883;
// bool mqttConnOK = false;
// const char* mqttClientName = "mqttClient_" "IoT";
// const char* DPTopicPub = "IOT/AHU/DP";

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);

Adafruit_MPU6050 mpu;
uRTCLib rtc(0x68);


char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};

void CallbackMqtt(char* topic, byte* payload, unsigned int length)
{
  Serial.print("Callback - ");
  Serial.print("Message:");
  for (int i = 0; i < length; i++)
  {
    Serial.print((char)payload[i]);
  }
  Serial.println();
}

void SetupMqtt()
{
  mqttClient.setServer(MQTT_SERVER, MQTT_PORT);
  // set the callback function
  mqttClient.setCallback(CallbackMqtt);
}

void ConnectToMqtt()
{
  Serial.println("Connecting to MQTT Broker...");
  while (!mqttClient.connected())
  {
    char clientId[100] = "\0";
    sprintf(clientId, "ESP32Client-%04X", random(0xffff));
    Serial.println(clientId);

    if (mqttClient.connect(clientId))
    {
      Serial.println("Connected to MQTT broker.");
      // subscribe to topic
      mqttClient.subscribe("/sensors/commands");
    }
  }
}

void ConnectToWiFi()
{
  Serial.print("Connecting to WiFi ");
  Serial.println(WIFI_SSID);

  WiFi.begin(WIFI_SSID, WIFI_PWD, 6);
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nConnected to ");
  Serial.println(WIFI_SSID);
}

void SetupMpu6050()
{
  if (!mpu.begin(0x69))
  {
    Serial.println("Failed to find MPU6050 chip");
    while (1)
    {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange())
  {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth())
  {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }
}

void SetupRtc()
{
  URTCLIB_WIRE.begin();
}


void setup()
{
  Serial.begin(115200);
  Serial.println("Setup begin");

  ConnectToWiFi();
  SetupMqtt();

  SetupMpu6050();
  SetupRtc();

  Serial.println("Setup end");
}

const size_t BUFFER_SIZE = 100;
const char *BUFFER_FORMAT = "%1.3f";
char buffer[BUFFER_SIZE + 1];  // Extra one for NULL terminator.

void loop()
{
  if (!mqttClient.connected())
  {
    ConnectToMqtt();
  }
  mqttClient.loop();

  static uint64_t last_time;
  uint64_t now = millis();
  if (now - last_time > 1 * 1000)
  {
    // Get new sensor events with the readings.
    sensors_event_t a, g, mpu_temp;
    mpu.getEvent(&a, &g, &mpu_temp);
    rtc.refresh();

    snprintf(buffer, BUFFER_SIZE, BUFFER_FORMAT, a.acceleration.x);
    mqttClient.publish("/sensors/mpu_ax", buffer);
    Serial.print("Acceleration X: ");
    Serial.print(buffer);

    snprintf(buffer, BUFFER_SIZE, BUFFER_FORMAT, a.acceleration.y);
    mqttClient.publish("/sensors/mpu_ay", buffer);
    Serial.print(", Y: ");
    Serial.print(buffer);
    Serial.print(", Z: ");

    snprintf(buffer, BUFFER_SIZE, BUFFER_FORMAT, a.acceleration.z);
    mqttClient.publish("/sensors/mpu_az", buffer);
    Serial.print(buffer);
    Serial.println(" m/s^2");

    snprintf(buffer, BUFFER_SIZE, "%02d/%02d/%02d (%s) %02d:%02d:%02d", rtc.year(), rtc.month(), rtc.day(), daysOfTheWeek[rtc.dayOfWeek() - 1], rtc.hour(), rtc.minute(), rtc.second());
    mqttClient.publish("/sensors/rtc_time", buffer);
    Serial.print("Current Date & Time: ");
    Serial.println(buffer);

    last_time = now;
  }
}