#include "motor_io.h"

#include <stdbool.h>

#include "MOTOR/motor_control.h"
#include "step_motor_init.h"
#include "driver/gpio.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"


static const char *TAG = "motor_io";
static TaskHandle_t s_motor_io_task_handle = NULL;
static volatile bool s_stop_transition_enabled = false;
static volatile int s_stop_from_14 = 0;
static volatile int s_stop_from_47 = 0;
static volatile int s_stop_to_14 = 0;
static volatile int s_stop_to_47 = 0;

volatile int s_io14_last_level ;
volatile int s_io47_last_level ;

volatile int s_io14_now_level  ;
volatile int s_io47_now_level  ;
static void motor_io_log_levels(const char *reason, int level_14, int level_47)
{
    ESP_LOGI(TAG, "%s GPIO14=%d GPIO47=%d", reason, level_14, level_47);
}

static void IRAM_ATTR motor_io_isr_handler(void *arg)
{
    (void)arg;
    BaseType_t hpw = pdFALSE;

    s_io14_now_level = motor_io_get_level_14();
    s_io47_now_level = motor_io_get_level_47();

    if (motor_control_handle != NULL)
    {
        if(s_io14_last_level == 0 && s_io47_last_level == 1)
        {
            if(s_io14_now_level == 1 && s_io47_now_level == 1)
            {
                //发送停止信号
                if(motor_current_position == MOTOR_POSITION_LEFT_2)
                {
                    stepper_motor_output_disable();
                    motor_post_cmd_from_isr((uint32_t)0x0Bu, &hpw);
                }
            }
        }
        if(s_io14_last_level == 0 && s_io47_last_level == 1)
        {
            if(s_io14_now_level == 0 && s_io47_now_level == 0)
            {
                //发送停止信号
             
                // xTaskNotifyFromISR(motor_control_handle,(uint32_t)0x0B,eSetValueWithOverwrite,NULL);

            }
        }
    }

    s_io14_last_level = s_io14_now_level;
    s_io47_last_level = s_io47_now_level;


    if (s_motor_io_task_handle != NULL)
    {
        vTaskNotifyGiveFromISR(s_motor_io_task_handle, &hpw);
    }

    if (hpw == pdTRUE)
    {
        portYIELD_FROM_ISR();
    }
}

int motor_io_get_level_14(void)
{
    return gpio_get_level(MOTOR_IO_PIN_14);
}

int motor_io_get_level_47(void)
{
    return gpio_get_level(MOTOR_IO_PIN_47);
}

void motor_io_get_levels(int *level_14, int *level_47)
{
    if (level_14 != NULL)
    {
        *level_14 = motor_io_get_level_14();
    }

    if (level_47 != NULL)
    {
        *level_47 = motor_io_get_level_47();
    }
}

void motor_io_set_stop_transition(int from_14, int from_47, int to_14, int to_47)
{
    s_stop_from_14 = from_14;
    s_stop_from_47 = from_47;
    s_stop_to_14 = to_14;
    s_stop_to_47 = to_47;
    s_stop_transition_enabled = true;
}

void motor_io_clear_stop_transition(void)
{
    s_stop_transition_enabled = false;
}

static void motor_io_monitor_task(void *pvParameter)
{
    (void)pvParameter;

    int last_level_14 = motor_io_get_level_14();
    int last_level_47 = motor_io_get_level_47();

    motor_io_log_levels("初始电平:", last_level_14, last_level_47);

    while (1)
    {
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        int level_14 = motor_io_get_level_14();
        int level_47 = motor_io_get_level_47();

        if ((level_14 != last_level_14) || (level_47 != last_level_47))
        {
            motor_io_log_levels("检测到电平变化:", level_14, level_47);

            if (s_stop_transition_enabled &&
                (last_level_14 == s_stop_from_14) &&
                (last_level_47 == s_stop_from_47) &&
                (level_14 == s_stop_to_14) &&
                (level_47 == s_stop_to_47))
            {
                ESP_LOGI(TAG, "命中停机跳变: GPIO14=%d GPIO47=%d -> GPIO14=%d GPIO47=%d",
                         last_level_14,
                         last_level_47,
                         level_14,
                         level_47);
                stepper_motor_stop();
            }

            last_level_14 = level_14;
            last_level_47 = level_47;
        }
    }
}

void motor_io_init(void)
{
    const gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << MOTOR_IO_PIN_14) | (1ULL << MOTOR_IO_PIN_47),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_ANYEDGE,
    };

    ESP_ERROR_CHECK(gpio_config(&io_conf));


    if (s_motor_io_task_handle == NULL)
    {
        xTaskCreatePinnedToCore(motor_io_monitor_task,
                                "motor_io_task",
                                MOTOR_IO_TASK_STACK_SIZE,
                                NULL,
                                MOTOR_IO_TASK_PRIORITY,
                                &s_motor_io_task_handle,
                                1);
    }

    esp_err_t ret = gpio_install_isr_service(0);
    if ((ret != ESP_OK) && (ret != ESP_ERR_INVALID_STATE))
    {
        ESP_ERROR_CHECK(ret);
    }

    ESP_ERROR_CHECK(gpio_isr_handler_add(MOTOR_IO_PIN_14, motor_io_isr_handler, NULL));
    ESP_ERROR_CHECK(gpio_isr_handler_add(MOTOR_IO_PIN_47, motor_io_isr_handler, NULL));
    
    s_io14_last_level = motor_io_get_level_14();
    s_io47_last_level = motor_io_get_level_47();

}