/*
该文件cmd用于对各种命令的中转处理，以防止在remote遥控器任务中过于冗杂堆叠
*/

/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "string.h"
#include "device/device.h"

/* Private function  -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/

void CMD_Init(CMD_t *cmd)
{

    cmd->cmd_power_on_safe = 0;
    cmd->mode = STOP;
}

static void CMD_remote(const CMD_RC_t *rc, CMD_t *cmd)
{

    if (rc->rc_type == RC_DR16)
    {

        cmd->Vx = rc->DJ.ch_r_x;
        cmd->Vy = rc->DJ.ch_r_y;
        cmd->Vw = rc->DJ.ch_l_x;
        // cmd->throttle = rc->DJ.ch_l_x;
    }
    if (rc->rc_type == RC_LD)
    {

        cmd->Vx = rc->LD.ch_r_x;
        cmd->Vy = rc->LD.ch_r_y;
        cmd->Vw = rc->LD.ch_l_x;
        //   cmd->throttle = rc->LD.ch_l_x;
    }
}
/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd)
{
    /* 机器人底盘运行模式恢复至放松模式 */
    cmd->mode = STOP;
}
void CMD_ParseRc(CMD_t *cmd, CMD_RC_t *rc)
{

    switch (rc->rc_type)
    {
    case RC_DR16:
        /*c当rc丢控时，恢复机器人至默认状态 */
        if ((rc->DJ.sw_l == CMD_SW_ERR) || (rc->DJ.sw_r == CMD_SW_ERR))
        {
            CMD_RcLostLogic(cmd);
            cmd->cmd_power_on_safe = 0;
        }
        else
        {
            /* 上电保护检查 */
            //		 if(!cmd->cmd_power_on_safe && !pos.Action_ready){
            //		    if (rc->sw_l == CMD_SW_MID && pos.Action_ready){
            //            cmd->cmd_power_on_safe = 1;
            //        }
            //				else{
            //				  // 否则保持停止状态
            //          cmd->C_cmd.mode = STOP;
            //				}
            //     }
            //		 else{
            CMD_remote(rc, cmd);
            //		 }
        }
        break;
    case RC_ET16s:
    case RC_LD:
        if (rc->LD.key_D == 0)
        {
            CMD_RcLostLogic(cmd);
        }
        else
        {
            /* 上电保护检查 */
            //			if(!cmd->cmd_power_on_safe){
            //		    if (rc->LD.key_A == CMD_SW_DOWN && rc->LD.key_D == CMD_SW_MID && rc->LD.key_E == CMD_SW_MID){
            //          cmd->cmd_power_on_safe = 1;
            //        }
            //				else{
            //				  // 否则保持停止状态
            //          cmd->C_cmd.mode = STOP;
            //				}
            //     }
            //		 else{
            CMD_remote(rc, cmd);
            //		 }
        }
        break;

    case Control_loss:
        CMD_RcLostLogic(cmd);
        break;
    }
}
int8_t CMD_CtrlSet(CMD_t *cmd, const CMD_RC_t *rc)
{

    // 大疆
    if (rc->rc_type == RC_DR16)
    {

        switch (rc->DJ.sw_l)
        {
        case CMD_SW_UP:
            cmd->mode = RC;

            break;
        case CMD_SW_DOWN:
            cmd->mode = STOP;
            break;
        case CMD_SW_MID:
            cmd->mode = STOP;
            break;
        case CMD_SW_ERR:
            cmd->mode = STOP;
            break;
        }
    }
    // 天地飞
    else if (rc->rc_type == RC_LD)
    {

        // 模式切换
        switch (rc->LD.key_A)
        {
        case CMD_SW_UP:
            cmd->mode = STOP;
            break;
        case CMD_SW_MID:
            cmd->mode = RC;
            break;
        case CMD_SW_DOWN:
            cmd->mode = STOP;
            break;
        case CMD_SW_ERR:
            cmd->mode = STOP;
            break;
        }
        switch (rc->LD.key_B)
        {
        case CMD_SW_UP:
          cmd->throttle = 1.4f;
            break;
        case CMD_SW_MID:
          cmd->throttle = 2.5f;
            break;  
        case CMD_SW_DOWN:
          cmd->throttle = 5.0f;
            break;
        case CMD_SW_ERR:
            break;
        
        default:
            break;
        }
    }
    return 0;
}