#include "app.h"
#include "cmd_parser.h"
#include "cmsis_os2.h"
#include <string.h>
#include <stdlib.h>
#include "FreeRTOS.h"
#include "queue.h"
#include "stm32f4xx_hal.h"

typedef enum {
    USER_CMD_IDLE = 0,
    USER_CMD_RECEIVE,
} user_cmd_state;

static struct {
    user_cmd_state       state;
    char                 buffer[MAX_CMD_LEN];
    uint8_t              index;
    osMessageQueueId_t   cmd_queue;
} g_cmd_parser;

/*
 * 支持的命令（格式：@command\n）:
 *   "motor enable"            — 使能全部电机（零速启动）
 *   "motor enable yaw"        — 使能 Yaw 电机
 *   "motor enable pitch"      — 使能 Pitch 电机
 *   "motor disable"           — 关闭全部电机（惰行 coast）
 *   "motor disable yaw"       — 关闭 Yaw 电机
 *   "motor disable pitch"     — 关闭 Pitch 电机
 *   "sensor enable"           — 恢复 IMU 读取
 *   "sensor disable"          — 暂停 IMU 读取
 *   "sensorcali acc enable"   — 开始加速度计标定
 *   "sensorcali acc disable"  — 停止加速度计标定
 *   "sensorcali mag enable"   — 开始磁场标定
 *   "sensorcali mag disable"  — 停止磁场标定
 */

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance != USART1) return;

    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    uint8_t rx = g_uart_rx_byte;

    switch (g_cmd_parser.state) {
        case USER_CMD_IDLE:
            if (rx == '@') {
                g_cmd_parser.state = USER_CMD_RECEIVE;
                g_cmd_parser.index = 0;
            }
            break;

        case USER_CMD_RECEIVE:
            if (rx == '\n') {
                /* 命令结束 —— null-terminate 并入队 */
                if (g_cmd_parser.cmd_queue != NULL) {
                    g_cmd_parser.buffer[g_cmd_parser.index] = '\0';
                    xQueueSendFromISR(g_cmd_parser.cmd_queue,
                                      g_cmd_parser.buffer,
                                      &xHigherPriorityTaskWoken);
                }
                g_cmd_parser.state = USER_CMD_IDLE;
                g_cmd_parser.index = 0;
            } else if (rx == '\r') {
                /* 忽略回车符（兼容终端发送 \r\n） */
            } else if (rx == '@') {
                /* 重新开始 —— 取消当前输入 */
                g_cmd_parser.index = 0;
            } else {
                if (g_cmd_parser.index < MAX_CMD_LEN - 1) {
                    g_cmd_parser.buffer[g_cmd_parser.index++] = (char)rx;
                } else {
                    /* 缓冲区溢出 — 丢弃并等待下一个 @ */
                    g_cmd_parser.state = USER_CMD_IDLE;
                    g_cmd_parser.index = 0;
                }
            }
            break;
    }

    /* 重新使能 UART 接收中断 */
    HAL_UART_Receive_IT(huart, &g_uart_rx_byte, 1);

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

/* ============================================================================
 * Command handlers (registered with cmd_parser)
 * ============================================================================ */

static int cmd_help(void *ctx, int argc, char **argv)
{
    (void)ctx; (void)argc; (void)argv;
    log_printf("[cmd] available commands:\r\n"
           "  help                    ==> show this message\r\n"
           "  motor enable [yaw|pitch] ==> enable motor(s), default both\r\n"
           "  motor disable [yaw|pitch] ==> disable motor(s), default both\r\n"
           "  sensor enable           ==> resume IMU data publishing\r\n"
           "  sensor disable          ==> pause IMU data publishing\r\n"
           "  sensorcali acc enable   ==> start accelerometer calibration\r\n"
           "  sensorcali acc disable  ==> stop accelerometer calibration\r\n"
           "  sensorcali mag enable   ==> start magnetometer calibration\r\n"
           "  sensorcali mag disable  ==> stop magnetometer calibration\r\n"
           "  pid                    ==> show all PID coefficients\r\n"
           "  pid <axis> <p> <v>     ==> set PID param (v÷10=实际值, motor disabled)\r\n"
           "      axis: roll|pitch, p: kp|ki|kd, v: float\r\n");
    return 0;
}

