#include <stdio.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_wifi.h"
#include "mqtt_client.h"
#include "driver/gpio.h"
#include "nvs_flash.h"

static const char *TAG = "MAIN";
static const char *TAG_WIFI = "WIFI";
static const char *TAG_MQTT = "MQTT";

esp_netif_t* sta_netif;
esp_event_handler_instance_t event_wifi;

esp_mqtt_client_handle_t client;

int PIN_GND[] = {25, 33, 27, 26, 12};
int PIN_LED[] = {17, 13, 4, 16, 18};
int PIN_LOCK[] = {19, 23, 5, 21, 22};

static void wifi_eventhandler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START)
    {
        ESP_LOGI(TAG_WIFI, "Started");
        esp_wifi_connect();
    } 
    else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED)
    {
        ESP_LOGI(TAG_WIFI, "Disconnected");
        esp_wifi_connect();
    }
    else if(event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_CONNECTED)
    {
        ESP_LOGI(TAG_WIFI, "Connected");
    }
    else if(event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP)
    {
        ESP_LOGI(TAG_WIFI, "IPv4 recived");
    }
}

void init()
{
    gpio_config_t config;
    
    config.pin_bit_mask = 0;
    config.pull_down_en = GPIO_PULLDOWN_DISABLE;
    config.pull_up_en = GPIO_PULLUP_DISABLE;
    config.mode = GPIO_MODE_OUTPUT;
    config.intr_type = GPIO_INTR_DISABLE;

    config.pin_bit_mask |= 1ULL << 14;
    for(int i = 0; i < 5; i++)
    {
        config.pin_bit_mask |= 1ULL << PIN_LED[i];
        config.pin_bit_mask |= 1ULL << PIN_LOCK[i];
        config.pin_bit_mask |= 1ULL << PIN_GND[i];
    }
    gpio_config(&config);

    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_LED[i], 0);
        gpio_set_level(PIN_LOCK[i], 0);
        gpio_set_level(PIN_GND[i], 0);
    }
    gpio_set_level(14, 0);
}

void trigger(int id)
{
    ESP_LOGI(TAG, "Trigger: %d", id);

    if(id >= 25)
    {
        ESP_LOGW(TAG, "Invalid ID: %d", id);
        return;
    }

    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 1);
    }

    gpio_set_level(PIN_LED[id/5], 1);
    gpio_set_level(PIN_GND[id%5], 0);
    vTaskDelay(pdMS_TO_TICKS(250));
    
    gpio_set_level(PIN_LED[id/5], 0);
    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 0);
    }
    gpio_set_level(PIN_LOCK[id/5], 1);
    gpio_set_level(PIN_GND[id%5], 1);

    vTaskDelay(pdMS_TO_TICKS(100));

    gpio_set_level(PIN_LOCK[id/5], 0);
    gpio_set_level(PIN_GND[id%5], 0);

    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 1);
    }

    gpio_set_level(PIN_LED[id/5], 1);
    gpio_set_level(PIN_GND[id%5], 0);
    vTaskDelay(pdMS_TO_TICKS(500));

    gpio_set_level(PIN_LED[id/5], 0);
    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 0);
    }
}

void identify(int id)
{
    ESP_LOGI(TAG, "Identify: %d", id);

    if(id >= 25)
    {
        ESP_LOGW(TAG, "Invalid ID: %d", id);
        return;
    }

    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 1);
    }

    for(int i = 0; i < 3; i++)
    {
        gpio_set_level(PIN_LED[id/5], 1);
        gpio_set_level(PIN_GND[id%5], 0);
        vTaskDelay(pdMS_TO_TICKS(250));

        gpio_set_level(PIN_LED[id/5], 0);
        gpio_set_level(PIN_GND[id%5], 1);
        vTaskDelay(pdMS_TO_TICKS(250));
    }

    gpio_set_level(PIN_LED[id/5], 0);
    for(int i = 0; i < 5; i++)
    {
        gpio_set_level(PIN_GND[i], 0);
    }
}


