/*
 * CMD 模块 V2 - 主控制模块实现
 */
#include "cmd.h"
#include "bsp/time.h"
#include <stdint.h>
#include <string.h>

/* ========================================================================== */
/*                           内部辅助函数                                       */
/* ========================================================================== */

/* 从RC输入生成底盘命令 */
static void CMD_RC_BuildChassisCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
    
    /* 根据左拨杆位置选择模式 */
    switch (input->sw[0]) {
        case CMD_SW_UP:
            ctx->chassis.cmd.mode = map->sw_left_up;
            break;
        case CMD_SW_MID:
            ctx->chassis.cmd.mode = map->sw_left_mid;
            break;
        case CMD_SW_DOWN:
            ctx->chassis.cmd.mode = map->sw_left_down;
            break;
        default:
            ctx->chassis.cmd.mode = CHASSIS_MODE_RELAX;
            break;
    }
    
    /* 摇杆控制移动 */
    ctx->chassis.cmd.ctrl_vec.vx = input->joy_right.x;
    ctx->chassis.cmd.ctrl_vec.vy = input->joy_right.y;
}

/* 从RC输入生成云台命令 */
static void CMD_RC_BuildGimbalCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
    
    /* 根据拨杆选择云台模式 */
    switch (input->sw[0]) {
        case CMD_SW_UP:
            ctx->gimbal.cmd.mode = map->gimbal_sw_up;
            break;
        case CMD_SW_MID:
            ctx->gimbal.cmd.mode = map->gimbal_sw_mid;
            break;
        case CMD_SW_DOWN:
            ctx->gimbal.cmd.mode = map->gimbal_sw_down;
            break;
        default:
            ctx->gimbal.cmd.mode = GIMBAL_MODE_RELAX;
            break;
    }
    
    /* 左摇杆控制云台 */
    ctx->gimbal.cmd.delta_yaw = -input->joy_left.x * 2.0f;
    ctx->gimbal.cmd.delta_pit = -input->joy_left.y * 1.5f;
}

/* 从RC输入生成射击命令 */
static void CMD_RC_BuildShootCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    if (input->online) {
        ctx->shoot.cmd.mode = SHOOT_MODE_SINGLE;
    } else {
        ctx->shoot.cmd.mode = SHOOT_MODE_SAFE;
    }
    
    /* 根据右拨杆控制射击 */
    switch (input->sw[1]) {
        case CMD_SW_DOWN:
            ctx->shoot.cmd.ready = true;
            ctx->shoot.cmd.firecmd = true;
            break;
        case CMD_SW_MID:
            ctx->shoot.cmd.ready = true;
            ctx->shoot.cmd.firecmd = false;
            break;
        case CMD_SW_UP:
        default:
            ctx->shoot.cmd.ready = false;
            ctx->shoot.cmd.firecmd = false;
            break;
    }
}

/* 从PC输入生成底盘命令 */
static void CMD_PC_BuildChassisCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
    
    if (!input->online) {
        ctx->chassis.cmd.mode = CHASSIS_MODE_RELAX;
        return;
    }
    
    ctx->chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
    
    /* WASD控制移动 */
    ctx->chassis.cmd.ctrl_vec.vx = 0.0f;
    ctx->chassis.cmd.ctrl_vec.vy = 0.0f;
    
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_W)) {
        ctx->chassis.cmd.ctrl_vec.vy += sens->move_sens;
    }
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_S)) {
        ctx->chassis.cmd.ctrl_vec.vy -= sens->move_sens;
    }
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_A)) {
        ctx->chassis.cmd.ctrl_vec.vx -= sens->move_sens;
    }
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_D)) {
        ctx->chassis.cmd.ctrl_vec.vx += sens->move_sens;
    }
    
    /* 加速/减速 */
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_SHIFT)) {
        ctx->chassis.cmd.ctrl_vec.vx *= sens->move_fast_mult;
        ctx->chassis.cmd.ctrl_vec.vy *= sens->move_fast_mult;
    }
    if (CMD_KEY_PRESSED(&input->keyboard, CMD_KEY_CTRL)) {
        ctx->chassis.cmd.ctrl_vec.vx *= sens->move_slow_mult;
        ctx->chassis.cmd.ctrl_vec.vy *= sens->move_slow_mult;
    }
}

/* 从PC输入生成云台命令 */
static void CMD_PC_BuildGimbalCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
    
    if (!input->online) {
        ctx->gimbal.cmd.mode = GIMBAL_MODE_RELAX;
        return;
    }
    
    ctx->gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
    
    /* 鼠标控制云台 */
    ctx->gimbal.cmd.delta_yaw = (float)-input->mouse.x * ctx->timer.dt * sens->mouse_sens;
    ctx->gimbal.cmd.delta_pit = (float)input->mouse.y * ctx->timer.dt * sens->mouse_sens * 1.5f;
}

