#include "button.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <inttypes.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include <hal/cpu_hal.h>

static QueueHandle_t callbackQueue = NULL;
static QueueHandle_t parameterQueue = NULL;
static int64_t pinHistory[32] = {0};

#ifndef GPIO_IN_REG
#define GPIO_IN_REG 0x6000403c
#endif

static void IRAM_ATTR rotaryPinChange(void *arg)
{
    rotaryPins *encoder = (rotaryPins *)arg;

    static char oldpinA = 0;
    static char oldpinB = 0;
    uint32_t io = REG_READ(GPIO_IN_REG);
    char pinA = (io >> encoder->PIN1) & 0x01;
    char pinB = (io >> encoder->PIN2) & 0x01;

    static uint8_t state = 0;

    switch (state)
    {

    case 0:
        // PINA Fallng CW
        if (oldpinA == 1 && pinA == 0 && pinB)
        {
            state++;
        }
        else if (oldpinB == 1 && pinB == 0 && pinA)
        {
            state++;
        }

        break;

    case 1:
        // PINB Falling CW
        if (oldpinB == 1 && pinB == 0 && !pinA)
        {
            state++;
        }
        else if (oldpinA == 1 && pinA == 0 && !pinB)
        {
            state++;
        }

        break;
        // PINA Rising
    case 2:
        if (oldpinA == 0 && pinA == 1 && !pinB)
        {
            state++;
        }
        else if (oldpinB == 0 && pinB == 1 && !pinA)
        {
            state++;
        }

        break;
        // PINB RISING
    case 3:
        if (oldpinB == 0 && pinB == 1 && pinA)
        {
            state = 0;
            xQueueSendFromISR(callbackQueue, &encoder->CWCallback, NULL);
            xQueueSendFromISR(parameterQueue, encoder, NULL);
        }
        else if (oldpinA == 0 && pinA == 1 && pinB)
        {
            state = 0;
            xQueueSendFromISR(callbackQueue, &encoder->CCWCallback, NULL);
            xQueueSendFromISR(parameterQueue, encoder, NULL);
        }

        break;
    default:
        break;
    }

    oldpinA = pinA;
    oldpinB = pinB;
}

static void IRAM_ATTR buttonPress(void *arg)
{
    rotaryPins *encoder = (rotaryPins *)arg;
    uint64_t time = esp_timer_get_time();

    if (time - encoder->lastButtonCall > 1000)
    {
        xQueueSendFromISR(callbackQueue, &encoder->buttonCallback, NULL);
        xQueueSendFromISR(parameterQueue, encoder, NULL);
        encoder->lastButtonCall = time;
    }
}

static void printStatus(void *arg)
{
    rotaryCallback callbackFunc;
    rotaryPins *rotStruct;
    for (;;)
    {
        if (xQueueReceive(callbackQueue, &callbackFunc, portMAX_DELAY) && xQueueReceive(parameterQueue, &rotStruct, portMAX_DELAY))
        {
            if (callbackFunc)
                callbackFunc(rotStruct);
        }
        vTaskDelay(20/portTICK_PERIOD_MS);
    }
}

void registerInterrupt(rotaryPins *rot)
{
    rot->ioMask = ((1ULL << rot->PIN1) | (1ULL << rot->PIN2) | (1ULL << rot->BUT));

    // zero-initialize the config structure.
    gpio_config_t io_conf = {};

    // interrupt of rising edge
    io_conf.intr_type = GPIO_INTR_ANYEDGE;
    // bit mask of the pins, use GPIO4/5 here
    io_conf.pin_bit_mask = rot->ioMask;
    // set as input mode
    io_conf.mode = GPIO_MODE_INPUT;
    // enable pull-up mode
    io_conf.pull_up_en = 1;
    io_conf.pull_down_en = 0;
    gpio_config(&io_conf);

    gpio_set_intr_type(rot->BUT,GPIO_INTR_POSEDGE);

    // create a queue to handle gpio event from isr
    callbackQueue = xQueueCreate(10, sizeof(rotaryCallback));
    parameterQueue = xQueueCreate(10, sizeof(rotaryPins *));
    // start gpio task
    xTaskCreate(printStatus, "rotaryQueueHandler", 2048, NULL, 10, NULL);

    // install gpio isr service
    gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
    // hook isr handler for specific gpio pin
    gpio_isr_handler_add(rot->PIN1, rotaryPinChange, (void *)rot);
    // hook isr handler for specific gpio pin
    gpio_isr_handler_add(rot->PIN2, rotaryPinChange, (void *)rot);

    gpio_isr_handler_add(rot->BUT, buttonPress, (void *)rot);

    esp_timer_early_init();
}