/*
  运动学计算
*/

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "user_math.h"

typedef struct {
  float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm)
  float W; // 当前实际电机速度(电机本身的速度)(rad/s)
  float Pos; // 当前电机位置(rad)
} Kinematics_JointData_t;

typedef struct {
  float T;             // 期望关节的输出力矩(电机本身的力矩)(Nm)
  float W;             // 期望关节速度(电机本身的速度)(rad/s)
  float Pos;           // 期望关节位置(rad)
  float K_P;           // 关节刚度系数(0-25.599)
  float K_W;           // 关节速度系数(0-25.599)
} Kinematics_JointCMD_t;

typedef struct {
  float hip;   // 髋关节长度(m)
  float thigh; // 大腿长度(m)
  float calf;  // 小腿长度(m)
} Kinematics_LinkLength_t;

typedef struct {
  float x;
  float y;
  float z;
} Kinematics_LegOffset_t;

typedef struct {
  float hip;   
  float thigh; 
  float calf;  
} Kinematics_Sign_t;


/**
 * @brief 计算实际反馈
 * @param real 减速后关节数据
 * @param in 输入关节数据
 * @param ratio 减速比
 * @return 
 */
int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero);

/**
 * @brief 计算实际输出
 * @param real 减速后关节输出
 * @param out 实际输出关节数据
 * @param ratio 减速比
 * @return 
 */
int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero);


/**
 * @brief 单腿正运动学
 * @param theta_in 关节角度输入数组（单位：弧度）：[hip, thigh, calf]
 * @param leg_params 单腿机械参数（长度）
 * @param foot_out 足端位置输出 [x, y, z]（单位：米）
 * @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
 */
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign);

/**
 * @brief 单腿逆运动学
 * @param foot_in 足端目标位置 [x, y, z]（单位：米）
 * @param leg_params 单腿机械参数（长度）
 * @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
 * @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
 * @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);


#ifdef __cplusplus
}
#endif