/**
 * @file mrobot.c
 * @brief MRobot CLI 实现
 */

/* Includes ----------------------------------------------------------------- */
#include "device/mrobot.h"
#include "device/device.h"
#include "device/motor.h"
#include "component/freertos_cli.h"
#include "bsp/uart.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <FreeRTOS.h>
#include <task.h>
#include <semphr.h>
#include <cmsis_os2.h>

/* Private constants -------------------------------------------------------- */
static const char *const CLI_WELCOME_MESSAGE =
    "\r\n"
    "  ______         __           _______               __              \r\n"
    " |   __ \\.-----.|  |--.-----.|   |   |.---.-.-----.|  |-.-----.-----.\r\n"
    " |      <|  _  ||  _  |  _  ||       ||  _  |__ --||   _|  -__|   _|\r\n"
    " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__|  \r\n"
    " -------------------------------------------------------------------\r\n"
    " FreeRTOS CLI. Type 'help' to view a list of registered commands.   \r\n"
    "\r\n";

/* ANSI 转义序列 */
#define ANSI_CLEAR_SCREEN   "\033[2J\033[H"
#define ANSI_CURSOR_HOME    "\033[H"
#define ANSI_BACKSPACE      "\b \b"

/* 弧度转角度常量 */
#define RAD_TO_DEG          57.2957795131f

/* Private types ------------------------------------------------------------ */
/* CLI 上下文结构体 - 封装所有状态 */
typedef struct {
    /* 设备管理 */
    MRobot_Device_t devices[MROBOT_MAX_DEVICES];
    uint8_t device_count;
    
    /* 自定义命令 */
    CLI_Command_Definition_t *custom_cmds[MROBOT_MAX_CUSTOM_COMMANDS];
    uint8_t custom_cmd_count;
    
    /* CLI 状态 */
    MRobot_State_t state;
    char current_path[MROBOT_PATH_MAX_LEN];
    
    /* 命令缓冲区 */
    uint8_t cmd_buffer[MROBOT_CMD_BUFFER_SIZE];
    volatile uint8_t cmd_index;
    volatile bool cmd_ready;
    
    /* UART 相关 */
    uint8_t uart_rx_char;
    volatile bool tx_complete;
    volatile bool htop_exit;
    
    /* 输出缓冲区 */
    char output_buffer[MROBOT_OUTPUT_BUFFER_SIZE];
    
    /* 初始化标志 */
    bool initialized;
    
    /* 互斥锁 */
    SemaphoreHandle_t mutex;
} MRobot_Context_t;

/* Private variables -------------------------------------------------------- */
static MRobot_Context_t ctx = {
    .device_count = 0,
    .custom_cmd_count = 0,
    .state = MROBOT_STATE_IDLE,
    .current_path = "/",
    .cmd_index = 0,
    .cmd_ready = false,
    .tx_complete = true,
    .htop_exit = false,
    .initialized = false,
    .mutex = NULL
};

/* Private function prototypes ---------------------------------------------- */
/* 命令处理函数 */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);

/* 内部辅助函数 */
static void uart_tx_callback(void);
static void uart_rx_callback(void);
static void send_string(const char *str);
static void send_prompt(void);
static int format_float(char *buf, size_t size, float val, int precision);
static int print_imu_device(const void *device_data, char *buffer, size_t buffer_size);
static int print_motor_device(const void *device_data, char *buffer, size_t buffer_size);

/* CLI 命令定义表 */
static const CLI_Command_Definition_t builtin_commands[] = {
    { "help", "help: 显示所有可用命令\r\n", cmd_help, 0 },
    { "htop", "htop: 动态显示 FreeRTOS 任务状态 (按 'q' 退出)\r\n", cmd_htop, 0 },
    { "cd",   "cd <path>: 切换目录\r\n", cmd_cd, 1 },
    { "ls",   "ls: 列出当前目录内容\r\n", cmd_ls, 0 },
    { "show", "show [device] [count]: 显示设备信息\r\n", cmd_show, -1 },
    { "pwd",  "pwd: 显示当前目录\r\n", cmd_pwd, 0 },
};
#define BUILTIN_CMD_COUNT (sizeof(builtin_commands) / sizeof(builtin_commands[0]))

