#include <Arduino.h>
#include <WiFi.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <MQTT.h>

// WiFi Configuration
const char* ssid = "your_SSID";
const char* password = "your_PASSWORD";

// MQTT Configuration
const char* mqtt_server = "your_MQTT_broker_IP";
const char* mqtt_username = "camille";
const char* mqtt_password = "camille";
const char* client_id = "your_client_id";

// LED Pins
const int led1Pin = 2;
const int led2Pin = 4;
const int led3Pin = 5;

// Button and Potentiometer Pins
const int button1Pin = 12;
const int button2Pin = 13;
const int potentiometerPin = 27;  // Example pin, adjust to your setup

// Task Handles
TaskHandle_t wifiTask;
TaskHandle_t lightControlTask;
TaskHandle_t controlLoopTask;

// Global Variables
bool light1Status = false;
bool light2Status = false;
int analogFeedbackValue = 0;

// MQTT Client
WiFiClient espClient;
MQTTClient mqttClient;

// WiFi Task
void wifiTaskFunction(void* pvParameters) {
  // Connect to WiFi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }

  Serial.println("Connected to WiFi");
  setupMQTT();  // Set up MQTT connection
  xTaskCreatePinnedToCore(lightControlTaskFunction, "lightControlTask", 10000, NULL, 1, &lightControlTask, 1);
  xTaskCreatePinnedToCore(controlLoopTaskFunction, "controlLoopTask", 10000, NULL, 2, &controlLoopTask, 1);
  vTaskDelete(wifiTask);
}

// Light Control Task
void lightControlTaskFunction(void* pvParameters) {
  while (1) {
    // Handle MQTT messages for light control
    mqttClient.loop();

    // Update LEDs based on light status
    digitalWrite(led1Pin, light1Status ? HIGH : LOW);
    digitalWrite(led2Pin, light2Status ? HIGH : LOW);

    // Publish current light status to MQTT
    mqttClient.publish("light/status", String("1:" + String(light1Status) + ",2:" + String(light2Status)).c_str());

    // Check button states to update light statuses
    if (digitalRead(button1Pin) == HIGH) {
      light1Status = !light1Status;
      delay(500);  // debounce
    }

    if (digitalRead(button2Pin) == HIGH) {
      light2Status = !light2Status;
      delay(500);  // debounce
    }

    // Delay for stability
    delay(100);
  }
}

// Control Loop Task
void controlLoopTaskFunction(void* pvParameters) {
  while (1) {
    // Read analog feedback value
    analogFeedbackValue = analogRead(potentiometerPin);

    // Perform closed-loop control based on feedback value
    // Update light3Status accordingly

    // Publish analog feedback value to MQTT
    mqttClient.publish("analog/feedback", String(analogFeedbackValue).c_str());

    // Delay for stability
    delay(1000);
  }
}

// MQTT Callback function for handling incoming messages
void mqttCallback(char* topic, byte* payload, unsigned int length) {
  String incomingTopic = String(topic);
  String payloadStr = "";
  for (int i = 0; i < length; i++) {
    payloadStr += (char)payload[i];
  }

  if (incomingTopic.equals("light/control")) {
    // Update light1Status and light2Status based on MQTT message
    light1Status = payloadStr.substring(0, 1).toInt();
    light2Status = payloadStr.substring(3, 4).toInt();
  } else if (incomingTopic.equals("analog/feedback")) {
    // Update analogFeedbackValue based on MQTT message
    analogFeedbackValue = payloadStr.toInt();
  }
}

// Setup MQTT connection
void setupMQTT() {
  mqttClient.begin(mqtt_server, 1883, espClient);
  mqttClient.onMessage(mqttCallback);
  while (!mqttClient.connect(client_id, mqtt_username, mqtt_password)) {
    Serial.println("Failed to connect to MQTT. Retrying...");
    delay(1000);
  }
  Serial.println("Connected to MQTT");
  mqttClient.subscribe("light/control");
  mqttClient.subscribe("analog/feedback");
}

void setup() {
  // Initialize Serial, pinMode for LEDs, and configure buttons
  Serial.begin(115200);
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(led3Pin, OUTPUT);
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);

  // Create WiFi task
  xTaskCreatePinnedToCore(wifiTaskFunction, "wifiTask", 10000, NULL, 0, &wifiTask, 1);
}

void loop() {
  // Reconnect to WiFi if disconnected
  if (WiFi.status() != WL_CONNECTED) {
    vTaskResume(wifiTask);
  }

  // Handle MQTT messages for general tasks
  mqttClient.loop();
  
  // Your additional loop logic if any
}