#include <qthing.h>

#include "io.h"
#include "mqtt.h"
#include "util.h"

#include <algorithm>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"

#include "esp_log.h"
#include "driver/gpio.h"

#define IO_TAG "IO"

#define ESP_INTR_FLAG_DEFAULT 0

#define DEBOUNCE_MILLIS 10
const TickType_t debounce_delay_ticks = std::max(DEBOUNCE_MILLIS / portTICK_PERIOD_MS, (TickType_t) 1);

struct input_t {
    TickType_t last_activation_millis = 0;
    gpio_num_t gpio_num;
    bool last_level = 0;
    std::function<void()> on_falling_edge;
    std::function<void()> on_rising_edge;
};

bool interrupts_enabled = false;
xQueueHandle gpio_event_queue = NULL;

void check_input_level(input_t* input) {
    bool level = gpio_get_level(input->gpio_num);

    if (level == input->last_level) {
        return;
    }

    ESP_LOGI(IO_TAG, "GPIO level changed: gpio %d, level %d", input->gpio_num, level);
    if (level) {
        if (input->on_rising_edge != NULL) {
            input->on_rising_edge();
        }
    }
    else {
        if (input->on_falling_edge != NULL) {
            input->on_falling_edge();
        }
    }

    input->last_level = level;
}

void interrupt_queue_task(void* arg) {
    input_t* input;
    while (true) {
        if (xQueueReceive(gpio_event_queue, &input, portMAX_DELAY)) {
            // immediately react to changing edge
            // value might be incorrect due to contact bouncing, but that's ok, it is read again after debouncing
            check_input_level(input);

            // wait for debouncing to finish
            bool wait;
            do {
                vTaskDelay(debounce_delay_ticks);
                TickType_t now = xTaskGetTickCount() * portTICK_PERIOD_MS;
                wait = now < input->last_activation_millis + DEBOUNCE_MILLIS;
            } while(wait);

            // handle input again after debouncing
            check_input_level(input);
        }
    }
}

void enable_interrupts() {
    if (!interrupts_enabled) {
        ESP_ERROR_CHECK(gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT));
        gpio_event_queue = xQueueCreate(10, sizeof(uint32_t));
        xTaskCreate(interrupt_queue_task, "interrupt_queue_task", 2048, NULL, 10, NULL);

        interrupts_enabled = true;
        ESP_LOGI(IO_TAG, "Interrupts enabled");

        ESP_LOGI(IO_TAG, "portTICK_PERIOD_MS = %d", portTICK_PERIOD_MS);
    }
}

void IRAM_ATTR gpio_isr_handler(void* arg) {
    input_t* input = (input_t*) arg;
    TickType_t now = xTaskGetTickCountFromISR() * portTICK_PERIOD_MS;
    if (now >= input->last_activation_millis + DEBOUNCE_MILLIS) {
        xQueueSendFromISR(gpio_event_queue, &input, NULL);
    }
    else {
        input->last_activation_millis = now;
    }
    input->last_activation_millis = now;
}

void add_digital_input(gpio_num_t gpio_num, pull_resistor_t pull_resistor, std::function<void()> on_falling_edge, std::function<void()> on_rising_edge) {
    enable_interrupts();

    input_t* input = new input_t;
    input->gpio_num = gpio_num;
    input->on_falling_edge = on_falling_edge;
    input->on_rising_edge = on_rising_edge;
    if (pull_resistor == pullup) {
        input->last_level = 1;
    }
    else {
        input->last_level = 0;
    }

    gpio_pad_select_gpio(gpio_num);
    ESP_ERROR_CHECK(gpio_set_direction(gpio_num, GPIO_MODE_INPUT));
    if (pull_resistor == pullup) {
        ESP_ERROR_CHECK(gpio_pullup_en(gpio_num));
    }
    else if (pull_resistor == pulldown) {
        ESP_ERROR_CHECK(gpio_pulldown_en(gpio_num));
    }
    ESP_ERROR_CHECK(gpio_set_intr_type(gpio_num, GPIO_INTR_ANYEDGE));

    ESP_ERROR_CHECK(gpio_isr_handler_add(gpio_num, gpio_isr_handler, input));
}

void add_digital_input(gpio_num_t gpio_num, pull_resistor_t pull_resistor, std::string topic) {
  add_digital_input(gpio_num, pull_resistor, [topic](){ publish_message(topic, "0"); }, [topic](){ publish_message(topic, "1"); });
}

void add_button(gpio_num_t gpio_num, std::function<void()> on_press, std::function<void()> on_release) {
  add_digital_input(gpio_num, pullup, on_press, on_release);
}

void add_button(gpio_num_t gpio, std::string topic, std::string message) {
    add_button(gpio, [topic, message](){ publish_message(topic, message); });
}

void gpio_init_output(gpio_num_t gpio_num, uint32_t level) {
    gpio_pad_select_gpio(gpio_num);
    ESP_ERROR_CHECK(gpio_set_direction(gpio_num, GPIO_MODE_OUTPUT));
    ESP_ERROR_CHECK(gpio_set_level(gpio_num, level));
}

void pulse_relay(gpio_num_t gpio) {
    ESP_ERROR_CHECK(gpio_set_level(gpio, 1));
    vTaskDelay(25 / portTICK_PERIOD_MS);
    ESP_ERROR_CHECK(gpio_set_level(gpio, 0));
}

void add_relay(const std::string& topic, gpio_num_t gpio_off, gpio_num_t gpio_on) {
    gpio_init_output(gpio_off, 0);
    gpio_init_output(gpio_on, 0);

    add_message_callback(topic, [gpio_off, gpio_on](std::string message){
        if (message == "0") {
            pulse_relay(gpio_off);
        }
        else if (message == "1") {
            pulse_relay(gpio_on);
        }
    });
}