/* ========================================================================== */
/*                              辅助函数实现                                    */
/* ========================================================================== */

/**
 * @brief 格式化浮点数为字符串（避免嵌入式 printf 浮点支持问题）
 */
static int format_float(char *buf, size_t size, float val, int precision) {
    if (buf == NULL || size == 0) return 0;
    
    int offset = 0;
    
    /* 处理负数 */
    if (val < 0) {
        if (offset < (int)size - 1) buf[offset++] = '-';
        val = -val;
    }
    
    /* 计算乘数 */
    int multiplier = 1;
    for (int i = 0; i < precision; i++) multiplier *= 10;
    
    int int_part = (int)val;
    int frac_part = (int)((val - int_part) * multiplier + 0.5f);
    
    /* 处理进位 */
    if (frac_part >= multiplier) {
        int_part++;
        frac_part -= multiplier;
    }
    
    /* 格式化输出 */
    int written = snprintf(buf + offset, size - offset, "%d.%0*d", int_part, precision, frac_part);
    return (written > 0) ? (offset + written) : offset;
}

/**
 * @brief 发送字符串到 UART（阻塞等待完成）
 */
static void send_string(const char *str) {
    if (str == NULL || *str == '\0') return;
    
    ctx.tx_complete = false;
    BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)str, strlen(str), true);
    while (!ctx.tx_complete) { osDelay(1); }
}

/**
 * @brief 发送命令提示符
 */
static void send_prompt(void) {
    char prompt[MROBOT_PATH_MAX_LEN + 20];
    snprintf(prompt, sizeof(prompt), "root@mrobot:%s$ ", ctx.current_path);
    send_string(prompt);
}

/**
 * @brief UART 发送完成回调
 */
static void uart_tx_callback(void) {
    ctx.tx_complete = true;
}

/**
 * @brief UART 接收回调
 */
static void uart_rx_callback(void) {
    uint8_t ch = ctx.uart_rx_char;
    
    /* htop 模式下检查退出键 */
    if (ctx.state == MROBOT_STATE_HTOP) {
        if (ch == 'q' || ch == 'Q' || ch == 27) {
            ctx.htop_exit = true;
        }
        BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
        return;
    }
    
    /* 正常命令输入处理 */
    if (ch == '\r' || ch == '\n') {
        if (ctx.cmd_index > 0) {
            ctx.cmd_buffer[ctx.cmd_index] = '\0';
            ctx.cmd_ready = true;
            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)"\r\n", 2, false);
        }
    } else if (ch == 127 || ch == 8) {  /* 退格键 */
        if (ctx.cmd_index > 0) {
            ctx.cmd_index--;
            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)ANSI_BACKSPACE, 3, false);
        }
    } else if (ch >= 32 && ch < 127 && ctx.cmd_index < sizeof(ctx.cmd_buffer) - 1) {
        ctx.cmd_buffer[ctx.cmd_index++] = ch;
        BSP_UART_Transmit(MROBOT_UART_PORT, &ch, 1, false);
    }
    
    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
}

/* ========================================================================== */
/*                            设备打印函数实现                                   */
/* ========================================================================== */

/**
 * @brief IMU 设备打印函数
 */
static int print_imu_device(const void *device_data, char *buffer, size_t buffer_size) {
    if (device_data == NULL || buffer == NULL || buffer_size == 0) return 0;
    
    const DEVICE_IMU_t *imu = (const DEVICE_IMU_t *)device_data;
    
    char ax[16], ay[16], az[16];
    char gx[16], gy[16], gz[16];
    char temp[16];
    char roll[16], pitch[16], yaw[16];
    
    format_float(ax, sizeof(ax), imu->accl.x, 3);
    format_float(ay, sizeof(ay), imu->accl.y, 3);
    format_float(az, sizeof(az), imu->accl.z, 3);
    format_float(gx, sizeof(gx), imu->gyro.x, 3);
    format_float(gy, sizeof(gy), imu->gyro.y, 3);
    format_float(gz, sizeof(gz), imu->gyro.z, 3);
    format_float(temp, sizeof(temp), imu->temp, 2);
    format_float(roll, sizeof(roll), imu->euler.rol * RAD_TO_DEG, 2);
    format_float(pitch, sizeof(pitch), imu->euler.pit * RAD_TO_DEG, 2);
    format_float(yaw, sizeof(yaw), imu->euler.yaw * RAD_TO_DEG, 2);
    
    return snprintf(buffer, buffer_size,
        "状态:     %s\r\n"
        "加速度计: X=%s Y=%s Z=%s m/s²\r\n"
        "陀螺仪:   X=%s Y=%s Z=%s rad/s\r\n"
        "温度:     %s °C\r\n"
        "欧拉角:   Roll=%s Pitch=%s Yaw=%s °\r\n",
        imu->header.online ? "在线" : "离线",
        ax, ay, az,
        gx, gy, gz,
        temp,
        roll, pitch, yaw);
}

