#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <cmath>
#include <chrono>
#include "arm_control.hpp"

using namespace std::chrono_literals;

ArmControl::ArmControl(int total_joints)
{
    // 初始化成员变量
    total_joints_ = total_joints;

    // 初始化正弦运动参数
    amplitude_deg_ = 0.5;  // 振幅（弧度）
    frequency_ = 0.5;      // 频率（Hz）

    // 初始化成员数组
    joint_position_.resize(total_joints_, 0.0);
    joint_velocity_.resize(total_joints_, 0.0);
    joint_acceleration_.resize(total_joints_, 0.0);
    joint_torque_.resize(total_joints_, 0.0);
    joint_position_desired_.resize(total_joints_, 0.0);

    // 初始化正弦运动参数
    amplitude_rad_ = amplitude_deg_ * M_PI / 180.0;  // 将角度转换为弧度

}

ArmControl::~ArmControl()
{
    // 析构函数
}

void ArmControl::DefaultArmMovement(std::vector<double>& joint_position_desired, double duration)
{
    double sine_pos = amplitude_rad_ * std::sin(2 * M_PI * frequency_ * duration);

    for (size_t i = 0; i < total_joints_; i++)
    {
        joint_position_desired_[i] = sine_pos;
        joint_position_desired = joint_position_desired_;
    }
}

void ArmControl::MoveToZeroPoint(double duration)
{
    std::vector<double> joint_position_desired(total_joints_, 0.0);
    MoveToTargetPoint(joint_position_desired, duration);
}

void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration)
{
    // 计算每个关节的目标位置
    for (size_t i = 0; i < total_joints_; i++)
    {
        joint_position_desired_[i] = joint_position_desired[i];
    }

    // 计算每个关节的所需时间
}

