#include "motor_control.h"
#include "freertos/projdefs.h"
#include "freertos/queue.h"
#include "step_motor_init.h"
#include "motor_io.h"
#include "foc.h"

#include <limits.h>
#include <stdint.h>
#include <stdbool.h>
#include "esp_check.h"
#include "esp_log.h"
#include "BLE/ble.h"

static const char *TAG_MOTOR_CTL = "motor_control";

TaskHandle_t motor_control_handle = NULL;

#define MOTOR_CMD_QUEUE_LEN 16

static QueueHandle_t s_motor_cmd_q = NULL;

typedef enum
{
    MOTOR_LAT_UNKNOWN = 0,
    MOTOR_LAT_LEFT,
    MOTOR_LAT_RIGHT,
    MOTOR_LAT_CENTER,
} motor_lateral_t;

/** 仅电机任务写、其它任务读；用于左右切换时是否插 0x0A */
static volatile motor_lateral_t s_motor_last_lateral = MOTOR_LAT_CENTER;
/** 最近一次成功入队的 play_sticker 方向字节；同字节+同侧向则不再重复入队（如已是 0x01 又来 0x01） */
static uint8_t s_last_play_sticker_direction = 0xFF;

// MOTOR_POSITION motor_position ;//记录步进电机的位置

MOTOR_POSITION motor_current_position  = MOTOR_POSITION_ZERO;
MOTOR_POSITION motor_target_position   = MOTOR_POSITION_ZERO;

MOTOR_DIR motor_dir = MOTOR_CW;

SemaphoreHandle_t motor_action_down_sem = NULL;

static volatile bool s_sentinel_motor_running = false;
static volatile bool s_sentinel_stop_requested = false;

#if MOTOR_WITH_MAGNET_RING
const MOTOR_RUN_TIME motor_run_time =
{
    .run_to_left_15   = 250,
    .run_to_left_30   = 500,
    .run_to_left_90   = 1000,
    .run_to_left_180  = 1800,
    .run_to_right_15  = 250,
    .run_to_right_30  = 500,
    .run_to_right_180 = 1800,
};
uint16_t OB_BACK_TIME = 130;
#elif OLD_MOTOR
const MOTOR_RUN_TIME motor_run_time =
{
    .run_to_left_15   = 250,
    .run_to_left_30   = 500,
    .run_to_left_90   = 750,
    .run_to_left_180  = 1300,
    .run_to_right_15  = 250,
    .run_to_right_30  = 500,
    .run_to_right_180 = 1300,
};
uint16_t OB_BACK_TIME = 55;
#else
const MOTOR_RUN_TIME motor_run_time =
{
    .run_to_left_15   = 250,
    .run_to_left_30   = 450,
    .run_to_left_90   = 1250,
    .run_to_left_180  = 1950,
    .run_to_right_15  = 250,
    .run_to_right_30  = 450,
    .run_to_right_180 = 1950,
};
uint16_t OB_BACK_TIME = 55;
#endif

static void motor_run_steps(uint32_t command);

/** 与上次已在电机任务里执行完的 notify 值相同则忽略，避免 BLE 连发 / completed_cb 重复补发导致同向连转 */
static uint32_t s_last_executed_motor_notify = UINT32_MAX;

static bool motor_cmd_is_left(uint32_t cmd)
{
    return cmd == 0x01u || cmd == 0x05u || cmd == 0x07u;
}

static bool motor_cmd_is_right(uint32_t cmd)
{
    return cmd == 0x02u || cmd == 0x06u || cmd == 0x08u;
}

static void motor_update_lateral_after_cmd(uint32_t cmd)
{
    if (motor_cmd_is_left(cmd))
    {
        s_motor_last_lateral = MOTOR_LAT_LEFT;
    }
    else if (motor_cmd_is_right(cmd))
    {
        s_motor_last_lateral = MOTOR_LAT_RIGHT;
    }
    else if (cmd == 0x00u || cmd == 0x0Au || cmd == 0x0Bu)
    {
        s_motor_last_lateral = MOTOR_LAT_CENTER;
    }
}

bool motor_post_cmd(uint32_t cmd)
{
    if (s_motor_cmd_q == NULL)
    {
        ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: queue not ready cmd=0x%02lX", (unsigned long)cmd);
        return false;
    }
    if (xQueueSend(s_motor_cmd_q, &cmd, 0) != pdPASS)
    {
        uint32_t dropped = 0;
        (void)xQueueReceive(s_motor_cmd_q, &dropped, 0);
        if (xQueueSend(s_motor_cmd_q, &cmd, 0) != pdPASS)
        {
            ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: enqueue failed cmd=0x%02lX", (unsigned long)cmd);
            return false;
        }
        ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: queue full dropped=0x%02lX enq=0x%02lX",
                 (unsigned long)dropped, (unsigned long)cmd);
    }
    return true;
}