/**
 * @brief 电机设备打印函数
 */
static int print_motor_device(const void *device_data, char *buffer, size_t buffer_size) {
    if (device_data == NULL || buffer == NULL || buffer_size == 0) return 0;
    
    const MOTOR_t *motor = (const MOTOR_t *)device_data;
    
    char angle[16], speed[16], current[16], temp[16];
    
    format_float(angle, sizeof(angle), motor->feedback.rotor_abs_angle, 2);
    format_float(speed, sizeof(speed), motor->feedback.rotor_speed, 2);
    format_float(current, sizeof(current), motor->feedback.torque_current, 2);
    format_float(temp, sizeof(temp), motor->feedback.temp, 1);
    
    return snprintf(buffer, buffer_size,
        "状态:     %s\r\n"
        "反装:     %s\r\n"
        "角度:     %s °\r\n"
        "转速:     %s RPM\r\n"
        "电流:     %s A\r\n"
        "温度:     %s °C\r\n",
        motor->header.online ? "在线" : "离线",
        motor->reverse ? "是" : "否",
        angle, speed, current, temp);
}

/* ========================================================================== */
/*                             CLI 命令实现                                     */
/* ========================================================================== */

/**
 * @brief help 命令 - 显示帮助信息
 */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    (void)pcCommandString;
    
    int offset = snprintf(pcWriteBuffer, xWriteBufferLen,
        "MRobot CLI v2.0\r\n"
        "================\r\n"
        "内置命令:\r\n");
    
    for (size_t i = 0; i < BUILTIN_CMD_COUNT && offset < (int)xWriteBufferLen - 50; i++) {
        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
            "  %s", builtin_commands[i].pcHelpString);
    }
    
    if (ctx.custom_cmd_count > 0) {
        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
            "\r\n自定义命令:\r\n");
        for (uint8_t i = 0; i < ctx.custom_cmd_count && offset < (int)xWriteBufferLen - 50; i++) {
            if (ctx.custom_cmds[i] != NULL) {
                offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                    "  %s", ctx.custom_cmds[i]->pcHelpString);
            }
        }
    }
    
    return pdFALSE;
}

/**
 * @brief htop 命令 - 设置 htop 模式标志
 */
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    (void)pcCommandString;
    (void)pcWriteBuffer;
    (void)xWriteBufferLen;
    /* htop 模式在 MRobot_Run 中处理 */
    return pdFALSE;
}

/**
 * @brief pwd 命令 - 显示当前路径
 */
static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    (void)pcCommandString;
    snprintf(pcWriteBuffer, xWriteBufferLen, "%s\r\n", ctx.current_path);
    return pdFALSE;
}

/**
 * @brief cd 命令 - 切换目录
 */
static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    const char *param;
    BaseType_t param_len;
    
    param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
    
    if (param == NULL) {
        /* 无参数时切换到根目录 */
        strcpy(ctx.current_path, "/");
        snprintf(pcWriteBuffer, xWriteBufferLen, "切换到: %s\r\n", ctx.current_path);
        return pdFALSE;
    }
    
    /* 安全复制路径参数 */
    char path[MROBOT_PATH_MAX_LEN];
    size_t copy_len = (size_t)param_len < sizeof(path) - 1 ? (size_t)param_len : sizeof(path) - 1;
    strncpy(path, param, copy_len);
    path[copy_len] = '\0';
    
    /* 路径解析 */
    if (strcmp(path, "/") == 0 || strcmp(path, "..") == 0 || strcmp(path, "~") == 0) {
        strcpy(ctx.current_path, "/");
    } else if (strcmp(path, "dev") == 0 || strcmp(path, "/dev") == 0) {
        strcpy(ctx.current_path, "/dev");
    } else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) {
        strcpy(ctx.current_path, "/modules");
    } else {
        snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 目录 '%s' 不存在\r\n", path);
        return pdFALSE;
    }
    
    snprintf(pcWriteBuffer, xWriteBufferLen, "切换到: %s\r\n", ctx.current_path);
    return pdFALSE;
}

