#include "zh_encoder.h"

#define TAG "zh_encoder"

#define ZH_ENCODER_LOGI(msg, ...) ESP_LOGI(TAG, msg, ##__VA_ARGS__)
#define ZH_ENCODER_LOGW(msg, ...) ESP_LOGW(TAG, msg, ##__VA_ARGS__)
#define ZH_ENCODER_LOGE(msg, ...) ESP_LOGE(TAG, msg, ##__VA_ARGS__)
#define ZH_ENCODER_LOGE_ERR(msg, err, ...) ESP_LOGE(TAG, "[%s:%d:%s] " msg, __FILE__, __LINE__, esp_err_to_name(err), ##__VA_ARGS__)

#define ZH_ENCODER_CHECK(cond, err, msg, ...) \
    if (!(cond))                              \
    {                                         \
        ZH_ENCODER_LOGE_ERR(msg, err);        \
        return err;                           \
    }

#define TABLE_ROWS 7
#define TABLE_COLS 4

#define DIR_NONE 0x0 // No complete step yet.
#define DIR_CW 0x10  // Clockwise step.
#define DIR_CCW 0x20 // Anti-clockwise step.

// Create the half-step state table (emits a code at 00 and 11)
#define R_START 0x0
#define H_CCW_BEGIN 0x1
#define H_CW_BEGIN 0x2
#define H_START_M 0x3
#define H_CW_BEGIN_M 0x4
#define H_CCW_BEGIN_M 0x5

static const uint8_t _encoder_matrix[TABLE_ROWS][TABLE_COLS] = {
    // 00                  01              10            11                   // BA
    {H_START_M, H_CW_BEGIN, H_CCW_BEGIN, R_START},            // R_START (00)
    {H_START_M | DIR_CCW, R_START, H_CCW_BEGIN, R_START},     // H_CCW_BEGIN
    {H_START_M | DIR_CW, H_CW_BEGIN, R_START, R_START},       // H_CW_BEGIN
    {H_START_M, H_CCW_BEGIN_M, H_CW_BEGIN_M, R_START},        // H_START_M (11)
    {H_START_M, H_START_M, H_CW_BEGIN_M, R_START | DIR_CW},   // H_CW_BEGIN_M
    {H_START_M, H_CCW_BEGIN_M, H_START_M, R_START | DIR_CCW}, // H_CCW_BEGIN_M
};

static QueueHandle_t _queue_handle = NULL;
static bool _is_initialized = false;

// #define ROTARY_ENCODER_DEBUG

// Use a single-item queue so that the last value can be easily overwritten by the interrupt handler
// #define EVENT_QUEUE_LENGTH 10

// #define TABLE_ROWS 7

// #define DIR_NONE 0x0 // No complete step yet.
// #define DIR_CW 0x10  // Clockwise step.
// #define DIR_CCW 0x20 // Anti-clockwise step.

// // Create the half-step state table (emits a code at 00 and 11)
// #define R_START 0x0
// #define H_CCW_BEGIN 0x1
// #define H_CW_BEGIN 0x2
// #define H_START_M 0x3
// #define H_CW_BEGIN_M 0x4
// #define H_CCW_BEGIN_M 0x5

// static const uint8_t _ttable_half[TABLE_ROWS][TABLE_COLS] = {
//     // 00                  01              10            11                   // BA
//     {H_START_M, H_CW_BEGIN, H_CCW_BEGIN, R_START},            // R_START (00)
//     {H_START_M | DIR_CCW, R_START, H_CCW_BEGIN, R_START},     // H_CCW_BEGIN
//     {H_START_M | DIR_CW, H_CW_BEGIN, R_START, R_START},       // H_CW_BEGIN
//     {H_START_M, H_CCW_BEGIN_M, H_CW_BEGIN_M, R_START},        // H_START_M (11)
//     {H_START_M, H_START_M, H_CW_BEGIN_M, R_START | DIR_CW},   // H_CW_BEGIN_M
//     {H_START_M, H_CCW_BEGIN_M, H_START_M, R_START | DIR_CCW}, // H_CCW_BEGIN_M
// };

// // Create the full-step state table (emits a code at 00 only)
// #define F_CW_FINAL 0x1
// #define F_CW_BEGIN 0x2
// #define F_CW_NEXT 0x3
// #define F_CCW_BEGIN 0x4
// #define F_CCW_FINAL 0x5
// #define F_CCW_NEXT 0x6

