// /* Includes ----------------------------------------------------------------- */
// #include <math.h>
// #include <stdint.h>
// #include <string.h>
// #include "arm.h"
// #include "bsp/mm.h"
// #include "bsp/time.h"
// #include "component/user_math.h"
// #include "device/motor_dm.h"
// #include "device/motor_lz.h"
// /* Private typedef ---------------------------------------------------------- */
// /* Private define ----------------------------------------------------------- */

// /* Private macro ------------------------------------------------------------ */
// /* Private variables -------------------------------------------------------- */

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

// // ============================================================================
// // Joint（关节）级别操作 - 面向对象接口
// // ============================================================================

// /**
//  * @brief 初始化单个关节
//  */
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
//                   const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq) {
//   if (joint == NULL || dh_params == NULL || pid_params == NULL) {
//     return -1;
//   }
  
//   joint->id = id;
//   joint->motor_type = motor_type;
//   memcpy(&joint->dh_params, dh_params, sizeof(Arm6dof_DHParams_t));
//   joint->q_offset = 0.0f;
  
//   PID_Init(&joint->pid, KPID_MODE_CALC_D, freq, pid_params);
  
//   joint->state.current_angle = 0.0f;
//   joint->state.current_velocity = 0.0f;
//   joint->state.current_torque = 0.0f;
//   joint->state.target_angle = 0.0f;
//   joint->state.target_velocity = 0.0f;
//   joint->state.online = false;
  
//   return 0;
// }

// /**
//  * @brief 更新关节状态
//  */
// int8_t Joint_Update(Joint_t *joint) {
//   if (joint == NULL) return -1;
  
//   if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
//     joint->state.current_angle = joint->motor.lz_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
//     joint->state.current_velocity = joint->motor.lz_motor.motor.feedback.rotor_speed;
//     joint->state.current_torque = joint->motor.lz_motor.motor.feedback.torque_current;
//     joint->state.online = joint->motor.lz_motor.motor.header.online;
//   } else {
//     joint->state.current_angle = joint->motor.dm_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
//     joint->state.current_velocity = joint->motor.dm_motor.motor.feedback.rotor_speed;
//     joint->state.current_torque = joint->motor.dm_motor.motor.feedback.torque_current;
//     joint->state.online = joint->motor.dm_motor.motor.header.online;
//   }
  
//   return 0;
// }

