/*
 * fsm_state_control.c
 *
 *  Created on: Jan 13, 2026
 *      Author: bm673
 */

/*=========================== 1. 头文件包含 ===========================*/
#include "fsm_state_control.h"
#include "Handset_Status_Setting.h"
#include "robot_move_actions.h"
#include "paint_gun_action.h"
#include "swing_action.h"
#include "motors_power_action.h"
#include "BHBF_ROBOT.h"

/*=========================== 2. 宏定义 ===========================*/
/* 本文件无宏定义，预留 */

/*=========================== 3. 类型定义 ===========================*/
/* 本文件无自定义类型，预留 */

/*=========================== 4. 全局变量 ===========================*/
/* 调试变量（普通全局变量） */
Robot_Mode g_debug_prev_mode = Halt_Mode;
InputEvent g_debug_curr_key = INPUT_NONE;
ActionFunc g_debug_current_action_func = NULL;
int error_detcet=0;     //获取机器人错误
uint32_t NVIC_Priority_Group;      // 中断优先级分组
uint32_t TIM8_PreemptPrio;        // 定时器8 抢占优先级
uint32_t TIM8_SubPrio;            // 定时器8 子优先级

/*=========================== 5. 静态函数声明 ===========================*/
/* 组函数（仅本文件使用） */
static void manual_forward_group(void);
static void manual_backward_group(void);
static void manual_left_group(void);
static void manual_right_group(void);
static void manual_auto_group(void);
static void horizontal_forward_group(void);
static void horizontal_backward_group(void);
static void weld_auto_group(void);
static void change_Road_group(void);
static void vertical_forward_group(void);  /* 虽未在表中使用，但保留 */
static void vertical_backward_group(void);
static void horizontal_auto_group(void);
static void vertical_auto_group(void);
static void Emergency_Stop_group(void);
// 读取指定中断的抢占/子优先级，存入全局变量
void Read_IRQ_Priority(IRQn_Type IRQn, uint32_t *preempt, uint32_t *sub);

/*=========================== 6. 函数指针表 ===========================*/
/* 动作映射表：模式 × 按键 -> 执行函数 */
ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
    // Halt_Mode - 所有输入都无操作
    [Halt_Mode] = { [0 ... KEY_COUNT-1] = Robot_Stop },

    // Manual_Mode - 手动模式
    [Manual_Mode] = {
        [INPUT_ROCKER_STOP]        = Robot_Stop,
        [INPUT_ROCKER_FORWARD]     = manual_forward_group,
        [INPUT_ROCKER_BACKWARD]    = manual_backward_group,
        [INPUT_ROCKER_TURN_LEFT]   = manual_left_group,
        [INPUT_ROCKER_TURN_RIGHT]  = manual_right_group,
        [INPUT_KEY_FORWARD_CRUISE]     = manual_forward_group,
        [INPUT_KEY_BACKWARD_CRUISE]     = manual_backward_group,
        [INPUT_KEY_AUTO_WORK_UP]    = manual_auto_group,
        [INPUT_KEY_LANE_CHANGE_UP]    = Robot_Stop,
        [EMERGENCE_STOP]     = Emergency_Stop_group,

    },

    // Horizontal_Mode - 水平模式
    [Horizontal_Mode] = {
        [INPUT_ROCKER_STOP]        = Robot_Stop,
        [INPUT_ROCKER_FORWARD]     = horizontal_forward_group,
        [INPUT_ROCKER_BACKWARD]    = horizontal_backward_group,
        [INPUT_ROCKER_TURN_LEFT]     = manual_left_group,
        [INPUT_ROCKER_TURN_RIGHT]     = manual_right_group,    //等待调整 INPUT_KEY_FORWARD_CRUISE
        [INPUT_KEY_FORWARD_CRUISE]     = horizontal_forward_group,
        [INPUT_KEY_BACKWARD_CRUISE]     = horizontal_backward_group,
        [INPUT_KEY_AUTO_WORK_UP]    = horizontal_auto_group,
        [INPUT_KEY_LANE_CHANGE_UP]    = change_Road_group,
        [EMERGENCE_STOP]     = Emergency_Stop_group,
        // 其他按键暂未实现，留空（NULL）
    },

    // Flat_Mode - 暂时用于焊缝模式（预留）
    [Flat_Mode] = {
		[INPUT_ROCKER_STOP]        = Robot_Stop,
        [INPUT_ROCKER_FORWARD]     = manual_forward_group,
        [INPUT_ROCKER_BACKWARD]    = manual_backward_group,
        [INPUT_ROCKER_TURN_LEFT]   = manual_left_group,
        [INPUT_ROCKER_TURN_RIGHT]  = manual_right_group,
		[INPUT_KEY_AUTO_WORK_UP]        = weld_auto_group,
		[EMERGENCE_STOP]     = Emergency_Stop_group,
        // 暂时用于焊缝跟踪
    },

    // Vertical_Mode_Left - 左侧垂直模式
    [Vertical_Mode_Left] = {
        [INPUT_ROCKER_STOP]        = Robot_Stop,
        [INPUT_ROCKER_FORWARD]     = vertical_forward_group,  // 注意：水平前进，可能是复用
        [INPUT_ROCKER_BACKWARD]     = vertical_backward_group,
        [INPUT_ROCKER_TURN_LEFT]     = manual_left_group,
        [INPUT_ROCKER_TURN_RIGHT]     = manual_right_group,    //等待调整
        [INPUT_KEY_FORWARD_CRUISE]     = vertical_forward_group,
        [INPUT_KEY_BACKWARD_CRUISE]     = vertical_backward_group,
        [INPUT_KEY_LANE_CHANGE_UP]    = change_Road_group,
        [INPUT_KEY_AUTO_WORK_UP]    = vertical_auto_group,
        [EMERGENCE_STOP]     = Emergency_Stop_group,
        // 其他预留
    },

    // Vertical_Mode_Right - 右侧垂直模式（预留）
    [Vertical_Mode_Right] = {
		[INPUT_ROCKER_STOP]        = Robot_Stop,
		[INPUT_ROCKER_FORWARD]     = vertical_forward_group,  // 注意：水平前进，可能是复用
		[INPUT_ROCKER_BACKWARD]     = vertical_backward_group,
		[INPUT_ROCKER_TURN_LEFT]     = manual_left_group,
		[INPUT_ROCKER_TURN_RIGHT]     = manual_right_group,    //等待调整
        [INPUT_KEY_FORWARD_CRUISE]     = vertical_forward_group,
        [INPUT_KEY_BACKWARD_CRUISE]     = vertical_backward_group,
		[INPUT_KEY_LANE_CHANGE_UP]    = change_Road_group,
		[INPUT_KEY_AUTO_WORK_UP]    = vertical_auto_group,
		[EMERGENCE_STOP]     = Emergency_Stop_group,
        // 未实现
    },

    // Regional_Horizontal_Automatic_Task - 区域水平自动任务
    [Regional_Horizontal_Automatic_Task] = {
        // 未实现
    },

    // Regional_Flat_Automatic_Task - 区域平面自动任务
    [Regional_Flat_Automatic_Task] = {
        // 未实现
    }

};

