/*
 * @file Handset_Status_Setting.c
 * @brief 遥控器状态处理、模式管理、按键/摇杆检测、参数补偿驱动文件
 * @author bm673
 * @date 2026年1月15日
 * @details 实现机器人遥控模式切换、急停检测、IO状态获取、姿态补偿等功能
 */

#include "Handset_Status_Setting.h"
#include "BHBF_ROBOT.h"
#include <math.h>

/*=========================== 宏定义配置 ===========================*/
// 摇杆阈值配置
#define ROCKER_THRESHOLD_DEFAULT     300U    // 摇杆死区阈值
#define ANGLE_DEAD_ZONE              20U     // 角度死区(度)

// 机器人运动角度判断阈值
#define ANGLE_FORWARD_LEFT_MIN       70
#define ANGLE_FORWARD_LEFT_MAX       110
#define ANGLE_FORWARD_RIGHT_MIN      -110
#define ANGLE_FORWARD_RIGHT_MAX      -70
#define ANGLE_REVERSE_MIN            160

// 摇杆方向角度区间定义
#define ANGLE_FORWARD_LIMIT1         70
#define ANGLE_FORWARD_LIMIT2         110
#define ANGLE_LEFT_LIMIT1            160
#define ANGLE_LEFT_LIMIT2            -160
#define ANGLE_RIGHT_LIMIT1           -20
#define ANGLE_RIGHT_LIMIT2           20
#define ANGLE_BACK_LIMIT1            -70
#define ANGLE_BACK_LIMIT2            -110

// 姿态补偿参数配置
#define COMP_DEADZONE                300     // 遥控器补偿生效阈值
#define COMP_COUNT_THRESH            200     // 补偿计数阈值(400ms累加一次)
#define COMP_STEP                    10      // 补偿步长
#define COMP_LIMIT_MAX               1000    // 正向补偿最大值
#define COMP_LIMIT_MIN               -1000   // 负向补偿最大值

/*=========================== 静态函数声明 ===========================*/
typedef InputEvent (*ModeEventHandler)(void);

// 急停检测
static inline InputEvent CheckEmergencyStop(void);
// 各模式事件处理函数
static InputEvent GetHaltModeEvents(void);
static InputEvent GetManualModeEvents(void);
static InputEvent GetHorizontalModeEvents(void);
static InputEvent GetFlatModeEvents(void);
static InputEvent GetVerticalLeftModeEvents(void);
static InputEvent GetVerticalRightModeEvents(void);
static InputEvent GetRegionalFlatTaskEvents(void);
static InputEvent GetRegionalHorizontalTaskEvents(void);
// 辅助功能函数
static InputEvent CheckCommonKeys(void);
static InputEvent CheckCommonKeys_to_manual(void);
static InputEvent CheckAllKeys(void);
static InputEvent CalculateRockerEvent(void);
static InputEvent CalculateRockerEvent_manual(void);
static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt);

/*=========================== 全局变量 ===========================*/
extern float left_compare_value;
extern float right_compare_value;
// 模式-事件处理函数映射表
static const ModeEventHandler modeEventHandlers[MODE_COUNT] = {
    [Halt_Mode]                       = GetHaltModeEvents,
    [Manual_Mode]                     = GetManualModeEvents,
    [Horizontal_Mode]                 = GetHorizontalModeEvents,
    [Flat_Mode]                       = GetFlatModeEvents,
    [Vertical_Mode_Left]              = GetVerticalLeftModeEvents,
    [Vertical_Mode_Right]             = GetVerticalRightModeEvents,
    [Regional_Horizontal_Automatic_Task] = GetRegionalHorizontalTaskEvents,
    [Regional_Flat_Automatic_Task]    = GetRegionalFlatTaskEvents,
};

/*=========================== 函数实现 ===========================*/
/**
 * @brief  获取机器人当前运行模式
 * @param  无
 * @return Robot_Mode 当前机器人模式枚举
 */
