#include "app_CiA402.h"

/**
 * @brief  辅助函数：CiA402 状态机管理
 * @note   使用 cia402_defs.h 标准宏定义重构，确保状态流转逻辑与原版本一致
 */
void Process_StateMachine(void)
{
    // 1. 获取对象字典条目句柄
    OD_entry_t *entry_ctrl = OD_find(OD, CIA402_INDEX_CONTROLWORD);
    OD_entry_t *entry_status = OD_find(OD, CIA402_INDEX_STATUSWORD);

    uint16_t cw = 0;
    uint16_t sw = 0;

    // 2. 读取当前的控制字与状态字
    OD_get_u16(entry_ctrl, 0, &cw, true);
    OD_get_u16(entry_status, 0, &sw, true);

    // 3. 屏蔽状态字低 7 位 (Bit 0-6)，准备根据当前状态重新填充反馈位
    sw &= 0xFF80;

    // 4. 标准 CiA402 状态流转逻辑处理
    switch (internal_state)
    {
    case STATE_SWITCH_ON_DISABLED:
        // 检查是否收到 Shutdown 命令 (对应 0x0006)
        if ((cw & CIA402_CMD_SHUTDOWN) == CIA402_CMD_SHUTDOWN)
            internal_state = STATE_READY_TO_SWITCH_ON;
        // 反馈状态：Switch on disabled
        sw |= CIA402_STATUS_SWITCH_ON_DISABLED;
        break;

    case STATE_READY_TO_SWITCH_ON:
        // 检查是否收到 Switch On 命令 (对应 0x0007)
        if ((cw & CIA402_CMD_SWITCH_ON) == CIA402_CMD_SWITCH_ON)
            internal_state = STATE_SWITCHED_ON;
        // 反馈状态：Ready to switch on + Quick stop (标志正常运行位)
        sw |= (CIA402_STATUS_READY_TO_SWITCH_ON | CIA402_STATUS_QUICK_STOP);
        break;

    case STATE_SWITCHED_ON:
        // 检查是否收到 Enable Operation 命令 (对应 0x000F)
        if ((cw & CIA402_CMD_ENABLE_OP) == CIA402_CMD_ENABLE_OP)
            internal_state = STATE_OPERATION_ENABLED;
        else if ((cw & CIA402_CMD_SWITCH_ON) != CIA402_CMD_SWITCH_ON)
            internal_state = STATE_READY_TO_SWITCH_ON;
        // 反馈状态：Switched on 等组合
        sw |= (CIA402_STATUS_READY_TO_SWITCH_ON | CIA402_STATUS_SWITCHED_ON | CIA402_STATUS_QUICK_STOP);
        break;

    case STATE_OPERATION_ENABLED:
        // 如果控制字不再满足使能条件，退回上一状态
        if ((cw & CIA402_CMD_ENABLE_OP) != CIA402_CMD_ENABLE_OP)
            internal_state = STATE_SWITCHED_ON;
        // 反馈状态：使能运行全标志 (对应掩码 0x0027)
        sw |= CIA402_STATUS_MASK_OP_ENABLE;
        break;

    default:
        internal_state = STATE_SWITCH_ON_DISABLED;
        break;
    }

    // 5. 更新状态字到对象字典
    OD_set_u16(entry_status, 0, sw, true);
}

// 根据脉冲更新绝对位置并写入 OD
void Update_Motion_State_To_OD(void)
{
    // 1. 获取 OD 条目句柄
    OD_entry_t *entry_pos = OD_find(OD, CIA402_INDEX_POS_ACTUAL);
    OD_entry_t *entry_vel = OD_find(OD, CIA402_INDEX_VEL_ACTUAL);
    OD_entry_t *entry_sw = OD_find(OD, CIA402_INDEX_STATUSWORD);

    if (!entry_pos || !entry_vel || !entry_sw)
        return;

    // 2. 物理位置逻辑计算
    float dist_moved_mm = (float)stepper_1.step_count / STEPS_PER_MM;

    if (stepper_1.dir == GPIO_PIN_RESET)
        current_absolute_pos_mm = last_move_start_pos_mm + dist_moved_mm;
    else
        current_absolute_pos_mm = last_move_start_pos_mm - dist_moved_mm;

    // 3. 计算并四舍五入位置值
    int32_t pos_to_set = 0;
    if (current_absolute_pos_mm >= 0)
        pos_to_set = (int32_t)(current_absolute_pos_mm + 0.5f);
    else
        pos_to_set = (int32_t)(current_absolute_pos_mm - 0.5f);

    // 4. 获取当前速度值
    int32_t vel_to_set = (stepper_1.state != STOP) ? (int32_t)stepper_1.current_speed : 0;

    // 5. 写入 OD (使用锁保护原子性)
    // 注意：电机板作为从机，通常直接使用 CO 指针访问
    CO_LOCK_OD(CO->CANmodule);

    OD_set_i32(entry_pos, 0, pos_to_set, true);
    OD_set_i32(entry_vel, 0, vel_to_set, true);

    // 6. 更新状态字 (Bit 10: Target Reached)
    uint16_t sw = 0;
    OD_get_u16(entry_sw, 0, &sw, true);

    if (stepper_1.state == STOP)
    {
        // 仅在非回零或回零完成且 Operation Enabled 时置位
        if (homing_state == HOMING_IDLE || homing_state == HOMING_DONE)
        {
            // 使用掩码检查 Ready+SwOn+OpEn+NoQuickStop (0x0027)
            if ((sw & CIA402_STATUS_MASK_OP_ENABLE) == CIA402_STATUS_MASK_OP_ENABLE)
            {
                sw |= CIA402_STATUS_TARGET_REACHED; // 置位目标到达 (Bit 10)
            }
        }
    }
    else
    {
        // 运动中清除已到达标志
        sw &= ~CIA402_STATUS_TARGET_REACHED;
    }

    OD_set_u16(entry_sw, 0, sw, true);
    CO_UNLOCK_OD(CO->CANmodule);
}