/* 从PC输入生成射击命令 */
static void CMD_PC_BuildShootCmd(CMD_Context_t *ctx, const CMD_RawInput_t *input) {
    if (!input->online) {
        ctx->shoot.cmd.mode = SHOOT_MODE_SAFE;
        return;
    }
    
    ctx->shoot.cmd.ready = true;
    ctx->shoot.cmd.firecmd = input->mouse.l_click;
}

/* 离线安全模式 */
static void CMD_SetOfflineMode(CMD_Context_t *ctx) {
    ctx->chassis.cmd.mode = CHASSIS_MODE_RELAX;
    ctx->gimbal.cmd.mode = GIMBAL_MODE_RELAX;
    ctx->shoot.cmd.mode = SHOOT_MODE_SAFE;
}

/* ========================================================================== */
/*                           公开API实现                                        */
/* ========================================================================== */

int8_t CMD_Init(CMD_Context_t *ctx, CMD_Config_t *config) {
    if (ctx == NULL || config == NULL) {
        return CMD_ERR_NULL;
    }
    
    memset(ctx, 0, sizeof(CMD_Context_t));
    ctx->config = config;
    
    /* 初始化适配器 */
    CMD_Adapter_InitAll();
    
    /* 初始化行为处理器 */
    CMD_Behavior_Init();
    
    return CMD_OK;
}

int8_t CMD_UpdateInput(CMD_Context_t *ctx) {
    if (ctx == NULL) {
        return CMD_ERR_NULL;
    }
    
    /* 保存上一帧输入 */
    memcpy(ctx->last_input, ctx->input, sizeof(ctx->input));
    
    /* 更新所有输入源 */
    for (int i = 0; i < CMD_SRC_NUM; i++) {
        CMD_Adapter_GetInput((CMD_InputSource_t)i, &ctx->input[i]);
    }
    
    return CMD_OK;
}

int8_t CMD_Arbitrate(CMD_Context_t *ctx) {
    if (ctx == NULL) {
        return CMD_ERR_NULL;
    }
    
    /* 智能仲裁算法：优先级 PC > RC > NUC */
    CMD_InputSource_t candidates[] = {CMD_SRC_PC, CMD_SRC_RC, CMD_SRC_NUC};
    const int num_candidates = sizeof(candidates) / sizeof(candidates[0]);
    
    /* 如果当前输入源仍然在线且有效，保持使用 */
    if (ctx->active_source < CMD_SRC_NUM && 
        ctx->active_source != CMD_SRC_REF && 
        ctx->input[ctx->active_source].online) {
        goto seize;
    }
    
    /* 否则选择第一个可用的控制输入源 */
    for (int i = 0; i < num_candidates; i++) {
        CMD_InputSource_t src = candidates[i];
        if (ctx->input[src].online) {
            ctx->active_source = src;
        }
    }
    ctx->chassis.source = ctx->active_source;
    ctx->gimbal.source = ctx->active_source;
    ctx->shoot.source = ctx->active_source;

    seize:
    



    /* 没有可用输入源 */
    ctx->active_source = CMD_SRC_NUM;
    return CMD_OK;
}

int8_t CMD_GenerateCommands(CMD_Context_t *ctx) {
    if (ctx == NULL) {
        return CMD_ERR_NULL;
    }
    
    /* 更新时间 */
    uint64_t now_us = BSP_TIME_Get_us();
    ctx->timer.now = now_us / 1000000.0f;
    ctx->timer.dt = (now_us - ctx->timer.last_us) / 1000000.0f;
    ctx->timer.last_us = now_us;
    
    /* 没有有效输入源 */
    if (ctx->active_source >= CMD_SRC_NUM) {
        CMD_SetOfflineMode(ctx);
        return CMD_ERR_NO_INPUT;
    }
    
    const CMD_RawInput_t *active_input = &ctx->input[ctx->active_source];
    const CMD_RawInput_t *last_input = &ctx->last_input[ctx->active_source];
    
    /* 根据输入源类型生成命令 */
    switch (ctx->active_source) {
        case CMD_SRC_RC:
            CMD_RC_BuildChassisCmd(ctx, active_input);
            CMD_RC_BuildGimbalCmd(ctx, active_input);
            CMD_RC_BuildShootCmd(ctx, active_input);
            break;
            
        case CMD_SRC_PC:
            CMD_PC_BuildChassisCmd(ctx, active_input);
            CMD_PC_BuildGimbalCmd(ctx, active_input);
            CMD_PC_BuildShootCmd(ctx, active_input);
            
            /* 处理行为 */
            CMD_Behavior_ProcessAll(ctx, active_input, last_input, CMD_MODULE_ALL);
            break;
            
        case CMD_SRC_NUC:
            /* TODO: NUC输入处理 */
            break;
            
        default:
            CMD_SetOfflineMode(ctx);
            return CMD_ERR_SOURCE;
    }
    
    return CMD_OK;
}

int8_t CMD_Update(CMD_Context_t *ctx) {
    int8_t ret;
    
    ret = CMD_UpdateInput(ctx);
    if (ret != CMD_OK) return ret;
    
    CMD_Arbitrate(ctx);
    
    ret = CMD_GenerateCommands(ctx);
    return ret;
}