Robot_Mode RobotRockerState(void)
{
    if (GV.PV.Robot_Operation_Mode <= 0 || GV.PV.Robot_Operation_Mode >= MODE_COUNT)
    {
        return Halt_Mode;
    }

    switch (GV.PV.Robot_Operation_Mode)
    {
        case 0:  return Halt_Mode;
        case 1:  return Manual_Mode;
        case 2:  return Horizontal_Mode;
        case 3:  return Flat_Mode;
        case 4:  return Vertical_Mode_Left;
        case 5:  return Vertical_Mode_Right;
        case 6:  return Regional_Horizontal_Automatic_Task;
        case 7:  return Regional_Flat_Automatic_Task;
        default: return Halt_Mode;
    }
}

/**
 * @brief  遥控器按键/事件主检测函数
 * @param  current_mode 当前机器人模式
 * @return InputEvent 触发的输入事件
 */
InputEvent RemoteControl_GetKeyIndex(Robot_Mode current_mode)
{
    // 安全校验
    if (current_mode < 0 || current_mode >= MODE_COUNT)
    {
        return INPUT_NONE;
    }

    // 获取对应模式的事件处理函数并执行
    ModeEventHandler handler = modeEventHandlers[current_mode];
    return (handler != NULL) ? handler() : INPUT_NONE;
}

/**
 * @brief  通用常用按键检测（手动/水平/垂直等模式共用）
 * @param  无
 * @return InputEvent 按键事件
 */
static inline InputEvent CheckCommonKeys(void)
{
    if (P_MK32->CH7_SD == -1000)  return INPUT_KEY_AUTO_WORK_UP;
    if (P_MK32->CH4_SA == -1000)  return INPUT_KEY_LANE_CHANGE_UP;
    if (P_MK32->CH5_SB == -1000)  return INPUT_KEY_FORWARD_CRUISE;
    if (P_MK32->CH5_SB == 1000)   return INPUT_KEY_BACKWARD_CRUISE;

    return INPUT_NONE;
}

/**
 * @brief  通用常用按键检测（手动/水平/垂直等模式共用）
 * @param  无
 * @return InputEvent 按键事件
 */
static inline InputEvent CheckCommonKeys_to_manual(void)
{
    if (P_MK32->CH7_SD == -1000)  return INPUT_KEY_AUTO_WORK_UP;
    if (P_MK32->CH5_SB == -1000)  return INPUT_KEY_FORWARD_CRUISE;
    if (P_MK32->CH5_SB == 1000)   return INPUT_KEY_BACKWARD_CRUISE;


    return INPUT_NONE;
}

/**
 * @brief  全按键检测（停机模式专用）
 * @param  无
 * @return InputEvent 按键事件
 */
static inline InputEvent CheckAllKeys(void)
{
    if (P_MK32->CH7_SD == -1000)  return INPUT_KEY_AUTO_WORK_UP;
    if (P_MK32->CH7_SD == 1000)   return INPUT_KEY_AUTO_WORK_DOWN;
    if (P_MK32->CH4_SA == -1000)  return INPUT_KEY_LANE_CHANGE_UP;
    if (P_MK32->CH4_SA == 1000)   return INPUT_KEY_LANE_CHANGE_DOWN;
    if (P_MK32->CH5_SB == -1000)  return INPUT_KEY_FORWARD_CRUISE;
    if (P_MK32->CH5_SB == 1000)   return INPUT_KEY_BACKWARD_CRUISE;


    return INPUT_NONE;
}

/**
 * @brief  遥控器摇杆方向计算与事件转换
 * @param  无
 * @return InputEvent 摇杆方向事件
 */