// static const uint8_t _ttable_full[TABLE_ROWS][TABLE_COLS] = {
//     // 00        01           10           11                  // BA
//     {R_START, F_CW_BEGIN, F_CCW_BEGIN, R_START},           // R_START
//     {F_CW_NEXT, R_START, F_CW_FINAL, R_START | DIR_CW},    // F_CW_FINAL
//     {F_CW_NEXT, F_CW_BEGIN, R_START, R_START},             // F_CW_BEGIN
//     {F_CW_NEXT, F_CW_BEGIN, F_CW_FINAL, R_START},          // F_CW_NEXT
//     {F_CCW_NEXT, R_START, F_CCW_BEGIN, R_START},           // F_CCW_BEGIN
//     {F_CCW_NEXT, F_CCW_FINAL, R_START, R_START | DIR_CCW}, // F_CCW_FINAL
//     {F_CCW_NEXT, F_CCW_FINAL, F_CCW_BEGIN, R_START},       // F_CCW_NEXT
// };

static esp_err_t _zh_encoder_validate_config(const zh_encoder_init_config_t *config);
static esp_err_t _zh_encoder_gpio_init(const zh_encoder_init_config_t *config);
static esp_err_t _zh_encoder_configure_interrupts(const zh_encoder_init_config_t *config, zh_encoder_handle_t *handle);
static esp_err_t _zh_encoder_init_resources(const zh_encoder_init_config_t *config);
static esp_err_t _zh_encoder_create_task(const zh_encoder_init_config_t *config);
static void _zh_encoder_isr_handler(void *arg);
static void _zh_encoder_isr_processing_task(void *pvParameter);

ESP_EVENT_DEFINE_BASE(ZH_ENCODER);

esp_err_t zh_encoder_init(const zh_encoder_init_config_t *config, zh_encoder_handle_t *handle)
{
    _zh_encoder_validate_config(config);
    _zh_encoder_gpio_init(config);
    handle->a_gpio_number = config->a_gpio_number;
    handle->b_gpio_number = config->b_gpio_number;
    _zh_encoder_configure_interrupts(config, handle);
    _zh_encoder_init_resources(config);
    _zh_encoder_create_task(config);
    handle->table = &_encoder_matrix[0]; // enable_half_step ? &_ttable_half[0] : &_ttable_full[0];
    handle->table_state = R_START;
    handle->state.position = 0;
    handle->state.direction = ROTARY_ENCODER_DIRECTION_NOT_SET;
    return ESP_OK;
}

esp_err_t zh_encoder_set(zh_encoder_handle_t *handle, float position)
{
    ZH_ENCODER_LOGI("Encoder set position started.");
    ZH_ENCODER_CHECK(handle->is_initialized == true, ESP_FAIL, "Encoder set position failed. Encoder not initialized.");
    ZH_ENCODER_CHECK(position <= handle->encoder_max_value && position >= handle->encoder_min_value, ESP_ERR_INVALID_ARG, "Encoder set position failed. Invalid argument.");
    handle->encoder_position = position;
    ZH_ENCODER_LOGI("Encoder set position completed successfully.");
    return ESP_OK;
}

esp_err_t zh_encoder_reset(zh_encoder_handle_t *handle)
{
    ZH_ENCODER_LOGI("Encoder reset started.");
    ZH_ENCODER_CHECK(handle->is_initialized == true, ESP_FAIL, "Encoder reset failed. Encoder not initialized.");
    handle->encoder_position = (handle->encoder_min_value + handle->encoder_max_value) / 2;
    ZH_ENCODER_LOGI("Encoder reset completed successfully.");
    return ESP_OK;
}
// static uint8_t _process(rotary_encoder_info_t *info)
// {
//     uint8_t event = 0;
//     if (info != NULL)
//     {
//         // Get state of input pins.
//         uint8_t pin_state = (gpio_get_level(info->pin_b) << 1) | gpio_get_level(info->pin_a);

//         // Determine new state from the pins and state table.
// #ifdef ROTARY_ENCODER_DEBUG
//         uint8_t old_state = info->table_state;
// #endif
//         info->table_state = info->table[info->table_state & 0xf][pin_state];