// @brief  处理 Mode 6 (回零模式) 的逻辑
void Handle_Homing_Mode(void)
{
    // 1. 获取 OD 条目句柄
    OD_entry_t *entry_ctrl = OD_find(OD, CIA402_INDEX_CONTROLWORD);
    OD_entry_t *entry_status = OD_find(OD, CIA402_INDEX_STATUSWORD);
    OD_entry_t *entry_pos = OD_find(OD, CIA402_INDEX_POS_ACTUAL);

    if (!entry_ctrl || !entry_status || !entry_pos)
        return;

    uint16_t cw = 0;
    OD_get_u16(entry_ctrl, 0, &cw, true); // 读取控制字

    // A. 启动回零 (Bit 4 上升沿)
    if ((cw & CIA402_CONTROL_HM_START) && homing_state == HOMING_IDLE)
    {
        debug_printf("[App] Homing Start...");
        last_move_start_pos_mm = current_absolute_pos_mm;

        stepper_1.dir = GPIO_PIN_SET; // 物理向右 (找开关)
        stepper_1.distance = 230.0f;  // 设大距离
        stepper_1.speed = 5.0f;      // 慢速
        stepper_1.acc = 10.0f;
        stepper_1.start_speed = 1.0f;

        stepper_1.x_zero = 0; // 清除标志
        Int_TMC2209_start(&stepper_1, &encoder_1);

        homing_state = HOMING_MOVING;

        // 原子化更新状态字：清除 Bit 10 (Target Reached) 和 Bit 12 (Homing Attained)
        CO_LOCK_OD(CO->CANmodule);
        uint16_t sw = 0;
        OD_get_u16(entry_status, 0, &sw, true);
        sw &= ~(CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12);
        OD_set_u16(entry_status, 0, sw, true);
        CO_UNLOCK_OD(CO->CANmodule);
    }

    // B. 正在回零 (检测开关)
    if (homing_state == HOMING_MOVING)
    {
        if (stepper_1.x_zero == 1) // 撞到开关,到达零点
        {
            Int_TMC2209_stop();
            stepper_1.state = STOP;   // 强制停止状态
            stepper_1.step_count = 0; // 清除底层脉冲

            /* --- 核心修复：原子化重置坐标系，防止 TPDO 跳变 --- */
            CO_LOCK_OD(CO->CANmodule);

            // 1. 内部浮点坐标重置
            current_absolute_pos_mm = 0.0f;
            last_move_start_pos_mm = 0.0f;

            // 2. 通过接口更新对象字典位置为 0
            // 这会自动标记 TPDO 为待发送状态
            OD_set_i32(entry_pos, 0, 0, true);

            // 3. 更新状态字 (Bit 10: Reached, Bit 12: Attained)
            uint16_t sw = 0;
            OD_get_u16(entry_status, 0, &sw, true);
            sw |= (CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12);
            OD_set_u16(entry_status, 0, sw, true);

            CO_UNLOCK_OD(CO->CANmodule);

            homing_state = HOMING_DONE;
            debug_printf("[App] Homing Success, Position Reset to 0.");
        }
        else if (stepper_1.state == STOP) // 没撞开关就停了
        {
            homing_state = HOMING_IDLE;
        }
    }
}

