#include <Wire.h>
#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include "TempHumidSensor.h"
#include "LightSensor.h"

#define TEMP_HUMID_SENSOR_ADDR    0x22
#define LIGHT_SENSOR_ADDR         0x23

// GPIO pins
#define SOIL_MOISTURE_SENSOR  1
#define I2C_SCL               39
#define I2C_SDA               40

// Define structure for sensor data
struct SensorData {
    float humidity;
    float lightIntensity;
};

// Function breadcrumbs
void readAirSensor(void *pvParameters);       // task to read air sensor
void readLightSensor(void *pvParameters);     // task to read light sensor
void readSoilSensor(void *pvParameters);      // task to read soil sensor
void publishMqttDataTask(void *pvParameters);  // task for mqtt data publish logic
void publishMqttData(const SensorData& data); // function to publish mqtt data
void countSeconds(); // function to count seconds passed

// Variables to count seconds
unsigned long previousMillis = 0; // Stores the last time the message was printed
const long interval = 1000; // Interval for 1 second (1000 milliseconds)
int seconds = 0; // Counter for seconds

// Sensor classes
LightSensor light_sensor(LIGHT_SENSOR_ADDR);
TempHumidSensor air_sensor(TEMP_HUMID_SENSOR_ADDR);

// Queue for sensor data
QueueHandle_t sensorDataQueue;

// NIM: 2602-10-48-12
const int X = 10;
const int Y = 48;
const int Z = 12;

// Last read variable for each metrics 
float lastTemperature = 0.0;
float lastHumidity = 0.0;
float lastLightIntensity = 0.0;
float lastSoilMoisture = 0.0;

// WiFi & MQTT configuration
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "test.mosquitto.org";
WiFiClient espClient;
PubSubClient mqttClient(espClient);

// external interference simulation
// int humidityReads = 0;
// int reconnects = 0;

void setup() {
  // setup serial and i2c
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL);

  // set up wifi
  WiFi.begin(ssid, password);
  Serial.print("Connecting to "); Serial.print(ssid);
  // Connect to WiFi
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nConnected to WiFi");

  mqttClient.setServer(mqtt_server, 1883); // set mqtt client server reference to defined mqtt server

  previousMillis = millis(); // update so it the countSeconds() function start after connecting to WiFi.

  // initialize values (this could be replaced by moving the delay of each read task to the bottom)
  lastTemperature = air_sensor.readTemperature();
  lastHumidity = air_sensor.readHumidity();
  lastLightIntensity = light_sensor.readIntensity();
  lastSoilMoisture = ( 5 - (analogRead(SOIL_MOISTURE_SENSOR) * (5.0 / 4095)) ) * 20;

  sensorDataQueue = xQueueCreate(10, sizeof(SensorData));   // create a new queue for sensor data
  
  // tasks to read (and send mqtt data) from sensor
  xTaskCreatePinnedToCore(readAirSensor, "Read Air Sensor Task", 4000, NULL, 1, NULL, 1);         // Run on core 1
  xTaskCreatePinnedToCore(readLightSensor, "Read Light Sensor Task", 4000, NULL, 1, NULL, 1);     // Run on core 1
  xTaskCreatePinnedToCore(readSoilSensor, "Read Soil Sensor Task", 4000, NULL, 1, NULL, 1);       // Run on core 1
  xTaskCreatePinnedToCore(publishMqttDataTask, "Publish to MQTT Task", 4000, NULL, 1, NULL, 1);   // Run on core 1
}

void loop() {
    countSeconds();
}

void readAirSensor(void *pvParameters) {
  while(true) {
    vTaskDelay(X*1000 / portTICK_PERIOD_MS);

    // get values from sensor
    lastTemperature = air_sensor.readTemperature();
    lastHumidity = air_sensor.readHumidity();

    // prints values from sensor
    Serial.println();
    Serial.printf("[Air Sensor] Temperature: %.2f°C\n", lastTemperature);
    Serial.printf("[Air Sensor] Humidity: %.2f%% RH\n", lastHumidity);
    
    // Create new sensor data structure with current readings
    SensorData data = {
        .humidity = lastHumidity,
        .lightIntensity = lastLightIntensity
    };

    // Print queue state before operations
    Serial.printf("[Queue] Available spaces before: %d\n", uxQueueSpacesAvailable(sensorDataQueue));

    // Check if queue is full
    if (uxQueueSpacesAvailable(sensorDataQueue) == 0) {
        SensorData oldData;
        // Remove the oldest item
        if (xQueueReceive(sensorDataQueue, &oldData, 0) == pdTRUE) {
            Serial.printf("[Queue] Removed oldest data - Humidity: %.2f, Light: %.2f\n", 
                         oldData.humidity, oldData.lightIntensity);
        } else {
            Serial.println("[Queue] Failed to remove old value even though queue appears full");
        }
    }

    // Now send the new value
    if(xQueueSend(sensorDataQueue, &data, 0) != pdPASS) {
        Serial.println("[Queue] Failed to send sensor data to queue");
    } else {
        Serial.printf("[Queue] Added new data - Humidity: %.2f, Light: %.2f\n", 
                     data.humidity, data.lightIntensity);
    }
    
    // Print queue state after operations
    Serial.printf("[Queue] Available spaces after: %d\n", uxQueueSpacesAvailable(sensorDataQueue));
    
    // humidity read test
    // humidityReads++;
    // if(humidityReads == 1) {
    //   WiFi.disconnect();
    // }
  }
}