//         // Return emit bits, i.e. the generated event.
//         event = info->table_state & 0x30;
// #ifdef ROTARY_ENCODER_DEBUG
//         ESP_EARLY_LOGD(TAG, "BA %d%d, state 0x%02x, new state 0x%02x, event 0x%02x",
//                        pin_state >> 1, pin_state & 1, old_state, info->table_state, event);
// #endif
//     }
//     return event;
// }

// static void IRAM_ATTR _isr_rotenc(void *args)
// {
//     rotary_encoder_info_t *info = (rotary_encoder_info_t *)args;
//     uint8_t event = _process(info);
//     bool send_event = false;

//     switch (event)
//     {
//     case DIR_CW:
//         ++info->state.position;
//         info->state.direction = ROTARY_ENCODER_DIRECTION_CLOCKWISE;
//         send_event = true;
//         break;
//     case DIR_CCW:
//         --info->state.position;
//         info->state.direction = ROTARY_ENCODER_DIRECTION_COUNTER_CLOCKWISE;
//         send_event = true;
//         break;
//     default:
//         break;
//     }

//     if (send_event && info->queue)
//     {
//         rotary_encoder_event_t queue_event =
//             {
//                 .state =
//                     {
//                         .position = info->state.position,
//                         .direction = info->state.direction,
//                     },
//             };
//         BaseType_t task_woken = pdFALSE;
//         xQueueSendFromISR(info->queue, &queue_event, &task_woken);
//         if (task_woken)
//         {
//             portYIELD_FROM_ISR();
//         }
//     }
// }

// esp_err_t rotary_encoder_init(rotary_encoder_info_t *info, gpio_num_t pin_a, gpio_num_t pin_b)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         info->pin_a = pin_a;
//         info->pin_b = pin_b;
//         info->table = &_ttable_full[0]; // enable_half_step ? &_ttable_half[0] : &_ttable_full[0];
//         info->table_state = R_START;
//         info->state.position = 0;
//         info->state.direction = ROTARY_ENCODER_DIRECTION_NOT_SET;

//         gpio_config_t pin_config = {
//             .mode = GPIO_MODE_INPUT,
//             .pin_bit_mask = (1ULL << info->pin_a) | (1ULL << info->pin_b),
//             .pull_up_en = GPIO_PULLUP_ENABLE,
//             .intr_type = GPIO_INTR_ANYEDGE};
//         gpio_config(&pin_config);
//         // configure GPIOs
//         // gpio_pad_select_gpio(info->pin_a);
//         // gpio_set_pull_mode(info->pin_a, GPIO_PULLUP_ONLY);
//         // gpio_set_direction(info->pin_a, GPIO_MODE_INPUT);
//         // gpio_set_intr_type(info->pin_a, GPIO_INTR_ANYEDGE);

//         // gpio_pad_select_gpio(info->pin_b);
//         // gpio_set_pull_mode(info->pin_b, GPIO_PULLUP_ONLY);
//         // gpio_set_direction(info->pin_b, GPIO_MODE_INPUT);
//         // gpio_set_intr_type(info->pin_b, GPIO_INTR_ANYEDGE);

//         // install interrupt handlers
//         gpio_isr_handler_add(info->pin_a, _isr_rotenc, info);
//         gpio_isr_handler_add(info->pin_b, _isr_rotenc, info);
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// esp_err_t rotary_encoder_enable_half_steps(rotary_encoder_info_t *info, bool enable)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         info->table = enable ? &_ttable_half[0] : &_ttable_full[0];
//         info->table_state = R_START;
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// esp_err_t rotary_encoder_flip_direction(rotary_encoder_info_t *info)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         gpio_num_t temp = info->pin_a;
//         info->pin_a = info->pin_b;
//         info->pin_b = temp;
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// esp_err_t rotary_encoder_uninit(rotary_encoder_info_t *info)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         gpio_isr_handler_remove(info->pin_a);
//         gpio_isr_handler_remove(info->pin_b);
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// QueueHandle_t rotary_encoder_create_queue(void)
// {
//     return xQueueCreate(EVENT_QUEUE_LENGTH, sizeof(rotary_encoder_event_t));
// }

// esp_err_t rotary_encoder_set_queue(rotary_encoder_info_t *info, QueueHandle_t queue)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         info->queue = queue;
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// esp_err_t rotary_encoder_get_state(const rotary_encoder_info_t *info, rotary_encoder_state_t *state)
// {
//     esp_err_t err = ESP_OK;
//     if (info && state)
//     {
//         // make a snapshot of the state
//         state->position = info->state.position;
//         state->direction = info->state.direction;
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info and/or state is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