static int cmd_motor(void *ctx, int argc, char **argv)
{
    (void)ctx;
    if (argc < 2) return -1;

    if (strcmp(argv[1], "enable") == 0) {
        if (argc == 2) {
            gimbal_motor_enable();
            log_printf("[cmd] both motors enabled\r\n");
        } else if (strcmp(argv[2], "yaw") == 0) {
            gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
            log_printf("[cmd] yaw motor enabled\r\n");
        } else if (strcmp(argv[2], "pitch") == 0) {
            gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
            log_printf("[cmd] pitch motor enabled\r\n");
        } else {
            log_printf("[cmd] unknown motor: %s\r\n", argv[2]);
        }
        return 0;
    }

    if (strcmp(argv[1], "disable") == 0) {
        if (argc == 2) {
            gimbal_motor_disable();
            log_printf("[cmd] both motors disabled\r\n");
        } else if (strcmp(argv[2], "yaw") == 0) {
            gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
            log_printf("[cmd] yaw motor disabled\r\n");
        } else if (strcmp(argv[2], "pitch") == 0) {
            gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
            log_printf("[cmd] pitch motor disabled\r\n");
        } else {
            log_printf("[cmd] unknown motor: %s\r\n", argv[2]);
        }
        return 0;
    }

    return -1;
}

static int cmd_sensor(void *ctx, int argc, char **argv)
{
    (void)ctx;
    if (argc < 2) return -1;

    if (strcmp(argv[1], "enable") == 0) {
        sensor_enable();
        log_printf("[cmd] sensor enabled\r\n");
    } else if (strcmp(argv[1], "disable") == 0) {
        sensor_disable();
        log_printf("[cmd] sensor disabled\r\n");
    } else {
        return -1;
    }
    return 0;
}

static int cmd_sensorcali(void *ctx, int argc, char **argv)
{
    (void)ctx;
    if (argc < 3) return -1;

    if (strcmp(argv[1], "acc") == 0) {
        if (strcmp(argv[2], "enable") == 0) {
            sensor_acc_cali_enable();
            log_printf("[cmd] acc calibration started\r\n");
        } else if (strcmp(argv[2], "disable") == 0) {
            sensor_acc_cali_disable();
            log_printf("[cmd] acc calibration stopped\r\n");
        } else {
            return -1;
        }
    } else if (strcmp(argv[1], "mag") == 0) {
        if (strcmp(argv[2], "enable") == 0) {
            sensor_mag_cali_enable();
            log_printf("[cmd] mag calibration started\r\n");
        } else if (strcmp(argv[2], "disable") == 0) {
            sensor_mag_cali_disable();
            log_printf("[cmd] mag calibration stopped\r\n");
        } else {
            return -1;
        }
    } else {
        return -1;
    }
    return 0;
}

static int cmd_pid(void *ctx, int argc, char **argv)
{
    (void)ctx;
    if (argc == 1) {
        float kp, ki, kd;
        gimbal_get_pid("roll", &kp, &ki, &kd);
        log_printf("[cmd] Roll:  Kp=%d  Ki=%d  Kd=%d\r\n",
               (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
        gimbal_get_pid("pitch", &kp, &ki, &kd);
        log_printf("[cmd] Pitch: Kp=%d  Ki=%d  Kd=%d\r\n",
               (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
        return 0;
    }

    if (argc == 4) {
        if (gimbal_is_motor_enabled()) {
            log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
            return 0;
        }
        char *end;
        long int_val = strtol(argv[3], &end, 10);
        if (*end != '\0' || end == argv[3]) {
            log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
            return 0;
        }
        float value = (float)int_val / 10.0f;
        if (gimbal_set_pid(argv[1], argv[2], value) == 0) {
            log_printf("[cmd] pid.%s.%s = %ld\r\n", argv[1], argv[2], int_val);
        } else {
            log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
        }
        return 0;
    }

    return -1;
}

void callback_task(void)
{
    char cmd_buf[MAX_CMD_LEN];
    char disp_buf[MAX_CMD_LEN];
    cmd_parser_t parser;

    g_cmd_parser.state     = USER_CMD_IDLE;
    g_cmd_parser.index     = 0;
    g_cmd_parser.cmd_queue = g_cmd_queue;

    cmd_parser_init(&parser);
    cmd_parser_register(&parser, "help",       cmd_help);
    cmd_parser_register(&parser, "motor",      cmd_motor);
    cmd_parser_register(&parser, "sensor",     cmd_sensor);
    cmd_parser_register(&parser, "sensorcali", cmd_sensorcali);
    cmd_parser_register(&parser, "pid",        cmd_pid);

    while (1) {
        osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever);
        if (qstat != osOK) continue;
        if (cmd_buf[0] == '\0') continue;

        memcpy(disp_buf, cmd_buf, MAX_CMD_LEN);
        if (cmd_parser_dispatch(&parser, cmd_buf, NULL) != 0) {
            log_printf("[cmd] unknown: %s\r\n", disp_buf);
        }
    }
}