/**
 * @brief ls 命令 - 列出目录内容
 */
static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    (void)pcCommandString;
    int offset = 0;
    
    if (strcmp(ctx.current_path, "/") == 0) {
        snprintf(pcWriteBuffer, xWriteBufferLen, 
            "dev/\r\n"
            "modules/\r\n");
    } else if (strcmp(ctx.current_path, "/dev") == 0) {
        offset = snprintf(pcWriteBuffer, xWriteBufferLen, "设备列表 (%d 个):\r\n", ctx.device_count);
        
        if (ctx.device_count == 0) {
            snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, "  (无设备)\r\n");
        } else {
            /* 按类型分组显示 */
            static const char *type_names[] = { "IMU", "电机", "传感器", "自定义" };
            for (uint8_t t = 0; t < MROBOT_DEVICE_TYPE_NUM && offset < (int)xWriteBufferLen - 50; t++) {
                bool has_type = false;
                for (uint8_t i = 0; i < ctx.device_count; i++) {
                    if (ctx.devices[i].type == t) {
                        if (!has_type) {
                            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                                "  [%s]\r\n", type_names[t]);
                            has_type = true;
                        }
                        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                            "    %s\r\n", ctx.devices[i].name);
                    }
                }
            }
        }
    } else if (strcmp(ctx.current_path, "/modules") == 0) {
        snprintf(pcWriteBuffer, xWriteBufferLen, "(模块功能暂未实现)\r\n");
    }
    
    return pdFALSE;
}

/**
 * @brief show 命令 - 显示设备信息
 */
static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
    const char *param;
    const char *count_param;
    BaseType_t param_len, count_param_len;
    
    /* 使用局部静态变量跟踪多次打印状态 */
    static uint32_t print_count = 0;
    static uint32_t current_iter = 0;
    static char target_device[MROBOT_DEVICE_NAME_LEN] = {0};
    
    /* 首次调用时解析参数 */
    if (current_iter == 0) {
        /* 检查是否在 /dev 目录 */
        if (strcmp(ctx.current_path, "/dev") != 0) {
            snprintf(pcWriteBuffer, xWriteBufferLen, 
                "错误: show 命令仅在 /dev 目录下可用\r\n"
                "提示: 使用 'cd /dev' 切换到设备目录\r\n");
            return pdFALSE;
        }
        
        param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
        count_param = FreeRTOS_CLIGetParameter(pcCommandString, 2, &count_param_len);
        
        /* 解析打印次数 */
        print_count = 1;
        if (count_param != NULL) {
            char count_str[16];
            size_t copy_len = (size_t)count_param_len < sizeof(count_str) - 1 ? 
                              (size_t)count_param_len : sizeof(count_str) - 1;
            strncpy(count_str, count_param, copy_len);
            count_str[copy_len] = '\0';
            int parsed = atoi(count_str);
            if (parsed > 0 && parsed <= 1000) {
                print_count = (uint32_t)parsed;
            }
        }
        
        /* 保存目标设备名称 */
        if (param != NULL) {
            size_t copy_len = (size_t)param_len < sizeof(target_device) - 1 ? 
                              (size_t)param_len : sizeof(target_device) - 1;
            strncpy(target_device, param, copy_len);
            target_device[copy_len] = '\0';
        } else {
            target_device[0] = '\0';
        }
    }
    
    int offset = 0;
    
    /* 连续打印模式：清屏 */
    if (print_count > 1) {
        offset = snprintf(pcWriteBuffer, xWriteBufferLen, "%s[%lu/%lu]\r\n",
                          ANSI_CLEAR_SCREEN,
                          (unsigned long)(current_iter + 1), 
                          (unsigned long)print_count);
    }
    
    if (target_device[0] == '\0') {
        /* 显示所有设备 */
        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, 
                          "=== 所有设备 ===\r\n\r\n");
        
        for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 100; i++) {
            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                              "--- %s ---\r\n", ctx.devices[i].name);
            
            if (ctx.devices[i].print_cb != NULL) {
                int written = ctx.devices[i].print_cb(ctx.devices[i].data,
                                                       pcWriteBuffer + offset,
                                                       xWriteBufferLen - offset);
                offset += (written > 0) ? written : 0;
            } else {
                offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                                  "(无打印函数)\r\n");
            }
            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, "\r\n");
        }
        
        if (ctx.device_count == 0) {
            offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                              "(无已注册设备)\r\n");
        }
    } else {
        /* 显示指定设备 */
        const MRobot_Device_t *dev = MRobot_FindDevice(target_device);
        
        if (dev == NULL) {
            snprintf(pcWriteBuffer, xWriteBufferLen, 
                    "错误: 设备 '%s' 未找到\r\n", target_device);
            current_iter = 0;
            return pdFALSE;
        }
        
        offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                          "=== %s ===\r\n", dev->name);
        
        if (dev->print_cb != NULL) {
            dev->print_cb(dev->data, pcWriteBuffer + offset, xWriteBufferLen - offset);
        } else {
            snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
                    "(无打印函数)\r\n");
        }
    }
    
    /* 判断是否继续打印 */
    current_iter++;
    if (current_iter < print_count) {
        osDelay(MROBOT_HTOP_REFRESH_MS);
        return pdTRUE;
    } else {
        current_iter = 0;
        return pdFALSE;
    }
}