void readLightSensor(void *pvParameters) {
  while(true) {
    vTaskDelay(Y*1000 / portTICK_PERIOD_MS);
    
    // get values from sensor
    lastLightIntensity = light_sensor.readIntensity();

    // prints values from sensor
    Serial.println();
    Serial.printf("[Light Sensor] Light Intensity: %.2f Lux\n", lastLightIntensity);
    Serial.println();
  }
}

void readSoilSensor(void *pvParameters) {
  while(true) {
    vTaskDelay(Z*1000 / portTICK_PERIOD_MS);
    
    // get analog value and then convert to moisture value
    int raw = analogRead(SOIL_MOISTURE_SENSOR);   // returns 0-4096 for voltage 0-5 V
    float voltage = raw * (5.0 / 4095);           // convert from 12 bit data to voltage value
    lastSoilMoisture = (5.0 - voltage) * 20;      // (100%-0%)/(5V-0V) = 20% per voltage

    Serial.println();
    Serial.printf("[Soil Sensor] Soil Moisture: %.2f%%\n", lastSoilMoisture);
    Serial.println();
  }
}

void publishMqttDataTask(void *pvParameters) {
  SensorData data;
  while(true) {
    vTaskDelay(10/ portTICK_PERIOD_MS); // prevents tight loop
    mqttClient.loop();

    // wait for sensor data in queue
    if(xQueueReceive(sensorDataQueue, &data, portMAX_DELAY) == pdPASS) {
      if(data.humidity < 80) continue;

      publishMqttData(data);
    }
  }
}

void reconnect() {
  // Loop until we're reconnected
  while (!mqttClient.connected()) {
    Serial.println("[MQTT] Attempting MQTT connection...");
    
    // Attempt to connect
    if (mqttClient.connect("2602104812")) {
      Serial.println("[MQTT] Connected");
    } else {
      Serial.print("[MQTT] failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" trying again in 3 seconds");
      
      // Wait 3 seconds before retrying
      vTaskDelay(3000 / portTICK_PERIOD_MS);

      // external interference test
      // reconnects++;
      // if(reconnects == 2) {
      //   WiFi.reconnect();
      // }
    }
  }
}

void publishMqttData(const SensorData& data) {
  if (!mqttClient.connected()) {
    reconnect(); // Call reconnect if not connected
  }

  // Create the payload as a formatted JSON string
  String payload = String("{\n") +
                   String("  \"device_id\": \"2602104812\",\n") +
                   String("  \"message\": \"Humidity is high, it will rain soon!\",\n") +
                   String("  \"air_humidity\": ") + String(data.humidity) + String(",\n") +
                   String("  \"light_intensity_lux\": ") + String(data.lightIntensity) + String("\n") +
                   String("}");

  // Publish to the specified topic
  mqttClient.publish("binus-comp-eng-uts/sensor/data/2602104812", payload.c_str());
  Serial.println("[MQTT] Data published to test.mosquitto.org/sensor/data/2602104812\n");
}

void countSeconds() {
    unsigned long currentMillis = millis(); // Get the current time

    // Check if a second has passed
    if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis; // Save the last time the message was printed
        seconds++; // Increment the seconds counter

        Serial.print("[INFO] Time passed: ");
        Serial.print(seconds); // Print the current seconds count
        Serial.print(" seconds");

        if(seconds % X == 0) Serial.printf(" | X: %i seconds mark", X);
        if(seconds % Y == 0) Serial.printf(" | Y: %i seconds mark", Y);
        if(seconds % Z == 0) Serial.printf(" | Z: %i seconds mark", Z);

        Serial.println();
    }
}