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

/* ---------------- WIFI ---------------- */
#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
#define WIFI_CHANNEL 6

/* ---------------- MQTT ---------------- */
const char *MQTT_BROKER_ADRESS = "broker.mqtt.cool";
const uint16_t MQTT_PORT = 1883;
const char *MQTT_CLIENT_NAME = "nazalopez";
const char *MQTT_USER = "a";
const char *MQTT_PASS = "b";

/* ---------------- LEDS ---------------- */
#define led0 22
#define led1 19
#define led2 18
#define led3 17
#define led4 16

int leds[] = {22, 19, 18, 17, 16};

/* ---------------- ESTADOS ---------------- */
#define ESTADO_0 0
#define ESTADO_1 1
#define ESTADO_2 2
#define ESTADO_3 3
#define ESTADO_4 4

volatile int state = ESTADO_0;

/* ---------------- FLAGS ---------------- */
volatile bool buttonA_released = false;
volatile bool buttonC_released = false;
volatile bool timerBlink_interrupted = false;

/* ---------------- RTOS ---------------- */
TaskHandle_t HandleTaskFSM;

/* ---------------- TIMER ---------------- */
hw_timer_t *timerBlink = NULL;

/* ---------------- MQTT CLIENT ---------------- */
WiFiClient espClient;
PubSubClient mqttClient(espClient);

/* ================================================= */
/* =================== WIFI ======================== */
/* ================================================= */
void ConnectWiFi() {
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD, WIFI_CHANNEL);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Connected!");
}

/* ================================================= */
/* =================== MQTT ======================== */
/* ================================================= */
void OnMqttReceived(char *topic, byte *payload, unsigned int length) {
  String message = "";
  for (unsigned int i = 0; i < length; i++) {
    message += (char)payload[i];
  }

  Serial.print("MQTT [");
  Serial.print(topic);
  Serial.print("] -> ");
  Serial.println(message);

  if (strcmp(topic, "/estado_siguiente") == 0) {
    if (message == "AGUA") {
      buttonA_released = true;
    }
    else if (message == "C") {
      buttonC_released = true;
    }
  }
}

void InitMqtt() {
  mqttClient.setServer(MQTT_BROKER_ADRESS, MQTT_PORT);
  mqttClient.setCallback(OnMqttReceived);
}

void ConnectMqtt() {
  while (!mqttClient.connected()) {
    Serial.print("Connecting to MQTT...");
    if (mqttClient.connect(MQTT_CLIENT_NAME, MQTT_USER, MQTT_PASS)) {
      mqttClient.subscribe("/estado_siguiente");
      Serial.println(" OK");
    } else {
      Serial.print(" failed, rc=");
      Serial.println(mqttClient.state());
      delay(2000);
    }
  }
}

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

/* ================================================= */
/* =================== LEDS ======================== */
/* ================================================= */
void actualizarLed(int state) {
  if (state == ESTADO_0) {
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  } else if (state == ESTADO_1) {
    digitalWrite(led0, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  } else if (state == ESTADO_2) {
    digitalWrite(led0, LOW);
    digitalWrite(led1, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  } else if (state == ESTADO_3) {
    digitalWrite(led0, LOW);
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led4, LOW);
  } else if (state == ESTADO_4) {
    digitalWrite(led0, LOW);
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
  }
}

/* ================================================= */
/* =================== TIMER ISR =================== */
/* ================================================= */
void IRAM_ATTR timerBlink_int() {
  timerBlink_interrupted = true;
}

/* ================================================= */
/* =================== FSM ========================= */
/* ================================================= */
void FSM(void *parameter) {
  TickType_t xLastWakeTime;
  const TickType_t xPeriod = pdMS_TO_TICKS(500);  
  xLastWakeTime = xTaskGetTickCount();
  while (true) {
    HandleMqtt();
    if (buttonA_released) {
      buttonA_released = false;
      state = (state == ESTADO_0) ? ESTADO_4 : state - 1;

      actualizarLed(state);

      char buffer[4];
      sprintf(buffer, "%d", state);
      mqttClient.publish("/estado_actual", buffer);
    }

    if (buttonC_released) {
      buttonC_released = false;
      state = (state == ESTADO_4) ? ESTADO_0 : state + 1;

      actualizarLed(state);

      char buffer[4];
      sprintf(buffer, "%d", state);
      mqttClient.publish("/estado_actual", buffer);
    }

    if (timerBlink_interrupted) {
      timerBlink_interrupted = false;
      Serial.println("kk");
      digitalWrite(leds[state], !digitalRead(leds[state]));
    }
    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

/* ================================================= */
/* =================== SETUP ======================= */
/* ================================================= */
void setup() {
  Serial.begin(115200);

  ConnectWiFi();
  InitMqtt();

  pinMode(led0, OUTPUT);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);

  actualizarLed(state);

  timerBlink = timerBegin(1000000);
  timerAttachInterrupt(timerBlink, &timerBlink_int);
  timerAlarm(timerBlink, 2000000, true, 0); // 2s
  timerStart(timerBlink);

  xTaskCreatePinnedToCore(
    FSM,
    "FSM",
    4096,
    NULL,
    1,
    &HandleTaskFSM,
    0
  );
/*
  xTaskCreatePinnedToCore(
    TaskMqtt,
    "MQTT",
    4096,
    NULL,
    2,
    &HandleTaskMqtt,
    0
  );

  */
}

/* ================================================= */
/* =================== LOOP ======================== */
/* ================================================= */
void loop() {
  delay(10);
}