/* ========================================================================== */
/*                              htop 模式实现                                   */
/* ========================================================================== */

/**
 * @brief 处理 htop 模式显示
 */
static void handle_htop_mode(void) {
    /* 清屏 */
    send_string(ANSI_CLEAR_SCREEN);
    
    /* 显示头部 */
    send_string(
        "MRobot Task Monitor (按 'q' 退出)\r\n"
        "================================================================================\r\n"
        "Task Name        State    Prio Stack    Num\r\n"
        "--------------------------------------------------------------------------------\r\n");
    
    /* 获取任务列表 */
    char task_buffer[1024];
    char display_line[128];
    
    vTaskList(task_buffer);
    
    /* 解析并格式化任务列表 */
    char *line = strtok(task_buffer, "\r\n");
    while (line != NULL) {
        char name[17] = {0};
        char state_char = '?';
        int prio = 0, stack = 0, num = 0;
        
        if (sscanf(line, "%16s %c %d %d %d", name, &state_char, &prio, &stack, &num) == 5) {
            const char *state_str;
            switch (state_char) {
                case 'R': state_str = "Running"; break;
                case 'B': state_str = "Blocked"; break;
                case 'S': state_str = "Suspend"; break;
                case 'D': state_str = "Deleted"; break;
                case 'X': state_str = "Ready";   break;
                default:  state_str = "Unknown"; break;
            }
            
            snprintf(display_line, sizeof(display_line),
                    "%-16s %-8s %-4d %-8d %-4d\r\n",
                    name, state_str, prio, stack, num);
            send_string(display_line);
        }
        line = strtok(NULL, "\r\n");
    }
    
    /* 显示系统信息 */
    snprintf(display_line, sizeof(display_line),
            "--------------------------------------------------------------------------------\r\n"
            "System Tick: %lu | Free Heap: %lu bytes\r\n",
            (unsigned long)xTaskGetTickCount(),
            (unsigned long)xPortGetFreeHeapSize());
    send_string(display_line);
    
    /* 检查退出 */
    if (ctx.htop_exit) {
        ctx.state = MROBOT_STATE_IDLE;
        ctx.htop_exit = false;
        send_string(ANSI_CLEAR_SCREEN);
        send_prompt();
    }
    
    osDelay(MROBOT_HTOP_REFRESH_MS);
}

/* ========================================================================== */
/*                              公共 API 实现                                   */
/* ========================================================================== */