static InputEvent CalculateRockerEvent(void)
{
    int16_t vert = P_MK32->CH2_LY_V;
    int16_t hori = P_MK32->CH3_LY_H;
    int16_t swing = P_MK32->CH0_RY_H;

    // 摇杆中位判断
    if ((fabs(vert) < ROCKER_THRESHOLD_DEFAULT) && (fabs(hori) < ROCKER_THRESHOLD_DEFAULT))
    {
        return INPUT_ROCKER_STOP;
    }

    // 角度计算
    double angle_rad = atan2(vert, hori);
    int16_t angle_deg = (int16_t)(angle_rad * 180.0 / M_PI);

    // 方向判断
    if (angle_deg <= ANGLE_FORWARD_LIMIT2 && angle_deg >= ANGLE_FORWARD_LIMIT1)
    {
        return INPUT_ROCKER_FORWARD;
    }
    if (angle_deg >= ANGLE_LEFT_LIMIT1 || angle_deg <= ANGLE_LEFT_LIMIT2)
    {
        return INPUT_ROCKER_TURN_LEFT;
    }
    if (angle_deg >= ANGLE_RIGHT_LIMIT1 && angle_deg <= ANGLE_RIGHT_LIMIT2)
    {
        return INPUT_ROCKER_TURN_RIGHT;
    }
    if (angle_deg >= ANGLE_BACK_LIMIT2 && angle_deg <= ANGLE_BACK_LIMIT1)
    {
        return INPUT_ROCKER_BACKWARD;
    }


    return INPUT_ROCKER_STOP;
}




/**
 * @brief  遥控器摇杆方向计算与事件转换（用于手动模式）
 * @param  无
 * @return InputEvent 摇杆方向事件
 */
static InputEvent CalculateRockerEvent_manual(void)
{
    int16_t vert = P_MK32->CH2_LY_V;
    int16_t hori = P_MK32->CH3_LY_H;

    // 摇杆中位判断
    if ((fabs(vert) < ROCKER_THRESHOLD_DEFAULT) && (fabs(hori) < ROCKER_THRESHOLD_DEFAULT))
    {
        if (P_MK32->CH4_SA == -1000)
        {
        	return INPUT_KEY_LANE_CHANGE_UP;
        }

        return INPUT_ROCKER_STOP;
    }

    // 角度计算
    double angle_rad = atan2(vert, hori);
    int16_t angle_deg = (int16_t)(angle_rad * 180.0 / M_PI);

    // 方向判断
    if (angle_deg <= ANGLE_FORWARD_LIMIT2 && angle_deg >= ANGLE_FORWARD_LIMIT1)
    {
        return INPUT_ROCKER_FORWARD;
    }
    if (angle_deg >= ANGLE_LEFT_LIMIT1 || angle_deg <= ANGLE_LEFT_LIMIT2)
    {
        return INPUT_ROCKER_TURN_LEFT;
    }
    if (angle_deg >= ANGLE_RIGHT_LIMIT1 && angle_deg <= ANGLE_RIGHT_LIMIT2)
    {
        return INPUT_ROCKER_TURN_RIGHT;
    }
    if (angle_deg >= ANGLE_BACK_LIMIT2 && angle_deg <= ANGLE_BACK_LIMIT1)
    {
        return INPUT_ROCKER_BACKWARD;
    }



    return INPUT_ROCKER_STOP;
}
/**
 * @brief  急停检测（最高优先级）
 * @param  无
 * @return InputEvent 急停事件/无事件
 */
static inline InputEvent CheckEmergencyStop(void)
{
    if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
    {
        return EMERGENCE_STOP;
    }
    return INPUT_NONE;
}

/*-------------------------- 各模式事件处理函数 --------------------------*/
static InputEvent GetHaltModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckAllKeys();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}

static InputEvent GetManualModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckCommonKeys_to_manual();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent_manual();
}

static InputEvent GetHorizontalModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckCommonKeys();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}

static InputEvent GetFlatModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckCommonKeys();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}

static InputEvent GetVerticalLeftModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckCommonKeys();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}

static InputEvent GetVerticalRightModeEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    key = CheckCommonKeys();
    return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}

static InputEvent GetRegionalHorizontalTaskEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
    return CalculateRockerEvent();
}

static InputEvent GetRegionalFlatTaskEvents(void)
{
    InputEvent key = CheckEmergencyStop();
    if (key != INPUT_NONE) return key;

    if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
    return CalculateRockerEvent();
}

/**
 * @brief  IO状态检测（升降控制）
 * @param  无
 * @return IO_State IO状态枚举
 */
IO_State GetIOState(void)
{
    if (P_MK32->CH1_RY_V < -300) return IO_STATE_RISE;
    if (P_MK32->CH1_RY_V > 300)  return IO_STATE_DESCEND;
    return IO_STATE_NONE;
}

/**
 * @brief  PV参数更新控制
 * @param  无
 * @return 无
 */