// esp_err_t rotary_encoder_reset(rotary_encoder_info_t *info)
// {
//     esp_err_t err = ESP_OK;
//     if (info)
//     {
//         info->state.position = 0;
//         info->state.direction = ROTARY_ENCODER_DIRECTION_NOT_SET;
//     }
//     else
//     {
//         ESP_LOGE(TAG, "info is NULL");
//         err = ESP_ERR_INVALID_ARG;
//     }
//     return err;
// }

static esp_err_t _zh_encoder_validate_config(const zh_encoder_init_config_t *config)
{
    ZH_ENCODER_CHECK(config != NULL, ESP_ERR_INVALID_ARG, "Invalid configuration.");
    ZH_ENCODER_CHECK(config->task_priority >= 10 && config->stack_size >= 2048, ESP_ERR_INVALID_ARG, "Invalid task settings.");
    ZH_ENCODER_CHECK(config->queue_size >= 10, ESP_ERR_INVALID_ARG, "Invalid queue size.");
    return ESP_OK;
}

static esp_err_t _zh_encoder_gpio_init(const zh_encoder_init_config_t *config)
{
    ZH_ENCODER_CHECK(config->a_gpio_number < GPIO_NUM_MAX || config->b_gpio_number < GPIO_NUM_MAX, ESP_ERR_INVALID_ARG, "Invalid GPIO number.")
    ZH_ENCODER_CHECK(config->a_gpio_number != config->b_gpio_number, ESP_ERR_INVALID_ARG, "Invalid GPIO number.")
    gpio_config_t pin_config = {
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << config->a_gpio_number) | (1ULL << config->b_gpio_number),
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .intr_type = GPIO_INTR_ANYEDGE};
    esp_err_t err = gpio_config(&pin_config);
    ZH_ENCODER_CHECK(err == ESP_OK, err, "GPIO initialization failed.");
    return ESP_OK;
}

static esp_err_t _zh_encoder_configure_interrupts(const zh_encoder_init_config_t *config, zh_encoder_handle_t *handle)
{
    gpio_install_isr_service(0);
    esp_err_t err = gpio_isr_handler_add(config->a_gpio_number, _zh_encoder_isr_handler, handle);
    ZH_ENCODER_CHECK(err == ESP_OK, err, "Interrupt initialization failed.");
    err = gpio_isr_handler_add(config->b_gpio_number, _zh_encoder_isr_handler, handle);
    ZH_ENCODER_CHECK(err == ESP_OK, err, "Interrupt initialization failed.");
    //  printf("queue.b_gpio_number %d\n", handle->b_gpio_number);
    //     printf("queue.q_gpio_number %d\n", handle->a_gpio_number);
    return ESP_OK;
}

static esp_err_t _zh_encoder_init_resources(const zh_encoder_init_config_t *config)
{
    if (_is_initialized == false)
    {
        _queue_handle = xQueueCreate(config->queue_size, sizeof(zh_encoder_handle_t));
        ZH_ENCODER_CHECK(_queue_handle != NULL, ESP_FAIL, "Queue creation failed.");
    }
    return ESP_OK;
}

static esp_err_t _zh_encoder_create_task(const zh_encoder_init_config_t *config)
{
    if (_is_initialized == false)
    {
        BaseType_t err = xTaskCreatePinnedToCore(
            &_zh_encoder_isr_processing_task,
            "zh_encoder_isr_processing",
            config->stack_size,
            NULL,
            config->task_priority,
            NULL,
            tskNO_AFFINITY);
        ZH_ENCODER_CHECK(err == pdPASS, ESP_FAIL, "Task creation failed.");
    }
    return ESP_OK;
}