// @brief 处理 Mode 1 (位置模式) 的逻辑
void Handle_Position_Mode(void)
{
    homing_state = HOMING_IDLE; // 复位回零状态
    static uint16_t last_cw = 0;

    // 1. 获取 OD 条目句柄
    OD_entry_t *entry_cw = OD_find(OD, CIA402_INDEX_CONTROLWORD);
    OD_entry_t *entry_sw = OD_find(OD, CIA402_INDEX_STATUSWORD);
    OD_entry_t *entry_tpos = OD_find(OD, CIA402_INDEX_TARGET_POS);
    OD_entry_t *entry_tvel = OD_find(OD, CIA402_INDEX_TARGET_VEL);
    OD_entry_t *entry_acc = OD_find(OD, CIA402_INDEX_PROFILE_ACC);
    OD_entry_t *entry_dec = OD_find(OD, CIA402_INDEX_PROFILE_DEC);

    if (!entry_cw || !entry_sw || !entry_tpos || !entry_tvel || !entry_acc || !entry_dec)
        return;

    uint16_t cw = 0;
    OD_get_u16(entry_cw, 0, &cw, true); // 读取当前控制字

    // A. 触发新运动 (Bit 4 上升沿：New set-point)
    if ((cw & CIA402_CONTROL_PP_NEW_SET_POINT) && !(last_cw & CIA402_CONTROL_PP_NEW_SET_POINT))
    {
        int32_t target_pos_raw = 0;
        int32_t target_vel_raw = 0;
        uint32_t target_acc_raw = 0;
        uint32_t target_dec_raw = 0;

        // 通过接口获取目标参数
        OD_get_i32(entry_tpos, 0, &target_pos_raw, true);
        OD_get_i32(entry_tvel, 0, &target_vel_raw, true);
        OD_get_u32(entry_acc, 0, &target_acc_raw, true);
        OD_get_u32(entry_dec, 0, &target_dec_raw, true);

        float target_pos = (float)target_pos_raw;
        float target_vel = (float)target_vel_raw;
        float target_acc = (float)target_acc_raw;
        float target_dec = (float)target_dec_raw;

        if (target_pos > SOFT_LIMIT_MAX_MM || target_pos < SOFT_LIMIT_MIN_MM)
        {
            // 如果超出 230mm 或小于 0mm，拦截指令并报错
            debug_printf("[App] ERROR: Target %.1f out of Range [0, 230]! Command Ignored.\r\n", target_pos);

            // 为了完成 CiA402 握手，即便不移动也需要置位 Bit 12 (Ack)
            CO_LOCK_OD(CO->CANmodule);
            uint16_t sw = 0;
            OD_get_u16(entry_sw, 0, &sw, true); // 读取当前状态字
            sw |= CIA402_STATUS_OMS_12;
            OD_set_u16(entry_sw, 0, sw, true);
            CO_UNLOCK_OD(CO->CANmodule);
            last_cw = cw;
            return;
        }

        // 默认参数保护
        if (target_vel < 0.1f)
            target_vel = 5.0f;
        if (target_acc < 0.1f)
            target_acc = 50.0f;
        if (target_dec < 0.1f)
            target_dec = 50.0f;

        float delta = target_pos - current_absolute_pos_mm;
        debug_printf("[App] PP Move: Tgt=%.1f, Delta=%.1f", target_pos, delta);

        // 设置方向与行程
        if (delta >= 0)
        {
            stepper_1.dir = GPIO_PIN_RESET;
            stepper_1.distance = delta;
        }
        else
        {
            stepper_1.dir = GPIO_PIN_SET;
            stepper_1.distance = -delta;
        }

        stepper_1.speed = target_vel;
        stepper_1.acc = target_acc;
        stepper_1.dec = target_dec;
        stepper_1.start_speed = 5.0f;

        // 启动运动或直接置位到达标志
        CO_LOCK_OD(CO->CANmodule); // 进入保护区修改状态字
        uint16_t sw = 0;
        OD_get_u16(entry_sw, 0, &sw, true);

        /* 清除丢步标志位 */
        sw &= (uint16_t)(~CIA402_STATUS_OMS_13);

        if (stepper_1.distance > 0.05f)
        {
            last_move_start_pos_mm = current_absolute_pos_mm; // 记录起点
            Int_TMC2209_start(&stepper_1, &encoder_1);

            sw &= ~CIA402_STATUS_TARGET_REACHED; // 清除 Target Reached (Bit 10)
            sw |= CIA402_STATUS_OMS_12;          // 置位 Set-point Acknowledge (Bit 12)
        }
        else
        {
            sw |= (CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12); // 已在目标位置
        }

        OD_set_u16(entry_sw, 0, sw, true);
        CO_UNLOCK_OD(CO->CANmodule);
    }

    // B. 握手信号处理 (Bit 4 下降沿清除 Ack 反馈)
    if (!(cw & 0x0010) && (last_cw & 0x0010))
    {
        CO_LOCK_OD(CO->CANmodule);
        uint16_t sw = 0;
        OD_get_u16(entry_sw, 0, &sw, true);
        sw &= ~CIA402_STATUS_OMS_12; // 清除 Set-point Acknowledge
        OD_set_u16(entry_sw, 0, sw, true);
        CO_UNLOCK_OD(CO->CANmodule);
    }

    last_cw = cw;
}