bool motor_post_play_sticker_direction(uint8_t direction)
{
    const uint32_t cmd = (uint32_t)direction;
    /* 0x00：协议表示「本条不追加转向」；若头仍在左/右侧，必须用 0x0A 物理回中，
     * 不能发 0x00（openloop stop 与回中语义不同），否则左播途中接 0x10/0x00 会丢回中。 */
    if (cmd == 0x00u)
    {
        const motor_lateral_t last = s_motor_last_lateral;
        if (last == MOTOR_LAT_LEFT || last == MOTOR_LAT_RIGHT)
        {
            if (!motor_post_cmd(0x0Au))
            {
                return false;
            }
            s_last_play_sticker_direction = 0xFF;
            return true;
        }
        return true;
    }
    if (cmd == 0x0Au || cmd == 0x0Bu)
    {
        if (!motor_post_cmd(cmd))
        {
            return false;
        }
        s_last_play_sticker_direction = 0xFF;
        return true;
    }
    if (!motor_cmd_is_left(cmd) && !motor_cmd_is_right(cmd))
    {
        if (!motor_post_cmd(cmd))
        {
            return false;
        }
        s_last_play_sticker_direction = (uint8_t)cmd;
        return true;
    }
    const motor_lateral_t last = s_motor_last_lateral;
    /* 侧向未变且与上次下发的方向字节相同：不重复入队（如当前已是 0x01 姿态又来 0x01） */
    if ((motor_cmd_is_left(cmd) && last == MOTOR_LAT_LEFT) || (motor_cmd_is_right(cmd) && last == MOTOR_LAT_RIGHT))
    {
        if ((uint8_t)cmd == s_last_play_sticker_direction)
        {
            ESP_LOGI(TAG_MOTOR_CTL, "skip redundant play_sticker direction 0x%02lX (same as last)", cmd);
            return true;
        }
    }
    const bool need_home =
        (last == MOTOR_LAT_LEFT && motor_cmd_is_right(cmd)) || (last == MOTOR_LAT_RIGHT && motor_cmd_is_left(cmd));
    if (need_home && !motor_post_cmd(0x0Au))
    {
        return false;
    }
    if (!motor_post_cmd(cmd))
    {
        return false;
    }
    s_last_play_sticker_direction = (uint8_t)cmd;
    return true;
}

void motor_post_cmd_from_isr(uint32_t cmd, BaseType_t *pxHigherPriorityTaskWoken)
{
    if (s_motor_cmd_q == NULL)
    {
        return;
    }
    if (xQueueSendFromISR(s_motor_cmd_q, &cmd, pxHigherPriorityTaskWoken) != pdPASS)
    {
        uint32_t dropped = 0;
        (void)xQueueReceiveFromISR(s_motor_cmd_q, &dropped, pxHigherPriorityTaskWoken);
        (void)xQueueSendFromISR(s_motor_cmd_q, &cmd, pxHigherPriorityTaskWoken);
    }
}


void motor_control_init(void)
{
    // PWM 初始化 任务创建
    //motor_io_init();
    stepper_motor_init();

    s_motor_cmd_q = xQueueCreate(MOTOR_CMD_QUEUE_LEN, sizeof(uint32_t));
    if (s_motor_cmd_q == NULL)
    {
        ESP_LOGE(TAG_MOTOR_CTL, "motor cmd queue create failed");
    }

    // 固定到 CPU1，避免与 CPU0 上的 LVGL task 竞争；优先级降为 5
    xTaskCreatePinnedToCore(motor_control_task, "motor_control_task", 4096, NULL, 8, &motor_control_handle, 0);
}

void motor_control_task(void *pvParameter)
{
    uint32_t bits = 0;
    motor_action_down_sem = xSemaphoreCreateBinary();
    xSemaphoreGive(motor_action_down_sem);
    (void)pvParameter;
    s_motor_last_lateral = MOTOR_LAT_CENTER;
    motor_current_position =MOTOR_POSITION_ZERO;
    motor_run_dir(MOTOR_CW);
    stepper_motor_output_enable();
    vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_180));
    motor_current_position = MOTOR_POSITION_LEFT_180;
    stepper_motor_output_disable();
    ESP_LOGW("TAG_MOTOR_CTL", "motor_control_task: TURNED LEFT DONE");
    s_motor_last_lateral = MOTOR_LAT_LEFT;
    vTaskDelay(pdMS_TO_TICKS(2000));
    motor_run_dir(MOTOR_CCW);
    stepper_motor_output_enable();
    vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_180));
    stepper_motor_output_disable();
    s_motor_last_lateral = MOTOR_LAT_CENTER;
    motor_current_position = MOTOR_POSITION_ZERO;
    ESP_LOGW("TAG_MOTOR_CTL", "motor_control_task: TURNED RIGHT DONE");
    while (1)
    {
        if (s_motor_cmd_q == NULL)
        {
            vTaskDelay(pdMS_TO_TICKS(20));
            continue;
        }
        if (xQueueReceive(s_motor_cmd_q, &bits, pdMS_TO_TICKS(20)) != pdPASS)
        {
            continue;
        }
        if (bits == s_last_executed_motor_notify)
        {
            ESP_LOGD(TAG_MOTOR_CTL, "skip duplicate motor cmd 0x%02lX", (unsigned long)bits);
            continue;
        }
        motor_run_steps(bits);
        s_last_executed_motor_notify = bits;
        motor_update_lateral_after_cmd(bits);
    }
}