// /**
//  * @brief 关节位置控制
//  */
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt) {
//   if (joint == NULL) return -1;
  
//   if (target_angle < joint->dh_params.qmin || target_angle > joint->dh_params.qmax) {
//     return -1;
//   }
  
//   joint->state.target_angle = target_angle;
//   float pid_output = PID_Calc(&joint->pid, target_angle, joint->state.current_angle, 0, dt);
  
//   if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
//     joint->output.lz_output.target_angle = target_angle + joint->q_offset;
//     joint->output.lz_output.target_velocity = 0.0f;
//     joint->output.lz_output.kp = 10.0f;
//     joint->output.lz_output.kd = 0.5f;
//     joint->output.lz_output.torque = pid_output;
//     MOTOR_LZ_MotionControl(&joint->motor_params.lz_params, &joint->output.lz_output);
//   } else {
//     joint->output.dm_output.angle = target_angle + joint->q_offset;
//     joint->output.dm_output.velocity = 0.0f;
//     joint->output.dm_output.kp = 50.0f;
//     joint->output.dm_output.kd = 3.0f;
//     joint->output.dm_output.torque = pid_output;
//     MOTOR_DM_MITCtrl(&joint->motor_params.dm_params, &joint->output.dm_output);
//   }
  
//   return 0;
// }

// /**
//  * @brief 关节松弛
//  */
// int8_t Joint_Relax(Joint_t *joint) {
//   if (joint == NULL) return -1;
  
//   if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
//     MOTOR_LZ_Relax(&joint->motor_params.lz_params);
//   } else {
//     MOTOR_DM_Relax(&joint->motor_params.dm_params);
//   }
//   return 0;
// }

// /**
//  * @brief 检查关节是否到达目标
//  */
// bool Joint_IsReached(const Joint_t *joint, float tolerance) {
//   if (joint == NULL) return false;
//   return fabsf(joint->state.target_angle - joint->state.current_angle) < tolerance;
// }

// // ============================================================================
// // Arm（机械臂）级别操作
// // ============================================================================

// /* Exported functions ------------------------------------------------------- */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq) {
//   if (arm == NULL||arm_param == NULL) {
//     return -1;
//   }
//   arm->param = arm_param;

//   BSP_CAN_Init();

//   MOTOR_LZ_Init();


//   // 初始化控制参数
//   arm->control.mode = ARM_MODE_IDLE;
//   arm->control.state = ARM_STATE_STOPPED;
//   arm->control.position_tolerance = 0.02f;  // 关节角容差：0.02rad ≈ 1.15度
//   arm->control.velocity_tolerance = 0.01f;  // 速度容差：0.01rad/s
//   arm->control.enable = false;

//   return 0;
// }

// int8_t Arm_Updata(Arm_t *arm) {
//   if (arm == NULL) {
//     return -1;
//   }


//   MOTOR_LZ_Update(&arm->param->jointMotor1_params);
//   MOTOR_LZ_Update(&arm->param->jointMotor2_params);
//   MOTOR_LZ_Update(&arm->param->jointMotor3_params);
//   MOTOR_DM_Update(&arm->param->jointMotor4_params);
//   MOTOR_DM_Update(&arm->param->jointMotor5_params);
//   MOTOR_DM_Update(&arm->param->jointMotor6_params);

//   // 同步电机实例数据到actuator
//   MOTOR_LZ_t *motor1 = MOTOR_LZ_GetMotor(&arm->param->jointMotor1_params);
//   if (motor1 != NULL) {
//     memcpy(&arm->actuator.jointMotor1, motor1, sizeof(MOTOR_LZ_t));
//   }
  
//   MOTOR_LZ_t *motor2 = MOTOR_LZ_GetMotor(&arm->param->jointMotor2_params);
//   if (motor2 != NULL) {
//     memcpy(&arm->actuator.jointMotor2, motor2, sizeof(MOTOR_LZ_t));
//   }
  
//   MOTOR_LZ_t *motor3 = MOTOR_LZ_GetMotor(&arm->param->jointMotor3_params);
//   if (motor3 != NULL) {
//     memcpy(&arm->actuator.jointMotor3, motor3, sizeof(MOTOR_LZ_t));
//   }
  
//   MOTOR_DM_t *motor4 = MOTOR_DM_GetMotor(&arm->param->jointMotor4_params);
//   if (motor4 != NULL) {
//     memcpy(&arm->actuator.jointMotor4, motor4, sizeof(MOTOR_DM_t));
//   }
  
//   MOTOR_DM_t *motor5 = MOTOR_DM_GetMotor(&arm->param->jointMotor5_params);
//   if (motor5 != NULL) {
//     memcpy(&arm->actuator.jointMotor5, motor5, sizeof(MOTOR_DM_t));
//   }
  
//   MOTOR_DM_t *motor6 = MOTOR_DM_GetMotor(&arm->param->jointMotor6_params);
//   if (motor6 != NULL) {
//     memcpy(&arm->actuator.jointMotor6, motor6, sizeof(MOTOR_DM_t));
//   }

//   // 从电机反馈更新当前关节角度
//   arm->stateVariable.currentJointAngles.q[0] = 
//     arm->actuator.jointMotor1.motor.feedback.rotor_abs_angle - arm->param->q_offset[0];
//   arm->stateVariable.currentJointAngles.q[1] = 
//     arm->actuator.jointMotor2.motor.feedback.rotor_abs_angle - arm->param->q_offset[1];
//   arm->stateVariable.currentJointAngles.q[2] = 
//     arm->actuator.jointMotor3.motor.feedback.rotor_abs_angle - arm->param->q_offset[2];
//   arm->stateVariable.currentJointAngles.q[3] = 
//     arm->actuator.jointMotor4.motor.feedback.rotor_abs_angle - arm->param->q_offset[3];
//   arm->stateVariable.currentJointAngles.q[4] = 
//     arm->actuator.jointMotor5.motor.feedback.rotor_abs_angle - arm->param->q_offset[4];
//   arm->stateVariable.currentJointAngles.q[5] = 
//     arm->actuator.jointMotor6.motor.feedback.rotor_abs_angle - arm->param->q_offset[5];

//   // 正运动学：计算当前末端位姿
//   Arm6dof_ForwardKinematics(&arm->stateVariable.currentJointAngles,
//                             &arm->stateVariable.currentEndPose);

//   return 0;
// }
// int8_t Arm_Ctrl(Arm_t *arm) {
//   if (arm == NULL) {
//     return -1;
//   }
  
//   arm->timer.now = BSP_TIME_Get_us() / 1000000.0f;
//   arm->timer.dt = (BSP_TIME_Get_us() - arm->timer.last_wakeup) / 1000000.0f;
//   arm->timer.last_wakeup = BSP_TIME_Get_us();

//   // 如果未使能，松弛所有电机
//   if (!arm->control.enable) {
//     MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
//     MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
//     MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
//     MOTOR_DM_Relax(&arm->param->jointMotor4_params);
//     MOTOR_DM_Relax(&arm->param->jointMotor5_params);
//     MOTOR_DM_Relax(&arm->param->jointMotor6_params);
//     arm->control.state = ARM_STATE_STOPPED;
//     return 0;
//   }
  
//   // 根据控制模式执行不同的控制策略
//   switch (arm->control.mode) {
//     case ARM_MODE_IDLE:
//       // 空闲模式：电机松弛
//       MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
//       MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
//       MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
//       MOTOR_DM_Relax(&arm->param->jointMotor4_params);
//       MOTOR_DM_Relax(&arm->param->jointMotor5_params);
//       MOTOR_DM_Relax(&arm->param->jointMotor6_params);
//       arm->control.state = ARM_STATE_STOPPED;
//       break;
      
//     case ARM_MODE_JOINT_POSITION:
//     case ARM_MODE_CARTESIAN_POSITION:
//       // 关节位置控制（关节空间和笛卡尔空间最终都转为关节角控制）
//       {
//         // 检查是否到达目标
//         bool reached = true;
//         float max_error = 0.0f;
        
//         for (int i = 0; i < 6; i++) {
//           float error = arm->stateVariable.targetJointAngles.q[i] - 
//                        arm->stateVariable.currentJointAngles.q[i];
//           if (fabsf(error) > arm->control.position_tolerance) {
//             reached = false;
//           }
//           if (fabsf(error) > max_error) {
//             max_error = fabsf(error);
//           }
//         }
        
//         if (reached) {
//           arm->control.state = ARM_STATE_REACHED;
//         } else {
//           arm->control.state = ARM_STATE_MOVING;
//         }
        
//         // PID位置控制（关节1-3使用LZ电机）
//         for (int i = 0; i < 3; i++) {
//           float pid_output = PID_Calc(&arm->controller.joint_pid[i], 
//                                           arm->stateVariable.targetJointAngles.q[i],
//                                           arm->stateVariable.currentJointAngles.q[i],
//                                           0,
//                                           arm->timer.dt
//                                           );
          
//           MOTOR_LZ_MotionParam_t* output = NULL;
//           if (i == 0) output = &arm->output.jointMotor1_output;
//           else if (i == 1) output = &arm->output.jointMotor2_output;
//           else output = &arm->output.jointMotor3_output;
          
//           output->target_angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
//           output->target_velocity = 0.0f;  // 位置模式速度由PID控制
//           output->kp = 10.0f;  // 可调参数
//           output->kd = 0.5f;
//           output->torque = pid_output;
//         }
        
//         // MIT模式控制（关节4-6使用DM电机）
//         for (int i = 3; i < 6; i++) {
//           float pid_output = PID_Calc(&arm->controller.joint_pid[i],
//                                           arm->stateVariable.targetJointAngles.q[i],
//                                           arm->stateVariable.currentJointAngles.q[i],
//                                           0,
//                                           arm->timer.dt
//                                           );
          
//           MOTOR_MIT_Output_t* output = NULL;
//           if (i == 3) output = &arm->output.jointMotor4_output;
//           else if (i == 4) output = &arm->output.jointMotor5_output;
//           else output = &arm->output.jointMotor6_output;
          
//           output->angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
//           output->velocity = 0.0f;
//           output->kp = 50.0f;   // 位置刚度（提供阻抗和稳定性）
//           output->kd = 3.0f;    // 速度阻尼（抑制振荡）
//           output->torque = pid_output;  // PID输出作为前馈力矩（主要驱动力）
//         }
        
//         // 发送控制命令
//         MOTOR_LZ_MotionControl(&arm->param->jointMotor1_params, &arm->output.jointMotor1_output);
//         MOTOR_LZ_MotionControl(&arm->param->jointMotor2_params, &arm->output.jointMotor2_output);
//         MOTOR_LZ_MotionControl(&arm->param->jointMotor3_params, &arm->output.jointMotor3_output);
//         MOTOR_DM_MITCtrl(&arm->param->jointMotor4_params, &arm->output.jointMotor4_output);
//         MOTOR_DM_MITCtrl(&arm->param->jointMotor5_params, &arm->output.jointMotor5_output);
//         MOTOR_DM_MITCtrl(&arm->param->jointMotor6_params, &arm->output.jointMotor6_output);
//       }
//       break;
      
//     case ARM_MODE_TRAJECTORY:
//       // 轨迹跟踪模式（待实现）
//       arm->control.state = ARM_STATE_ERROR;
//       break;
      
//     case ARM_MODE_TEACH:
//       // 示教模式（待实现）
//       arm->control.state = ARM_STATE_ERROR;
//       break;
      
//     default:
//       arm->control.state = ARM_STATE_ERROR;
//       break;
//   }
  
//   return 0;
// }

// /**
//  * @brief 逆运动学求解：根据目标末端位姿计算关节角度
//  * @note 此函数计算量大，不要在控制循环中频繁调用
//  */
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles) {
//   if (arm == NULL || target_pose == NULL || result_angles == NULL) {
//     return -1;
//   }
  
//   // 验证目标位姿的有效性
//   if (isnan(target_pose->x) || isnan(target_pose->y) || isnan(target_pose->z) ||
//       isnan(target_pose->roll) || isnan(target_pose->pitch) || isnan(target_pose->yaw) ||
//       isinf(target_pose->x) || isinf(target_pose->y) || isinf(target_pose->z) ||
//       isinf(target_pose->roll) || isinf(target_pose->pitch) || isinf(target_pose->yaw)) {
//     return -1;  // 无效的目标位姿
//   }
  
//   // 使用当前关节角度作为初始猜测值（如果已初始化）
//   Arm6dof_JointAngles_t initial_guess;
  
//   // 检查currentJointAngles是否已初始化（不全为0）
//   bool has_init = false;
//   for (int i = 0; i < 6; i++) {
//     if (arm->stateVariable.currentJointAngles.q[i] != 0.0f) {
//       has_init = true;
//       break;
//     }
//   }
  
//   if (has_init) {
//     // 使用当前角度作为初始猜测
//     memcpy(&initial_guess, &arm->stateVariable.currentJointAngles, sizeof(Arm6dof_JointAngles_t));
//   } else {
//     // 使用零位作为初始猜测（机械臂竖直向上）
//     memset(&initial_guess, 0, sizeof(Arm6dof_JointAngles_t));
//   }
  
//   // 调用逆运动学求解，容限1mm，最大迭代10次（减少计算量）
//   int ret = Arm6dof_InverseKinematics(target_pose, &initial_guess, result_angles, 1.0f, 10);
  
//   if (ret == 0) {
//     // 求解成功，更新到inverseKineJointAngles
//     memcpy(&arm->stateVariable.inverseKineJointAngles, result_angles, sizeof(Arm6dof_JointAngles_t));
//   }
  
//   return ret;
// }

// // ============================================================================
// // 控制API函数实现
// // ============================================================================

// /**
//  * @brief 设置控制模式
//  */
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode) {
//   if (arm == NULL) {
//     return -1;
//   }
//   arm->control.mode = mode;
//   arm->control.state = ARM_STATE_STOPPED;
//   return 0;
// }

// /**
//  * @brief 使能/失能机械臂
//  */
// int8_t Arm_Enable(Arm_t *arm, bool enable) {
//   if (arm == NULL) {
//     return -1;
//   }
//   arm->control.enable = enable;
  
//   if (!enable) {
//     // 失能时切换到空闲模式
//     arm->control.mode = ARM_MODE_IDLE;
//     arm->control.state = ARM_STATE_STOPPED;
//   }
  
//   return 0;
// }

// /**
//  * @brief 关节空间位置控制
//  */
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles) {
//   if (arm == NULL || target_angles == NULL) {
//     return -1;
//   }
  
//   // 检查关节角度是否在限制范围内
//   for (int i = 0; i < 6; i++) {
//     float qmin = arm->param->arm6dof_params.DH_params[i].qmin;
//     float qmax = arm->param->arm6dof_params.DH_params[i].qmax;
    
//     if (target_angles->q[i] < qmin || target_angles->q[i] > qmax) {
//       return -1;  // 超出关节限位
//     }
//   }
  
//   // 更新目标角度
//   memcpy(&arm->stateVariable.targetJointAngles, target_angles, sizeof(Arm6dof_JointAngles_t));
  
//   // 切换到关节位置控制模式
//   arm->control.mode = ARM_MODE_JOINT_POSITION;
//   arm->control.state = ARM_STATE_MOVING;
  
//   return 0;
// }

// /**
//  * @brief 笛卡尔空间位置控制
//  */
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose) {
//   if (arm == NULL || target_pose == NULL) {
//     return -1;
//   }
  
//   // 求解逆运动学
//   Arm6dof_JointAngles_t ik_solution;
//   if (Arm_SolveIK(arm, target_pose, &ik_solution) != 0) {
//     return -1;  // 逆运动学求解失败
//   }
  
//   // 更新目标
//   memcpy(&arm->stateVariable.targetEndPose, target_pose, sizeof(Arm6dof_Pose_t));
//   memcpy(&arm->stateVariable.targetJointAngles, &ik_solution, sizeof(Arm6dof_JointAngles_t));
  
//   // 切换到笛卡尔位置控制模式
//   arm->control.mode = ARM_MODE_CARTESIAN_POSITION;
//   arm->control.state = ARM_STATE_MOVING;
  
//   return 0;
// }

// /**
//  * @brief 急停
//  */
// int8_t Arm_Stop(Arm_t *arm) {
//   if (arm == NULL) {
//     return -1;
//   }
  
//   // 将目标设置为当前位置
//   memcpy(&arm->stateVariable.targetJointAngles, 
//          &arm->stateVariable.currentJointAngles, 
//          sizeof(Arm6dof_JointAngles_t));
  
//   arm->control.state = ARM_STATE_STOPPED;
  
//   return 0;
// }

// /**
//  * @brief 获取当前运动状态
//  */
// Arm_MotionState_t Arm_GetState(Arm_t *arm) {
//   if (arm == NULL) {
//     return ARM_STATE_ERROR;
//   }
//   return arm->control.state;
// }