#include <WiFi.h>
#include <PubSubClient.h>
#include <Bounce2.h>

const char* ssid = "Verbruggen";
const char* password = "D93579084A";
const char* mqttServer = "10.1.13.173";
const int mqttPort = 1883;
const char* mqttTopic = "test";

const int gpioPins[] = {25, 26, 27, 14};  // GPIO pins to check for shorting
const unsigned int numPins = sizeof(gpioPins) / sizeof(gpioPins[0]);
const unsigned long debounceDelay = 50;  // Debounce delay in milliseconds

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);

TaskHandle_t mqttTaskHandle = NULL;

Bounce debouncers[numPins];

void setupWiFi() {
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");
}

void callback(char* topic, byte* payload, unsigned int length) {
  // This function is called when a message is received from the MQTT server
  // Add your desired code here to handle the received message
}

void reconnectMQTT() {
  while (!mqttClient.connected()) {
    Serial.println("Connecting to MQTT server...");
    if (mqttClient.connect("ESP32Client")) {
      Serial.println("Connected to MQTT server");
      mqttClient.subscribe(mqttTopic);
    } else {
      Serial.print("MQTT connection failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" Retrying in 5 seconds...");
      delay(5000);
    }
  }
}

void mqttTask(void* parameter) {
  for (;;) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification

    if (mqttClient.connected()) {
      for (unsigned int i = 0; i < numPins; i++) {
        if (debouncers[i].fell()) {
          String message = "Pin shorted: " + String(gpioPins[i]);
          mqttClient.publish(mqttTopic, message.c_str());
          Serial.println("Message sent to MQTT server");
        }
      }
    }
  }
}

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

  setupWiFi();
  mqttClient.setServer(mqttServer, mqttPort);
  mqttClient.setCallback(callback);

  for (unsigned int i = 0; i < numPins; i++) {
    pinMode(gpioPins[i], INPUT_PULLUP);
    debouncers[i].attach(gpioPins[i]);
    debouncers[i].interval(debounceDelay);
  }

  xTaskCreatePinnedToCore(
      mqttTask,        // Task function
      "mqttTask",      // Task name
      4096,            // Stack size (bytes)
      NULL,            // Task parameter
      1,               // Task priority
      &mqttTaskHandle, // Task handle
      1                // Task core (0 or 1)
  );
}

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

  for (unsigned int i = 0; i < numPins; i++) {
    debouncers[i].update();
  }

  bool anyPinFell = false;

  for (unsigned int i = 0; i < numPins; i++) {
    if (debouncers[i].fell()) {
      anyPinFell = true;
      break;
    }
  }

  if (anyPinFell) {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    vTaskNotifyGiveFromISR(mqttTaskHandle, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken == pdTRUE) {
      portYIELD_FROM_ISR();
    }
  }

  delay(10); // Adjust the delay as per your requirements
}