void MRobot_Init(void) {
    if (ctx.initialized) return;
    
    /* 创建互斥锁 */
    ctx.mutex = xSemaphoreCreateMutex();
    
    /* 初始化状态 */
    memset(ctx.devices, 0, sizeof(ctx.devices));
    ctx.device_count = 0;
    ctx.custom_cmd_count = 0;
    ctx.state = MROBOT_STATE_IDLE;
    strcpy(ctx.current_path, "/");
    ctx.cmd_index = 0;
    ctx.cmd_ready = false;
    ctx.tx_complete = true;
    ctx.htop_exit = false;
    
    /* 注册内置命令 */
    for (size_t i = 0; i < BUILTIN_CMD_COUNT; i++) {
        FreeRTOS_CLIRegisterCommand(&builtin_commands[i]);
    }
    
    /* 注册 UART 回调 */
    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_RX_CPLT_CB, uart_rx_callback);
    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_TX_CPLT_CB, uart_tx_callback);
    
    /* 启动 UART 接收 */
    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
    
    /* 等待用户按下回车 */
    while (ctx.uart_rx_char != '\r' && ctx.uart_rx_char != '\n') {
        osDelay(10);
    }
    
    /* 发送欢迎消息和提示符 */
    send_string(CLI_WELCOME_MESSAGE);
    send_prompt();
    
    ctx.initialized = true;
}

void MRobot_DeInit(void) {
    if (!ctx.initialized) return;
    
    /* 释放自定义命令内存 */
    for (uint8_t i = 0; i < ctx.custom_cmd_count; i++) {
        if (ctx.custom_cmds[i] != NULL) {
            vPortFree(ctx.custom_cmds[i]);
            ctx.custom_cmds[i] = NULL;
        }
    }
    
    /* 删除互斥锁 */
    if (ctx.mutex != NULL) {
        vSemaphoreDelete(ctx.mutex);
        ctx.mutex = NULL;
    }
    
    ctx.initialized = false;
}

MRobot_State_t MRobot_GetState(void) {
    return ctx.state;
}

MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
                                      void *data, MRobot_PrintCallback_t print_cb) {
    if (name == NULL || data == NULL) {
        return MROBOT_ERR_NULL_PTR;
    }
    
    if (ctx.device_count >= MROBOT_MAX_DEVICES) {
        return MROBOT_ERR_FULL;
    }
    
    if (type >= MROBOT_DEVICE_TYPE_NUM) {
        return MROBOT_ERR_INVALID_ARG;
    }
    
    /* 检查重名 */
    for (uint8_t i = 0; i < ctx.device_count; i++) {
        if (strcmp(ctx.devices[i].name, name) == 0) {
            return MROBOT_ERR_INVALID_ARG;  /* 设备名已存在 */
        }
    }
    
    /* 线程安全写入 */
    if (ctx.mutex != NULL) {
        xSemaphoreTake(ctx.mutex, portMAX_DELAY);
    }
    
    strncpy(ctx.devices[ctx.device_count].name, name, MROBOT_DEVICE_NAME_LEN - 1);
    ctx.devices[ctx.device_count].name[MROBOT_DEVICE_NAME_LEN - 1] = '\0';
    ctx.devices[ctx.device_count].type = type;
    ctx.devices[ctx.device_count].data = data;
    ctx.devices[ctx.device_count].print_cb = print_cb;
    ctx.device_count++;
    
    if (ctx.mutex != NULL) {
        xSemaphoreGive(ctx.mutex);
    }
    
    return MROBOT_OK;
}

MRobot_Error_t MRobot_UnregisterDevice(const char *name) {
    if (name == NULL) {
        return MROBOT_ERR_NULL_PTR;
    }
    
    if (ctx.mutex != NULL) {
        xSemaphoreTake(ctx.mutex, portMAX_DELAY);
    }
    
    for (uint8_t i = 0; i < ctx.device_count; i++) {
        if (strcmp(ctx.devices[i].name, name) == 0) {
            /* 移动后续设备 */
            for (uint8_t j = i; j < ctx.device_count - 1; j++) {
                ctx.devices[j] = ctx.devices[j + 1];
            }
            ctx.device_count--;
            
            if (ctx.mutex != NULL) {
                xSemaphoreGive(ctx.mutex);
            }
            return MROBOT_OK;
        }
    }
    
    if (ctx.mutex != NULL) {
        xSemaphoreGive(ctx.mutex);
    }
    return MROBOT_ERR_NOT_FOUND;
}

