#ifndef ROBOT_CONTROL__ARMCONTROL_HPP_
#define ROBOT_CONTROL__ARMCONTROL_HPP_

#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>

class ArmControl
{
public:
    ArmControl(int total_joints);
    ~ArmControl() = default;

    void MoveToZeroPoint(double duration);

    void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);

    void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);

    void DefaultArmMovement(std::vector<double>& joint_position_desired, double duration);

private:

    int total_joints_;         // 总关节数

    std::vector<double> joint_position_;  // 机械臂关节位置
    std::vector<double> joint_velocity_;  // 机械臂关节速度
    std::vector<double> joint_acceleration_;  // 机械臂关节加速度
    std::vector<double> joint_torque_;  // 机械臂关节力矩
    std::vector<double> joint_angle_;  // 机械臂关节角度

    std::vector<double> joint_position_desired_;  // 机械臂关节期望位置

    double amplitude_rad_;  // 振幅 （弧度）
    double amplitude_deg_;  // 振幅 （角度）
    double frequency_;        // 频率
};

#endif  // ROBOT_CONTROL__ARMCONTROL_HPP_