/*
 * 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"
#include "B03_Para_01_100.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 子优先级
uint32_t Operating_times=0;       // 统计运行时间
uint32_t Total_times=0;

/*=========================== 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] = {
        // 未实现
    },

    [Emergency_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_LANE_CHANGE_UP]    = Robot_Stop,
		[EMERGENCE_STOP]     = Emergency_Stop_group,
    }

};

/*=========================== 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_flag=0;   //上电检测电机及sbus没问题代表机器人运行正常，可以进行错误检测了
	if(Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == CONNECTED
		&&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == CONNECTED)
		&&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == CONNECTED)
		&&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == CONNECTED)
		&&Is_All_Button_Reset==1)
	{
		start_flag=1;
		//SystemErrorCode = 0;
	}




	if(start_flag==1)
	{
		error_detcet=AbnormalDetect();
		IV.robot_start=2;  //表示机器人成功启动
		if(IV.reason_of_robot_error==0)   //只获取最初的错误原因
		{
			IV.reason_of_robot_error = error_detcet;
		}
	}


    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&&(start_flag==1))
    {
    	robot_start_flag=1;
    	PV_control();
        prev_mode = RobotRockerState();
        prev_key = curr_key;

        //若有错误只保留手动模式
        if(error_detcet)
        {
        	prev_mode=Emergency_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();
    	get_swing_mode();
    	PaintGun_Contronl();
    }

    flag_reset();

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

    //tuigan_auto();

    watch_dog=DevMon_IsOnline(DEV_LEFT_MOTOR);

    //统计机器人总运行时间
    Operating_times++;
    if(Operating_times>30000)
    {
    	Operating_times=0;
    	CV = GF_BSP_EEPROM_Get_CV();
    	Total_times=CV.operating_times+1;
    	g_B03_param_table[ROBOT_NUMBER].operating_times=Total_times;
    	memcpy(&CV, &g_B03_param_table[ROBOT_NUMBER], sizeof(CV));
    	GF_BSP_EEPROM_Set_CV(CV);
    }

}

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

// ====================== 第一步：先假设正常 ======================
	int is_error = 0;

	/* SBUS出错 */
	if (Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == DISCONNECTED)
	{
		is_error = 1;
	}
	/* 地面端出错 */
	else if(Get_BIT(SystemErrorCode, ComError_Ground_Management) == DISCONNECTED)
	{
		is_error = 3;
	}
	/* 遥控器关机或失联 */
	else if (P_MK32->IsOnline == 0)
	{
		is_error = 6;
	}
	/* 陀螺仪无信号 */
	else if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
	{
		is_error = 4;
	}
	/* 按钮未复位 */
	else if (Get_BIT(SystemErrorCode, ComError_MK32_InitialState) != Has_Reset)
	{
		is_error = 7;
	}
	/* 电机失联 */
	else if(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)
	{
		is_error = 8;
	}
	/* 串口出错 */
	else if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == DISCONNECTED)
	{
		if(GV.wire_status==1)  //无线连接状态
		{
			is_error = 2;
		}
	}
	else
	{
		// 完全正常 → 立刻清0计数
		cnt = 0;
		return 0;
	}

	// ====================== 第二步：统一计数 ======================
	cnt++;

	// ====================== 第三步：需要延迟触发的错误 ======================
	if (is_error == 1 || is_error == 3 ||is_error == 6||is_error == 8)
	{
		if (cnt > 1000)
		{
			// 触发软急停
			GV.GroundManagementValue.MaualControlPower = 1;
			GV.GroundManagementValue.MaualPowerState = 0;
			cnt = 0;

		}
		if (is_error == 1) return 1;
		if (is_error == 3) return 3;
		if (is_error == 6) return 6;
		if (is_error == 8) return 8;
	}

	// ====================== 第四步：不需要延迟，直接触发 ======================
	else if (is_error == 2)
	{
		return 2;
	}
	else if (is_error == 4)
	{
		return 4;
	}
	else if(is_error == 5)
	{
		return 5;
	}

	// 正在计数，还没到触发条件
	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);
}