/*
  运动学计算
*/

#include "filter.h"
#include <math.h>
#include "kinematics.h"

static float q1_ik(float py, float pz, float l1) {
    float q1;
    float L = sqrtf(py*py + pz*pz - l1*l1);
    q1 = atan2f(pz*l1 + py*L, py*l1 - pz*L);
    return q1;
}

static float q3_ik(float b3z, float b4z, float b) {
    float q3, temp;
    temp = (b3z*b3z + b4z*b4z - b*b) / (2.0f * fabsf(b3z * b4z));
    if (temp > 1.0f) temp = 1.0f;
    if (temp < -1.0f) temp = -1.0f;
    q3 = acosf(temp);
    q3 = -(M_PI - q3);
    return q3;
}

static float q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z) {
    float a1 = py * sinf(q1) - pz * cosf(q1);
    float a2 = px;
    float m1 = b4z * sinf(q3);
    float m2 = b3z + b4z * cosf(q3);
    float q2 = atan2f(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1);
    return q2;
}


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;
}

void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign) {
    float l1 = leg_params->hip * sign->hip;
    float l2 = -leg_params->thigh;
    float l3 = -leg_params->calf;

    float q1 = theta_in[0];
    float q2 = theta_in[1] * sign->thigh;
    float q3 = theta_in[2] * sign->calf;

    float s1 = sinf(q1);
    float s2 = sinf(q2);
    float s3 = sinf(q3);

    float c1 = cosf(q1);
    float c2 = cosf(q2);
    float c3 = cosf(q3);

    float c23 = c2 * c3 - s2 * s3;
    float s23 = s2 * c3 + c2 * s3;

    foot_out[0] = l3 * s23 + l2 * s2;
    foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
    foot_out[2] =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
}

/**
 * @brief 单腿逆运动学
 * @param foot_in 足端目标位置 [x, y, z]（单位：米）
 * @param leg_params 单腿机械参数（长度）
 * @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
 * @return 0成功，-1目标不可达
 */
int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
    float px = foot_in[0];
    float py = foot_in[1];
    float pz = foot_in[2];

    float b2y = leg_params->hip * sign->hip;
    float b3z = -leg_params->thigh;
    float b4z = -leg_params->calf;
    float a = leg_params->hip;
    float c = sqrtf(px*px + py*py + pz*pz);
    float b = sqrtf(c*c - a*a);

    // 检查可达性
    if (b != b) return -1; // NaN
    if (b > fabsf(b3z) + fabsf(b4z)) return -1;

    float q1 = q1_ik(py, pz, b2y);
    float q3 = q3_ik(b3z, b4z, b);
    float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);

    theta_out[0] = q1;
    theta_out[1] = q2 * sign->thigh;
    theta_out[2] = q3 * sign->calf;
    return 0;
}