/*=========================== 7. 静态函数实现 ===========================*/
/*-------------------------------------------------------------------
 * 组函数实现（静态）
 *-------------------------------------------------------------------*/
static void manual_forward_group(void)
{
    Manually_Forward();
    PaintGun_Contronl();
    Robot_Swing_Operation_Function();

}

static void manual_backward_group(void)
{
    Manually_Backward();
    PaintGun_Contronl();
    Robot_Swing_Operation_Function();

}

static void manual_left_group(void)
{
    Turn_Left();
    PaintGun_Contronl();
    Robot_Swing_Operation_Function();

}

static void manual_right_group(void)
{
    Turn_Right();
    PaintGun_Contronl();
    Robot_Swing_Operation_Function();

}

static void horizontal_forward_group(void)
{
	GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
	horizontal_forward();
	PaintGun_Contronl_Press();
	Robot_Swing_Operation_Function();

    /* 若需喷枪控制，可在此添加 */
}
static void horizontal_backward_group(void)
{
	GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
	horizontal_forward();
	PaintGun_Contronl_Press();
	Robot_Swing_Operation_Function();
    /* 若需喷枪控制，可在此添加 */
}

static void manual_auto_group(void)
{
	Move_Manual_Auto_Sub_Func();
	PaintGun_Contronl_Press();
}

static void horizontal_auto_group(void)
{
//	horizontal_work();
	Move_Horizontal_Auto_Sub_Func();
	PaintGun_Contronl_Press();
    /* 若需喷枪控制，可在此添加 */
}

static void weld_auto_group(void)
{
	GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
	Weld_Auto_Sub_Func();
	PaintGun_Contronl_Press();
    /* 若需喷枪控制，可在此添加 */
}

static void change_Road_group(void)
{
	Change_Road_Func();
	PaintGun_Contronl_Press();
    /* 若需喷枪控制，可在此添加 */
}


static void vertical_forward_group(void)
{
	GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
	vertical_forward();
	PaintGun_Contronl_Press();
	Robot_Swing_Operation_Function();
}

static void vertical_backward_group(void)
{
	GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
	vertical_forward();
	PaintGun_Contronl_Press();
	Robot_Swing_Operation_Function();
}

