#include "kinematics.hpp"
#include "arm_define.h"
#include <stdexcept>
#include <cstring>

ArmKinematics::ArmKinematics(RM_Service *rmService) :
    rmService_(rmService)
{
    rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
    rm_force_type_e Type = RM_MODEL_RM_B_E;
    rm_algo_init_sys_data(Mode, Type);
}

ArmKinematics::ArmKinematics()
{
    rmService_ = new RM_Service();
    int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
    if (result != 0) {
        throw std::runtime_error("Failed to init rm.");
        return;
    }
    
    rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
    rm_force_type_e Type = RM_MODEL_RM_B_E;
    rm_algo_init_sys_data(Mode, Type);
}

ArmKinematics::~ArmKinematics()
{
    delete rmService_;
}

int ArmKinematics::ForwardKinematics(float *jointAngles, float *endEffectorPose)
{
    if (!rmService_)
    {
        return UNKNOWN_ERR; // Error: RM_Service not initialized
    }

    // Call the RM_Service's ForwardKinematics method
    rm_pose_t pose = rmService_->rm_algo_forward_kinematics(NULL, jointAngles);
    endEffectorPose[POSE_POSITION_X] = pose.position.x;
    endEffectorPose[POSE_POSITION_Y] = pose.position.y;
    endEffectorPose[POSE_POSITION_Z] = pose.position.z;
    endEffectorPose[POSE_QUATERNION_X] = pose.quaternion.x;
    endEffectorPose[POSE_QUATERNION_Y] = pose.quaternion.y;
    endEffectorPose[POSE_QUATERNION_Z] = pose.quaternion.z;
    endEffectorPose[POSE_QUATERNION_W] = pose.quaternion.w;
    return 0;
}

int ArmKinematics::InverseKinematics(const float *endEffectorPose, float *jointAngles)
{
    if (!rmService_)
    {
        return UNKNOWN_ERR; // Error: RM_Service not initialized
    }

    float nowJoints_[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    for (int i = 0; i < USED_ARM_DOF; i++) {
        nowJoints_[i] = jointAngles[i];
    }
    rm_inverse_kinematics_params_t inverse_params;
    memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
    inverse_params.q_pose.position.x = endEffectorPose[POSE_POSITION_X];
    inverse_params.q_pose.position.y = endEffectorPose[POSE_POSITION_Y];
    inverse_params.q_pose.position.z = endEffectorPose[POSE_POSITION_Z];
    inverse_params.q_pose.quaternion.x = endEffectorPose[POSE_QUATERNION_X];
    inverse_params.q_pose.quaternion.y = endEffectorPose[POSE_QUATERNION_Y];
    inverse_params.q_pose.quaternion.z = endEffectorPose[POSE_QUATERNION_Z];
    inverse_params.q_pose.quaternion.w = endEffectorPose[POSE_QUATERNION_W];
    inverse_params.flag = 0;

    int result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, nowJoints_);
    if (result != 0){
        return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
    }
    memcpy(jointAngles, nowJoints_, sizeof(float) * USED_ARM_DOF);
    return 0;
}