MRobot_Error_t MRobot_RegisterIMU(const char *name, DEVICE_IMU_t *imu_device) {
    return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_IMU, imu_device, print_imu_device);
}

MRobot_Error_t MRobot_RegisterMotor(const char *name, void *motor) {
    return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_MOTOR, motor, print_motor_device);
}

MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text,
                                       MRobot_CommandCallback_t callback, int8_t param_count) {
    if (command == NULL || help_text == NULL || callback == NULL) {
        return MROBOT_ERR_NULL_PTR;
    }
    
    if (ctx.custom_cmd_count >= MROBOT_MAX_CUSTOM_COMMANDS) {
        return MROBOT_ERR_FULL;
    }
    
    /* 动态分配命令结构体 */
    CLI_Command_Definition_t *cmd_def = pvPortMalloc(sizeof(CLI_Command_Definition_t));
    if (cmd_def == NULL) {
        return MROBOT_ERR_ALLOC;
    }
    
    /* 初始化命令定义 */
    *(const char **)&cmd_def->pcCommand = command;
    *(const char **)&cmd_def->pcHelpString = help_text;
    *(pdCOMMAND_LINE_CALLBACK *)&cmd_def->pxCommandInterpreter = (pdCOMMAND_LINE_CALLBACK)callback;
    cmd_def->cExpectedNumberOfParameters = param_count;
    
    /* 注册到 FreeRTOS CLI */
    FreeRTOS_CLIRegisterCommand(cmd_def);
    
    ctx.custom_cmds[ctx.custom_cmd_count] = cmd_def;
    ctx.custom_cmd_count++;
    
    return MROBOT_OK;
}

uint8_t MRobot_GetDeviceCount(void) {
    return ctx.device_count;
}

const MRobot_Device_t *MRobot_FindDevice(const char *name) {
    if (name == NULL) return NULL;
    
    for (uint8_t i = 0; i < ctx.device_count; i++) {
        if (strcmp(ctx.devices[i].name, name) == 0) {
            return &ctx.devices[i];
        }
    }
    return NULL;
}

MRobot_Error_t MRobot_Print(const char *str) {
    if (str == NULL) return MROBOT_ERR_NULL_PTR;
    if (!ctx.initialized) return MROBOT_ERR_NOT_INIT;
    
    send_string(str);
    return MROBOT_OK;
}

int MRobot_Printf(const char *fmt, ...) {
    if (fmt == NULL || !ctx.initialized) return -1;
    
    char buffer[MROBOT_OUTPUT_BUFFER_SIZE];
    va_list args;
    va_start(args, fmt);
    int len = vsnprintf(buffer, sizeof(buffer), fmt, args);
    va_end(args);
    
    if (len > 0) {
        send_string(buffer);
    }
    return len;
}

void MRobot_Run(void) {
    if (!ctx.initialized) return;
    
    /* htop 模式 */
    if (ctx.state == MROBOT_STATE_HTOP) {
        handle_htop_mode();
        return;
    }
    
    /* 处理命令 */
    if (ctx.cmd_ready) {
        ctx.state = MROBOT_STATE_PROCESSING;
        
        /* 检查是否是 htop 命令 */
        if (strcmp((char *)ctx.cmd_buffer, "htop") == 0) {
            ctx.state = MROBOT_STATE_HTOP;
            ctx.htop_exit = false;
        } else {
            /* 处理其他命令 */
            BaseType_t more;
            do {
                ctx.output_buffer[0] = '\0';
                more = FreeRTOS_CLIProcessCommand((char *)ctx.cmd_buffer,
                                                   ctx.output_buffer,
                                                   sizeof(ctx.output_buffer));
                
                if (ctx.output_buffer[0] != '\0') {
                    send_string(ctx.output_buffer);
                }
            } while (more != pdFALSE);
            
            send_prompt();
            ctx.state = MROBOT_STATE_IDLE;
        }
        
        ctx.cmd_index = 0;
        ctx.cmd_ready = false;
    }
    
    osDelay(10);
}