#include "preProc.h"
#include <ArduinoJson.h>

void mqtt_callback(char* topic, uint8_t* payload, unsigned int length) {
  String message = "";
  JsonDocument doc;
  DeserializationError error = deserializeJson(doc, payload, length);

  for (unsigned int i = 0; i < length; i++) {
      message += (char)payload[i];
  }
  String topicStr = String(topic);
  
  tone(BUZ_PIN, 200);
}

void setup_wifi() {
  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFi connected!");
  Serial.print("IP Address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  while (!mqttClient.connected()) {
    Serial.print("Connecting to MQTT...");
    if (mqttClient.connect("WokwiClient")) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttClient.state());
      delay(2000);
    }
  }
}

// === SETUP ===
void setup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, GPS_RX, GPS_TX);
  setup_wifi();

  mqttClient.setServer(mqtt_server, mqtt_port);
  mqttClient.setCallback(mqtt_callback);
  mqttClient.setBufferSize(512);

  tmp = new Tmp(TMPIN);
  mpu = new MPU(SDA0, SCL0);
  rtc = new TimeRTC(SDA1, SCL1);
  cd  = new CustomSensor<int>(CADENCE_PIN, MAX_RPM);
  ill = new CustomSensor<double>(ILLUMINANCE_PIN, MAX_LUX);

  pinMode(LIGHT_PIN, OUTPUT);
  pinMode(LIGHT_BATTERY_PIN, OUTPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);
  pinMode(BUZ_PIN, OUTPUT);

  cur = digitalRead(BTN_PIN);

  Serial.println("Setup completato.");
    dht.begin();
}

void loop() {

  if (!mqttClient.connected()) reconnect();
  mqttClient.loop();
  static unsigned long lastMsg = 0;
  if (millis() - lastMsg > 1000) {
    cur = digitalRead(BTN_PIN);
    if (cur == LOW && oldValue == HIGH) {
      active = !active;
      Serial.print("Stato attivo: ");
      Serial.println(active ? "ON" : "OFF");
    }
    oldValue = cur;

    int batLvl = analogRead(BATTERY_PIN);

    DateTime now = rtc->GetRTC().now();
    String dateStr = String(now.year()) + "-" + String(now.month()) + "-" + String(now.day()) + " " +
                    String(now.hour()) + ":" + String(now.minute()) + ":" + String(now.second());
    tmp->Loop();
    
    cd->Loop(analogRead(cd->GetPid()));
    ill->Loop(analogRead(ill->GetPid()));

    if(ill->GetValue() < 60000) digitalWrite(LIGHT_PIN, LOW);
    else digitalWrite(LIGHT_PIN, HIGH);

    if(batLvl > 10) digitalWrite(LIGHT_BATTERY_PIN, LOW);
    else digitalWrite(LIGHT_BATTERY_PIN, HIGH);

    mpu->Loop();
    long rssi = WiFi.RSSI();
    while (Serial1.available() > 0) {
      if (gps.encode(Serial1.read())) {
          String json = String("{") +
                  "\"id\": \"" + String(idDevice) + "\", " +
                  "\"date\": \"" + dateStr + "\", " 
                  "\"tmp\": " + String(tmp->GetTmp()) + ", " +
                  "\"rssi\": " + String(rssi) + ", " +
                  "\"accx\": " + String(mpu->GETACC().acceleration.x) + ", " +
                  "\"accy\": " + String(mpu->GETACC().acceleration.y) + ", " +
                  "\"girx\": " + String(mpu->GETGYR().gyro.x) + ", " +
                  "\"giry\": " + String(mpu->GETGYR().gyro.y) + ", " +
                  "\"gpsx\": " + String(gps.location.lng(), 6) + ", " +
                  "\"gpsy\": " + String(gps.location.lat(), 6) + ", " +
                  "\"speed\": " + String(gps.speed.kmph()) + ", " +
                  "\"battlvl\": " + String(batLvl) + ", " +
                  "\"lux\": " + String(ill->GetValue()) + ", " +
                  "\"rpm\": " + String(cd->GetValue())  + ", " +
                  "\"umidity\": " + String(dht.readHumidity()) + 
                  "}";
          lclBuf.push_back(json);
      }
    }
    bool success = false;
    if (mqttClient.connected()) {
      if(lclBuf.size() > 0) {
        success = mqttClient.publish(mqtt_topic, lclBuf.back().c_str(), true);      
        if (success) lclBuf.pop_back();
      }
      mqttClient.subscribe(("readBikeCommand/" + String(idDevice)).c_str());
    }
    
    lastMsg = millis();
  }
}