static void motor_run_steps(uint32_t command)
{
    uint32_t back_time_ms = 0;
    if (command == 0x00)
    {
        stepper_motor_stop();
        motor_current_position = MOTOR_POSITION_ZERO;
    }
    else if (command == 0x01)
    {
        motor_run_dir(MOTOR_CW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_30));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_LEFT_30;

    }
    else if (command == 0x02)
    {
        motor_run_dir(MOTOR_CCW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_30));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_RIGHT_30;
        
    }
    else if (command == 0x05)
    {
        // stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_15 * 4);
        motor_run_dir(MOTOR_CW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_15));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_LEFT_15; 
        
    }
    else if (command == 0x06)
    {
        // stepper_motor_run_steps(STEPPER_DIR_REVERSE, ANGLE_STEP_15 * 4);
        motor_run_dir(MOTOR_CCW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_15));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_RIGHT_15;
        
    }
    else if (command == 0x07)
    {
        // stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_180 * 4);
        motor_run_dir(MOTOR_CW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_180));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_LEFT_180;
        if(sentinel_mode_is_active == true)
        {
            vTaskDelay(pdMS_TO_TICKS(2000));
            xSemaphoreGive(motor_action_down_sem);
        }
        
    }
    else if (command == 0x08)
    {
        // stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_180 * 4);
        motor_run_dir(MOTOR_CCW);
        stepper_motor_output_enable();
        vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_180));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_RIGHT_180;
        if(sentinel_mode_is_active == true)
        {
            vTaskDelay(pdMS_TO_TICKS(2000));
            xSemaphoreGive(motor_action_down_sem);
        }

    }
    else if (command == 0x0A)
    {
        // stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_30 * 4);
        if(motor_current_position == MOTOR_POSITION_ZERO)back_time_ms = 0;
        else if(motor_current_position == MOTOR_POSITION_LEFT_15 || motor_current_position == MOTOR_POSITION_LEFT_30 || motor_current_position == MOTOR_POSITION_LEFT_180)
        {
            motor_run_dir(MOTOR_CCW);
            if(motor_current_position == MOTOR_POSITION_LEFT_15)back_time_ms = motor_run_time.run_to_right_15;
            else if(motor_current_position == MOTOR_POSITION_LEFT_30)back_time_ms = motor_run_time.run_to_right_30;
            else if(motor_current_position == MOTOR_POSITION_LEFT_180)back_time_ms = motor_run_time.run_to_right_180;             
        
        }
        else if(motor_current_position == MOTOR_POSITION_RIGHT_15 || motor_current_position == MOTOR_POSITION_RIGHT_30|| motor_current_position == MOTOR_POSITION_RIGHT_180)
        {
            motor_run_dir(MOTOR_CW);
            if(motor_current_position == MOTOR_POSITION_RIGHT_15)back_time_ms = motor_run_time.run_to_left_15;
            else if(motor_current_position == MOTOR_POSITION_RIGHT_30)back_time_ms = motor_run_time.run_to_left_30;
            else if(motor_current_position == MOTOR_POSITION_RIGHT_180)back_time_ms = motor_run_time.run_to_left_180;
        } 
        if(back_time_ms != 0)
        {
            ESP_LOGW(TAG_MOTOR_CTL, "0X0A back_time_ms START,motor_current_position=%d,back_time_ms=%d",motor_current_position,back_time_ms);
            stepper_motor_output_enable();
            vTaskDelay(pdMS_TO_TICKS(back_time_ms));
        }
        stepper_motor_output_disable();
        ESP_LOGW(TAG_MOTOR_CTL, "0X0A back_time_ms END");
        if(sentinel_mode_is_active == true)
        {
            vTaskDelay(pdMS_TO_TICKS(2000));
            xSemaphoreGive(motor_action_down_sem);
        }
        motor_current_position = MOTOR_POSITION_ZERO;
  
    }
    else if (command == 0x0B)
    {
         ESP_LOGE("MOTOR_CONTROL", "MOTOR_CONTROL_STOP");
        vTaskDelay(pdMS_TO_TICKS(100));
        // motor_run_dir(MOTOR_CW);
        // stepper_motor_output_enable();
        // vTaskDelay(pdMS_TO_TICKS(35));
        stepper_motor_output_disable();
        motor_current_position = MOTOR_POSITION_ZERO;
    }
}

void motor_run_dir(MOTOR_DIR dir)
{
    
    if(dir == MOTOR_CW)
    {
        motor_dir = MOTOR_CW;
    }
    if(dir == MOTOR_CCW)
    {
        motor_dir = MOTOR_CCW;
    }

}