#include "app.h"
#include "cmsis_os2.h"
#include <string.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 1"          — 使能电机 1
 *   "motor enable 2"          — 使能电机 2
 *   "motor disable"           — 关闭全部电机（惰行 coast）
 *   "motor disable 1"         — 关闭电机 1
 *   "motor disable 2"         — 关闭电机 2
 *   "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);
}

void callback_task(void)
{
    char cmd_buf[MAX_CMD_LEN];

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

    while (1) {
        osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever);
        if (qstat != osOK) {
            continue;
        }
        if (strcmp(cmd_buf, "help") == 0) {
            log_printf("[cmd] available commands:\r\n"
                   "  help                    ==> show this message\r\n"
                   "  motor enable [1|2] ==> enable motor(s), default both\r\n"
                   "  motor disable [1|2] ==> 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 <motor> <p> <v>   ==> set PID param (v÷10=实际值, motor disabled)\r\n"
                   "      motor: 1|2, p: kp|ki|kd, v: float\r\n");
        } else if (strncmp(cmd_buf, "motor enable", 12) == 0) {
            const char *p = cmd_buf + 12;
            while (*p == ' ') p++;
            if (*p == '\0') {
                gimbal_motor_enable();
                log_printf("[cmd] both motors enabled\r\n");
            } else if (strcmp(p, "1") == 0) {
                gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
                log_printf("[cmd] motor 1 enabled\r\n");
            } else if (strcmp(p, "2") == 0) {
                gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
                log_printf("[cmd] motor 2 enabled\r\n");
            } else {
                log_printf("[cmd] unknown motor: %s\r\n", p);
            }
        } else if (strncmp(cmd_buf, "motor disable", 13) == 0) {
            const char *p = cmd_buf + 13;
            while (*p == ' ') p++;
            if (*p == '\0') {
                gimbal_motor_disable();
                log_printf("[cmd] both motors disabled\r\n");
            } else if (strcmp(p, "1") == 0) {
                gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
                log_printf("[cmd] motor 1 disabled\r\n");
            } else if (strcmp(p, "2") == 0) {
                gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
                log_printf("[cmd] motor 2 disabled\r\n");
            } else {
                log_printf("[cmd] unknown motor: %s\r\n", p);
            }
        } else if (strcmp(cmd_buf, "sensor enable") == 0) {
            sensor_enable();
            log_printf("[cmd] sensor enabled\r\n");
        } else if (strcmp(cmd_buf, "sensor disable") == 0) {
            sensor_disable();
            log_printf("[cmd] sensor disabled\r\n");
        } else if (strcmp(cmd_buf, "sensorcali acc enable") == 0) {
            sensor_acc_cali_enable();
            log_printf("[cmd] acc calibration started\r\n");
        } else if (strcmp(cmd_buf, "sensorcali acc disable") == 0) {
            sensor_acc_cali_disable();
            log_printf("[cmd] acc calibration stopped\r\n");
        } else if (strcmp(cmd_buf, "sensorcali mag enable") == 0) {
            sensor_mag_cali_enable();
            log_printf("[cmd] mag calibration started\r\n");
        } else if (strcmp(cmd_buf, "sensorcali mag disable") == 0) {
            sensor_mag_cali_disable();
            log_printf("[cmd] mag calibration stopped\r\n");
        } else if (strcmp(cmd_buf, "pid") == 0) {
            /* 查看当前 PID 系数（整数显示，值 = 实际系数 × 10） */
            float kp, ki, kd;
            gimbal_get_pid("roll", &kp, &ki, &kd);
            log_printf("[cmd] Motor 1: 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] Motor 2: Kp=%d  Ki=%d  Kd=%d\r\n",
                   (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
        } else if (strncmp(cmd_buf, "pid ", 4) == 0) {
            /* 调参：pid <1|2> <kp|ki|kd> <value>  （value ÷ 10 = 实际系数） */
            char param[8];
            int motor_id, int_val;
            if (sscanf(cmd_buf, "pid %d %7s %d", &motor_id, param, &int_val) == 3) {
                if (motor_id < 1 || motor_id > 2) {
                    log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
                } else if (gimbal_is_motor_enabled()) {
                    log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
                } else {
                    const char *axis = (motor_id == 1) ? "roll" : "pitch";
                    float value = (float)int_val / 10.0f;
                    if (gimbal_set_pid(axis, param, value) == 0) {
                        log_printf("[cmd] motor %d.%s = %d\r\n", motor_id, param, int_val);
                    } else {
                        log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
                    }
                }
            } else {
                log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
            }
        } else {
            log_printf("[cmd] unknown: %s\r\n", cmd_buf);
        }
    }
}