static void mqtt_eventhandler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data)
{
    esp_mqtt_event_handle_t event = event_data;
    esp_mqtt_client_handle_t client = event->client;

    char topic_subscribe1[] = "fablock/00000/+/trigger\0";
    snprintf(topic_subscribe1, strlen(topic_subscribe1)+1, "fablock/%05d/+/trigger", CONFIG_FABLOCK_ID);

    char topic_subscribe2[] = "fablock/00000/+/identify\0";
    snprintf(topic_subscribe2, strlen(topic_subscribe2)+1, "fablock/%05d/+/identify", CONFIG_FABLOCK_ID);

    char* topic_trigger = "fablock/00000/00000/trigger";
    char* topic_identify = "fablock/00000/00000/identify";

    char str_id[6] = {0};

    switch ((esp_mqtt_event_id_t)event_id) 
    {
        case MQTT_EVENT_CONNECTED:
            ESP_LOGI(TAG_MQTT, "Connected");

            ESP_LOGI(TAG_MQTT, "Subscribe: %s", topic_subscribe1);
            esp_mqtt_client_subscribe(client, topic_subscribe1, 1);

            ESP_LOGI(TAG_MQTT, "Subscribe: %s", topic_subscribe2);
            esp_mqtt_client_subscribe(client, topic_subscribe2, 1);
            break;
        case MQTT_EVENT_DISCONNECTED:
            ESP_LOGI(TAG_MQTT, "Disconnected");
            break;

        case MQTT_EVENT_DATA:
            ESP_LOGI(TAG_MQTT, "Data recieved");

            ESP_LOGI(TAG_MQTT, "TOPIC=%.*s", event->topic_len, event->topic);

            if(strlen(topic_trigger) == event->topic_len)
            {
                if(!strncmp(event->topic + strlen("fablock/00000/00000"), "/trigger", 8))
                {
                    strncpy(str_id, event->topic + 14, 5);
                    int id = atoi(str_id);

                    trigger(id);
                }
            }

            if(strlen(topic_identify) == event->topic_len)
            {
                if(!strncmp(event->topic + strlen("fablock/00000/00000"), "/identify", 9))
                {
                    strncpy(str_id, event->topic + 14, 5);
                    int id = atoi(str_id);

                    identify(id);
                }
            }
            break;
        default:
            break;
    }
}

void app_main(void)
{
    if(CONFIG_FABLOCK_ID > 99999 || CONFIG_FABLOCK_ID < 0)
    {
        ESP_LOGE(TAG, "Invalid FabLock ID");
    }

    init();
    ESP_LOGI(TAG, "GPIO initied");

    nvs_flash_init();
    esp_event_loop_create_default();

    esp_netif_init();

    sta_netif = esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    esp_wifi_init(&cfg);

    esp_event_handler_instance_register(WIFI_EVENT,
                                        ESP_EVENT_ANY_ID,
                                        &wifi_eventhandler,
                                        NULL,
                                        &event_wifi);

    esp_event_handler_instance_register(IP_EVENT,
                                        ESP_EVENT_ANY_ID,
                                        &wifi_eventhandler,
                                        NULL,
                                        &event_wifi);

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = CONFIG_FABLOCK_WIFI_SSID,
            .password = CONFIG_FABLOCK_WIFI_PSK,
            .threshold.authmode = WIFI_AUTH_WPA2_PSK,
        },
    };

    esp_wifi_set_mode(WIFI_MODE_STA);
    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
    esp_wifi_start();
    ESP_LOGI(TAG, "WIFI started");

    esp_mqtt_client_config_t mqtt_cfg = 
        {
            .broker.address.uri = "mqtt://" CONFIG_FABLOCK_MQTT_HOST,
            .broker.address.port = 1883,
            .credentials.username = CONFIG_FABLOCK_MQTT_USERNAME,
            .credentials.authentication.password = CONFIG_FABLOCK_MQTT_PASSWORD
        };

        client = esp_mqtt_client_init(&mqtt_cfg);
        esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID,  mqtt_eventhandler, NULL);

    esp_mqtt_client_start(client);
    ESP_LOGI(TAG, "MQTT started");
}