void PV_control(void)
{
    GV.PV = decoded_PV_variable;
    GV.Robot_Move_Speed          = ((float)GV.PV.Robot_Move_Speed) / 10;
    GV.LaneChangeDistance        = GV.PV.Robot_Change_Lane_Distance;
    GV.Vertical_Adjust           = ((float)GV.PV.Robot_Vertical_Adjust) / 10;
    GV.symmetricalOrNot          = GV.PV.Robot_symmetricalOrNot;
    GV.Robot_backMode            = GV.PV.Robot_backMode;
    GV.Robot_Swing_Speed         = GV.PV.Robot_Swing_Speed;
    GV.robot_back_distance       = 10;//GV.PV.Robot_Back_Distance;
}

/**
 * @brief  IV状态参数更新
 * @param  无
 * @return 无
 */
void IV_control(void)
{
	GV.PV = decoded_PV_variable;
	GV.Vertical_Adjust           = ((float)GV.PV.Robot_Vertical_Adjust) / 10;
	GV.symmetricalOrNot          = GV.PV.Robot_symmetricalOrNot;
	GV.Robot_Swing_Speed         = GV.PV.Robot_Swing_Speed;
    IV.SystemError           = GV.SystemErrorData.Com_Error_Code;
    IV.Robot_Gyro            = GV.TL720DParameters.RF_Angle_Roll;
    IV.Is_Online             = GV.P_MK32.IsOnline;
    IV.Robot_Move_Deri_Speed = GV.Left_Speed_M_min * 10;
    IV.LeftCompensation      = GV.Left_Compensation;
    IV.RightCompensation     = GV.Right_Compensation;
    IV.Weld_data             = (int32_t)(GV.weld_data_X*100);
    IV.Weld_exist            = GV.weld_exist;
    IV.Present_press         = GV.Now_press/10;
    IV.Turn_difference       = GV.turn_center_difference;
    IV.left_angle            = left_compare_value;
    IV.right_angle            = right_compare_value;
}

/**
 * @brief  单通道姿态补偿更新
 * @param  chVal   遥控器通道值
 * @param  pComp   补偿变量指针
 * @param  pCnt    补偿计数器指针
 * @return 无
 */
static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt)
{
    if (chVal > COMP_DEADZONE)
    {
        (*pCnt)++;
        if (*pCnt > COMP_COUNT_THRESH)
        {
            *pComp += COMP_STEP;
            *pComp = (*pComp > COMP_LIMIT_MAX) ? COMP_LIMIT_MAX : *pComp;
            *pCnt = 0;
        }
    }
    else if (chVal < -COMP_DEADZONE)
    {
        (*pCnt)--;
        if (*pCnt < -COMP_COUNT_THRESH)
        {
            *pComp -= COMP_STEP;
            *pComp = (*pComp < COMP_LIMIT_MIN) ? COMP_LIMIT_MIN : *pComp;
            *pCnt = 0;
        }
    }
    else
    {
        *pCnt = 0;
    }
}

/**
 * @brief  左右轮姿态补偿总更新函数
 * @param  无
 * @return 无
 */
void Compensation_Update(void)
{
    static int leftCnt  = 0;
    static int rightCnt = 0;

//    static float Left_Compensation_Degre_100  = 0;
//    static float Right_Compensation_Degre_100 = 0;
//
//    GV.Left_Compensation  = Left_Compensation_Degre_100  / 100.0f;
//    GV.Right_Compensation = Right_Compensation_Degre_100 / 100.0f;

    Update_Single_Compensation(P_MK32->CH14_LT, &GV.Left_Compensation, &leftCnt);
    Update_Single_Compensation(P_MK32->CH15_RT, &GV.Right_Compensation, &rightCnt);
}

void get_weld_data(void)
{
	GV.weld_data_X = ReadLazorData->Feature_X;
	GV.weld_exist = *Weld_Out_Flag;   // 16说明没有焊缝  0说明有焊缝
	if((*Weld_Out_Flag)==0)
	{
		GV.weld_exist = 1;   // 16说明没有焊缝  0说明有焊缝
	}
	else
	{
		GV.weld_exist = 0;
	}
}