static void _zh_encoder_isr_handler(void *arg)
{
    zh_encoder_handle_t *queue = (zh_encoder_handle_t *)arg;
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    uint8_t pin_state = (gpio_get_level(queue->b_gpio_number) << 1) | gpio_get_level(queue->a_gpio_number);
    // printf("pin_state %d\n", pin_state);
    queue->table_state = queue->table[queue->table_state & 0xf][pin_state];
    uint8_t event = queue->table_state & 0x30;
    switch (event)
    {
    case DIR_CW:
        ++queue->state.position;
        queue->state.direction = ROTARY_ENCODER_DIRECTION_CLOCKWISE;
        // printf("event %d\n", event);
        // send_event = true;
        // printf("state.position %ld\n", queue->state.position);
        xQueueSendFromISR(_queue_handle, queue, &xHigherPriorityTaskWoken);
        break;
    case DIR_CCW:
        --queue->state.position;
        queue->state.direction = ROTARY_ENCODER_DIRECTION_COUNTER_CLOCKWISE;
        // printf("event %d\n", event);
        // send_event = true;
        // printf("state.position %ld\n", queue.state.position);
        xQueueSendFromISR(_queue_handle, queue, &xHigherPriorityTaskWoken);
        break;
    default:
        break;
    }

    // BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    // xQueueSendFromISR(_queue_handle, queue, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken == pdTRUE)
    {
        portYIELD_FROM_ISR();
    };
}

static void _zh_encoder_isr_processing_task(void *pvParameter)
{
    zh_encoder_handle_t queue = {0};
    while (xQueueReceive(_queue_handle, &queue, portMAX_DELAY) == pdTRUE)
    {
        // printf("queue.b_gpio_number %d\n", queue.b_gpio_number);
        // printf("queue.q_gpio_number %d\n", queue.a_gpio_number);
        // uint8_t pin_state = (gpio_get_level(queue.b_gpio_number) << 1) | gpio_get_level(queue.a_gpio_number);
        // // printf("pin_state %d\n", pin_state);
        // queue.table_state = queue.table[queue.table_state & 0xf][pin_state];
        // uint8_t event = queue.table_state & 0x30;
        // switch (event)
        // {
        // case DIR_CW:
        //     ++queue.state.position;
        //     queue.state.direction = ROTARY_ENCODER_DIRECTION_CLOCKWISE;
        //     printf("event %d\n", event);
        //     // send_event = true;
        //     printf("state.position %ld\n", queue.state.position);
        //     break;
        // case DIR_CCW:
        //     --queue.state.position;
        //     queue.state.direction = ROTARY_ENCODER_DIRECTION_COUNTER_CLOCKWISE;
        //     printf("event %d\n", event);
        //     // send_event = true;
        //     printf("state.position %ld\n", queue.state.position);
        //     break;
        // default:
        //     break;
        // }
        printf("state.position %ld\n", queue.state.position);
    }
    vTaskDelete(NULL);

    // rotary_encoder_info_t *info = (rotary_encoder_info_t *)args;

    //     uint8_t event = 0;
    //     if (info != NULL)
    //     {
    //         // Get state of input pins.
    //         uint8_t pin_state = (gpio_get_level(info->pin_b) << 1) | gpio_get_level(info->pin_a);

    //         // Determine new state from the pins and state table.
    // #ifdef ROTARY_ENCODER_DEBUG
    //         uint8_t old_state = info->table_state;
    // #endif
    //         queue.table_state = queue.table[queue.table_state & 0xf][pin_state];

    //         // Return emit bits, i.e. the generated event.
    //         event = queue.table_state & 0x30;
    // #ifdef ROTARY_ENCODER_DEBUG
    //         ESP_EARLY_LOGD(TAG, "BA %d%d, state 0x%02x, new state 0x%02x, event 0x%02x",
    //                        pin_state >> 1, pin_state & 1, old_state, info->table_state, event);
    // #endif
    //     }
    //     return event;

    // uint8_t event = _process(info);
    // bool send_event = false;

    // switch (event)
    // {
    // case DIR_CW:
    //     ++info->state.position;
    //     info->state.direction = ROTARY_ENCODER_DIRECTION_CLOCKWISE;
    //     // send_event = true;
    //     break;
    // case DIR_CCW:
    //     --info->state.position;
    //     info->state.direction = ROTARY_ENCODER_DIRECTION_COUNTER_CLOCKWISE;
    //     send_event = true;
    //     break;
    // default:
    //     break;
    // }

    // if (send_event && info->queue)
    // {
    //     rotary_encoder_event_t queue_event =
    //         {
    //             .state =
    //                 {
    //                     .position = info->state.position,
    //                     .direction = info->state.direction,
    //                 },
    //         };
    //     BaseType_t task_woken = pdFALSE;
    //     xQueueSendFromISR(info->queue, &queue_event, &task_woken);
    //     if (task_woken)
    //     {
    //         portYIELD_FROM_ISR();
    //     }
    // }
}