static void vertical_auto_group(void)
{
//	horizontal_work();
	Move_Vertical_Auto_Sub_Func();
	PaintGun_Contronl_Press();
    /* 若需喷枪控制，可在此添加 */
}

static void Emergency_Stop_group(void)
{
	Emergency_Stop_Action();
	Is_All_Button_Reset = 0;
}

/*=========================== 8. 对外接口函数（需外部调用） ===========================*/
int AbnormalDetect(void);
/*-------------------------------------------------------------------
 * 初始化函数
 *-------------------------------------------------------------------*/
void Fsm_Init(void)
{
    // 原注释掉的初始化代码
    // current_robot_move_state.p_state = &robot_halt_state;
    // current_paintgun_state.p_state   = &paintgun_off_state;
    // current_motor_power_state.p_state = &motor_power_off_state;


	GV.GroundManagementValue.MaualControlPower=0;

    GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}

/*-------------------------------------------------------------------
 * 主调度函数（2ms 周期调用）
 *-------------------------------------------------------------------*/
Robot_Mode g_debug_pre_mode = Halt_Mode;
int robot_start_flag=0;
int watch_dog=0;
void GF_Dispatch(void)
{
	Read_IRQ_Priority(TIM8_UP_TIM13_IRQn, &TIM8_PreemptPrio, &TIM8_SubPrio);
	static int32_t start_time=0;   //机器人上电启动时间约30s
	if(start_time<20000)
	{
		start_time++;
		//SystemErrorCode = 0;
	}
	else
	{
		error_detcet=AbnormalDetect();
		IV.robot_start=2;  //表示机器人成功启动
	}


    IV_control();

    static Robot_Mode prev_mode = Halt_Mode;
    static InputEvent prev_key = INPUT_ROCKER_STOP;
    static ActionFunc prev_action_func = NULL;
    g_debug_pre_mode=prev_mode;

    // 获取当前模式和按键
    InputEvent curr_key = RemoteControl_GetKeyIndex(prev_mode);

    // 仅按键变化时更新动作
    if (curr_key != prev_key)
    {
    	robot_start_flag=1;
    	PV_control();
        prev_mode = RobotRockerState();
        prev_key = curr_key;

        //若有错误只保留手动模式
        if(error_detcet)
        {
        	prev_mode=Manual_Mode;
        }

        prev_action_func = actionTable[prev_mode][prev_key];

    }

    // 执行动作
    if (prev_action_func != NULL)
    {
        prev_action_func();
        g_debug_current_action_func = prev_action_func;
    }

    if(robot_start_flag==0) //保证上电之后摆臂就能动,不加这句的话，就要先动其它摇杆，摆臂才能动
    {
    	Robot_Swing_Operation_Function();
    }

    flag_reset();

    // 更新调试变量
    g_debug_prev_mode = prev_mode;
    g_debug_curr_key  = curr_key;
    Compensation_Update();
    get_weld_data();
}

/*-------------------------------------------------------------------
 * 异常检测
 *-------------------------------------------------------------------*/
int cnt=0;
int AbnormalDetect(void)
{

    /* SBUS 出错 */
    if (Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == DISCONNECTED
    	||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == DISCONNECTED)
    	||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == DISCONNECTED)
    	||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == DISCONNECTED)
    	||(Get_BIT(SystemErrorCode, ComError_Ground_Management) == DISCONNECTED))
    {
    	cnt++;
    	if(cnt>250)
    	{
        	//触发软急停
        	GV.GroundManagementValue.MaualControlPower=1;
        	GV.GroundManagementValue.MaualPowerState=0;
        	cnt=0;
            return 1;
    	}

    }



    /* 串口出错 */
    if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == DISCONNECTED)
    {
        return 2;
    }



    /* 陀螺仪无信号 */
    if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
    {
        return 4;
    }


    /* 遥控器关机或失联 */
    if (P_MK32->IsOnline == 0)
    {
    	GV.GroundManagementValue.MaualControlPower=1;
    	GV.GroundManagementValue.MaualPowerState=0;
        Is_All_Button_Reset = 0;
        return 6;
    }


    /* 按钮未复位 */
    if (Get_BIT(SystemErrorCode, ComError_MK32_InitialState) != Has_Reset)
    {
        return 3;
    }


    return 0;
}


// 读取指定中断的抢占/子优先级，存入全局变量
void Read_IRQ_Priority(IRQn_Type IRQn, uint32_t *preempt, uint32_t *sub)
{
    // 调用你官方的4参数HAL函数
    HAL_NVIC_GetPriority(IRQn, NVIC_Priority_Group, preempt, sub);
}