#include <WiFi.h>
#include <PubSubClient.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <RTClib.h>
#include <cstdlib> // For random function
#include <string>

LiquidCrystal_I2C lcd1(0x27, 16, 2);
LiquidCrystal_I2C lcd2(0x28, 20, 4);
LiquidCrystal_I2C lcd3(0x29, 20, 4);
LiquidCrystal_I2C lcd4(0x30, 20, 4);

RTC_DS1307 rtc;

const char* mqtt_server = "0.tcp.ap.ngrok.io";
const int mqtt_port = 10002;
const char* mqtt_user = "";
const char* mqtt_password = "";

String vessel = "ahts";
String t_fms1 = "fms/bfccf2bf4384df20422a7f2189443e8e";
String t_fms2 = "fms/e65b523f49666e452b6baae6124d6557";
String t_gps  = "gps/e120925088da11e731ca4923b3021406";

const char* status;
float latitude = 0.0;
float longitude = 0.0;
float height = 0.0;
float latIncrement = 0.0001;
float longIncrement = 0.0001;
float heightIncrement = 0.001;

float consumptionPerSecond1 = 0.0;
double totalConsumption1 = 0.0;
float consumptionPerSecond2 = 0.0;
double totalConsumption2 = 0.0;

WiFiClient espClient;
PubSubClient mqtt_client(espClient);

char* timenow() {
  DateTime now = rtc.now();
  unsigned long micros_since_start = micros();
  char* timestamp = new char[34];
  sprintf(timestamp, "%04d-%02d-%02d %02d:%02d:%02d.%06lu", now.year(), now.month(), now.day(), now.hour(), now.minute(), now.second(), micros_since_start);
  return timestamp;
}

void fmStatic(LiquidCrystal_I2C & lcd) {
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("Name  :");

  lcd.setCursor(0, 1);
  lcd.print("Temp  :");
  lcd.setCursor(19, 1);
  lcd.print("C");

  lcd.setCursor(0, 2);
  lcd.print("Flow  :");
  lcd.setCursor(17, 2);
  lcd.print("L/m");

  lcd.setCursor(0, 3);
  lcd.print("Total :");
  lcd.setCursor(19, 3);
  lcd.print("L");
}

void fmDynamic(LiquidCrystal_I2C & lcd, String name, String temp, float flow, float total) {
  lcd.setCursor(8, 0);
  lcd.print(name);

  lcd.setCursor(8, 1);
  lcd.print(temp);

  lcd.setCursor(8, 2);
  lcd.print(flow);

  lcd.setCursor(8, 3);
  lcd.print(total);
}

void callSensor(boolean isConnect, char* timestamp) {
  Serial.println(timestamp);

  if (isConnect == true) {
    Serial.println("Connected       ");
  } else {
    Serial.println("Not Connected!  ");
  }

  // FMS
  consumptionPerSecond1 = random(12.000001, 21.0000001);
  totalConsumption1 += consumptionPerSecond1;
  float temperature1 = random(60.000001, 65.000001);
  std::string stemp1 = std::to_string(temperature1);
  bool isReturn1 = false;
  bool isTemp1 = true;

  consumptionPerSecond2 = random(0, 11);
  totalConsumption2 += consumptionPerSecond2;
  float temperature2 = random(50, 55);
  std::string stemp2 = "null";
  bool isReturn2 = false;
  bool isTemp2 = true;

  fmDynamic(lcd2, "FMS 1 Inlet", stemp1.c_str(), consumptionPerSecond1, totalConsumption1);
  fmDynamic(lcd3, "FMS 2 Return", stemp2.c_str(), consumptionPerSecond2, totalConsumption2);
  
  static float position[3];
  float latChange = (rand() % 3 - 1) * latIncrement;
  float longChange = (rand() % 3 - 1) * longIncrement;
  float heightChange = (rand() % 3 - 1) * heightIncrement;

  latitude += latChange;
  longitude += longChange;
  height += heightChange;

  position[0] = constrain(latitude, -90.0, 90.0);
  position[1] = constrain(longitude, -180.0, 180.0);
  position[2] = constrain(height, 0.0, 500.0);

  // MQTT
  char fms1[200], fms2[200], formatted_gps[100];

  // Value_IsReturn_Temp_IsTemp_TimeRTC_IsBackup
  sprintf(fms1, "%f_%d_%s_%d_%s_%d", totalConsumption1, 0, stemp1.c_str(), 1, timestamp, 0);
  sprintf(fms2, "%f_%d_%s_%d_%s_%d", totalConsumption2, 1, stemp2.c_str(), 0, timestamp, 0);

  // Long_Lat_Height_isHeight_TimeRTC_IsBackup
  sprintf(formatted_gps, "%.6f_%.6f_%.6f_%s", position[0], position[1], position[2], timestamp);

  mqtt_client.publish(t_fms1.c_str(), fms1);
  mqtt_client.publish(t_fms2.c_str(), fms2);
  mqtt_client.publish(t_gps.c_str(), formatted_gps);

  lcd4.setCursor(6, 1);
  lcd4.print(position[0], 6);

  lcd4.setCursor(6, 2);
  lcd4.print(position[1], 6);

  lcd4.setCursor(6, 3);
  lcd4.print(position[2], 6);

  if(isConnect == true){
    status = "1";
  }else{
    status = "0";
  }
}

void setup_wifi() {
  Serial.print("Connecting to WiFi");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println("\nWifi Connected!");
}

void setup() {
  Serial.begin(115200);

  setup_wifi();
  mqtt_client.setServer(mqtt_server, mqtt_port);

  Wire.begin();
  if (!rtc.begin()) {
    Serial.println("RTC No Connected");
    while (1);
  }

  lcd1.init();
  lcd1.backlight();
  lcd1.setCursor(0, 0);
  lcd1.print("Connecting ...");
  lcd1.setCursor(0, 1);
  lcd1.print("Current Time ...");

  lcd4.init();
  lcd4.backlight();
  lcd4.setCursor(0, 0);
  lcd4.print("GPS Coordinates");
  lcd4.setCursor(0, 1);
  lcd4.print("Lon :");
  lcd4.setCursor(0, 2);
  lcd4.print("Lat :");
  lcd4.setCursor(0, 3);
  lcd4.print("Hgt :");

  fmStatic(lcd2);
  fmStatic(lcd3);

  lcd1.setCursor(0, 1);
  lcd1.print(timenow());

  if (mqtt_client.connect(vessel.c_str())) {
    lcd1.setCursor(0, 0);
    lcd1.print("Connected       ");
    Serial.println("MQTT Connected");
    callSensor(true, timenow());
  }
}

void loop(){
  char* timestamp = timenow();
  lcd1.setCursor(0, 1);
  lcd1.print(timestamp);
  lcd1.setCursor(0, 0);

  // If Disconnected
  while (!mqtt_client.connected()) {
    lcd1.setCursor(0, 0);
    lcd1.print("Reconnecting... ");
    Serial.println("MQTT Reconnecting");
    callSensor(false, timestamp);
    if (mqtt_client.connect(vessel.c_str())) {
      Serial.println("MQTT Connected");
      callSensor(true, timestamp);
    }
  }
  
  lcd1.print("Connected       ");
  callSensor(true, timestamp);
  mqtt_client.loop();
}