/*
  运动学计算
*/

#include "filter.h"

#include "kinematics.h"


int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero){
    real->Pos = (in->Pos-joint_zero) / ratio;
    real->T = in->T * ratio;
    real->W = in->W /ratio;
    return 0;
}

int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero){
    out->Pos = real->Pos * ratio + joint_zero;
    out->T = real->T / ratio;
    out->W = real->W * ratio;
    out->K_P = real->K_P;
    out->K_W = real->K_W;
    return 0;
}

/**
 * @brief 单腿正运动学
 * @param theta[3] 关节角度数组（单位：弧度）：[hip, thigh, calf]
 * @param leg_params 单腿机械参数（长度）
 * @param foot_out 足端位置输出 [x, y, z]（单位：米）
 */
// void Leg_ForwardKinematics(const float theta[3], const joint_params *leg_params, float foot_out[3]) {
//     float l1 = leg_params->named.lf_hip;    // 髋关节到大腿长度
//     float l2 = leg_params->named.lf_thigh;  // 大腿长度
//     float l3 = leg_params->named.lf_calf;   // 小腿长度

//     float t1 = theta[0]; // 髋关节侧摆（绕y轴）
//     float t2 = theta[1]; // 髋关节前后
//     float t3 = theta[2]; // 膝关节

//     // 末端位置
//     float leg_proj = l2 * cosf(t2) + l3 * cosf(t2 + t3);
//     foot_out[0] = cosf(t1) * leg_proj + l1 * cosf(t1); // x
//     foot_out[1] = sinf(t1) * leg_proj + l1 * sinf(t1); // y
//     foot_out[2] = l2 * sinf(t2) + l3 * sinf(t2 + t3);  // z
// }

/**
 * @brief 单腿逆运动学
 * @param foot_in 足端目标位置 [x, y, z]（单位：米）
 * @param leg_params 单腿机械参数（长度）
 * @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
 * @return 0成功，-1目标不可达
 */
// int Leg_InverseKinematics(const float foot_in[3], const joint_params *leg_params, float theta_out[3]) {
//     float l1 = leg_params->named.lf_hip;
//     float l2 = leg_params->named.lf_thigh;
//     float l3 = leg_params->named.lf_calf;

//     float x = foot_in[0];
//     float y = foot_in[1];
//     float z = foot_in[2];

//     // 1. 先算髋关节侧摆角（绕y轴）
//     float r = sqrtf(x * x + y * y);
//     float t1 = atan2f(y, x); // 髋关节侧摆角：使用 atan2f，计

//     // 2. 去掉髋关节侧摆影响，得到在sagittal平面上的投影
//     float xp = r - l1;
//     float zp = z;

//     // 3. 逆解余下两个关节
//     float D = (xp * xp + zp * zp - l2 * l2 - l3 * l3) / (2 * l2 * l3);
//     if (D < -1.0f || D > 1.0f) return -1; // 不可达

//     float t3 = atan2f(-sqrtf(1 - D * D), D); // 膝关节
//     float k1 = l2 + l3 * cosf(t3);
//     float k2 = l3 * sinf(t3);
//     float t2 = atan2f(zp, xp) - atan2f(k2, k1);

//     theta_out[0] = t1;
//     theta_out[1] = t2;
//     theta_out[2] = t3;
//     return 0;
// }