/*
一些运动学相关的计算函数
*/

#include "component/kinematics.h"
#include "component/user_math.h"

/*串联腿单腿转摆动杆正运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/

/**
 * @brief 串联腿单腿转摆动杆正运动学
 * @param param 腿部长度参数
 * @param hip_angle 髋关节角度(弧度)
 * @param knee_angle 膝关节角度(弧度)
 * @param angle 计算到的等效摆动杆角度
 * @param heigh 计算到的等效摆动杆高度
 * @return 
 */
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
    if (param == NULL || hip_angle == NULL || knee_angle == NULL || angle == NULL || height == NULL) {
        return -1; // 参数错误
    }
    
    float L1 = param->thigh_length;
    float L2 = param->calf_length;
    float q1 = *hip_angle;      // 髋关节角度
    float q2 = *knee_angle;     // 膝关节角度
    
    float q4 = (M_PI - q1 - q2)/2;

    // 使用余弦定理求解等效摆动杆长度
    // L2*L2 = L1*L1 + L3*L3 - 2*L1*L3*cosf(q4);
    // 整理得：L3*L3 - 2*L1*cosf(q4)*L3 + (L1*L1 - L2*L2) = 0
    
    float a = 1.0f;
    float b = -2.0f * L1 * cosf(q4);
    float c = L1*L1 - L2*L2;
    
    float discriminant = b*b - 4*a*c;
    
    // 检查判别式，确保有实数解
    if (discriminant < 0) {
        return -2; // 无实数解，配置不可达
    }
    
    float sqrt_discriminant = sqrtf(discriminant);
    float L3_1 = (-b + sqrt_discriminant) / (2*a);
    float L3_2 = (-b - sqrt_discriminant) / (2*a);
    
    // 选择正的解（物理意义上的长度）
    float L3;
    if (L3_1 > 0 && L3_2 > 0) {
        // 两个正解，选择较小的（通常对应机构的正常工作姿态）
        L3 = (L3_1 < L3_2) ? L3_1 : L3_2;
    } else if (L3_1 > 0) {
        L3 = L3_1;
    } else if (L3_2 > 0) {
        L3 = L3_2;
    } else {
        return -3; // 无正解
    }

    *angle = q1 + q4;
    *height = L3;

    return 0; // 成功
}

/*串联腿单腿转摆动杆逆运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/
/**
 * @brief 串联腿单腿转摆动杆逆运动学
 * @param param 腿部长度参数
 * @param angle 输入的等效摆动杆角度
 * @param height 输入的等效摆动杆高度
 * @param hip_angle 计算到的髋关节角度(弧度)
 * @param knee_angle 计算到的膝关节角度(弧度)
 * @return 
 */
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
    if (param == NULL || angle == NULL || height == NULL || hip_angle == NULL || knee_angle == NULL) {
        return -1; // 参数错误
    }
    float L1 = param->thigh_length;
    float L2 = param->calf_length;
    float q = *angle;  // 摆动杆角度
    float h = *height; // 摆动杆长度

    // 由正解可知：q = q1 + q4，h = L3
    // 由余弦定理：L2^2 = L1^2 + h^2 - 2*L1*h*cos(q4)
    // 整理得：cos(q4) = (L1^2 + h^2 - L2^2) / (2*L1*h)
    float cos_q4 = (L1*L1 + h*h - L2*L2) / (2.0f * L1 * h);

    // 检查 cos_q4 是否在 [-1, 1] 范围内
    if (cos_q4 < -1.0f || cos_q4 > 1.0f) {
        return -2; // 不可达
    }

    float q4 = acosf(cos_q4);

    // 由正解：q = q1 + q4，且 q4 = (PI - q1 - q2)/2
    // 整理得：q1 = q - q4
    float q1 = q - q4;

    // 再由 q4 = (PI - q1 - q2)/2，整理得：q2 = PI - 2*q4 - q1
    float q2 = M_PI - 2.0f * q4 - q1;

    *hip_angle = q1;
    *knee_angle = q2;

    return 0; // 成功
}