#pragma once

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "kinematics.h"

/* Exported types ----------------------------------------------------------- */

/**
 * @brief VMC虚拟模型控制参数结构体
 */
typedef struct {
    float hip_length;       // 髋关节间距
    float leg_1;           // 大腿前端长度 (L1)
    float leg_2;           // 大腿后端长度 (L2) 
    float leg_3;           // 小腿长度 (L3)
    float leg_4;           // 小腿前端长度 (L4)
    float wheel_radius;    // 轮子半径
    float wheel_mass;      // 轮子质量
} VMC_Param_t;

/**
 * @brief VMC腿部运动学状态结构体
 */
typedef struct {
    // 关节角度
    float phi1;            // 后髋关节角度 (rad)
    float phi2;            // 大腿后端角度 (rad)
    float phi3;            // 小腿角度 (rad) 
    float phi4;            // 前髋关节角度 (rad)
    
    // 足端坐标
    float foot_x;          // 足端x坐标
    float foot_y;          // 足端y坐标
    
    // 等效摆动杆参数
    float L0;              // 等效摆动杆长度
    float d_L0;            // 等效摆动杆长度变化率
    float theta;           // 等效摆动杆角度
    float d_theta;         // 等效摆动杆角速度
    float dd_theta;        // 等效摆动杆角加速度
    
    // 虚拟力和力矩
    float F_virtual;       // 虚拟支撑力
    float T_virtual;       // 虚拟摆动力矩
    
    // 雅可比矩阵元素
    float J11, J12, J21, J22;
    
    // 输出力矩
    float tau_hip_front;   // 前髋关节输出力矩
    float tau_hip_rear;    // 后髋关节输出力矩
    
    // 内部计算变量
    float XB, YB, XC, YC, XD, YD;  // 各关键点坐标
    float lBD;             // BD连杆长度
    float A0, B0, C0;      // 运动学计算中间变量
    float phi0;            // 足端极角
    float alpha;           // 等效摆动杆与竖直方向夹角
    float d_phi0;          // 足端极角变化率
    float d_alpha;         // alpha角变化率
    
    // 历史值（用于数值微分）
    float last_phi0;
    float last_L0; 
    float last_d_L0;
    float last_d_theta;
    
    // 地面接触检测
    float Fn;              // 地面法向力
    bool is_ground_contact; // 地面接触标志
} VMC_Leg_t;

/**
 * @brief VMC控制器结构体
 */
typedef struct {
    VMC_Param_t param;     // VMC参数
    VMC_Leg_t leg;         // 腿部状态
    float dt;              // 控制周期
    bool initialized;      // 初始化标志
} VMC_t;

/* Exported constants ------------------------------------------------------- */

#define VMC_PI_2        (1.5707963267948966f)
#define VMC_PI          (3.1415926535897932f)
#define VMC_2PI         (6.2831853071795865f)

/* Exported macros ---------------------------------------------------------- */

/**
 * @brief 角度范围限制到[-PI, PI]
 */
#define VMC_ANGLE_NORMALIZE(angle) do { \
    while((angle) > VMC_PI) (angle) -= VMC_2PI; \
    while((angle) < -VMC_PI) (angle) += VMC_2PI; \
} while(0)

/* Exported functions prototypes -------------------------------------------- */

/**
 * @brief 初始化VMC控制器
 * @param vmc VMC控制器实例
 * @param param VMC参数
 * @param sample_freq 采样频率 (Hz)
 * @return 0:成功, -1:失败
 */
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);

/**
 * @brief VMC五连杆正解算
 * @param vmc VMC控制器实例
 * @param phi1 后髋关节角度 (rad)
 * @param phi4 前髋关节角度 (rad)
 * @param body_pitch 机体pitch角 (rad)
 * @param body_pitch_rate 机体pitch角速度 (rad/s)
 * @return 0:成功, -1:失败
 */
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);

/**
 * @brief VMC五连杆逆解算(力矩分配)
 * @param vmc VMC控制器实例
 * @param F_virtual 期望虚拟支撑力 (N)
 * @param T_virtual 期望虚拟摆动力矩 (N*m)
 * @return 0:成功, -1:失败
 */
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);

/**
 * @brief 地面接触检测
 * @param vmc VMC控制器实例
 * @return 地面法向力 (N)
 */
float VMC_GroundContactDetection(VMC_t *vmc);

/**
 * @brief 获取足端位置(直角坐标)
 * @param vmc VMC控制器实例
 * @param x 足端x坐标输出
 * @param y 足端y坐标输出
 * @return 0:成功, -1:失败
 */
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);

/**
 * @brief 获取等效摆动杆参数
 * @param vmc VMC控制器实例
 * @param length 等效摆动杆长度输出
 * @param angle 等效摆动杆角度输出
 * @param d_length 等效摆动杆长度变化率输出
 * @param d_angle 等效摆动杆角速度输出
 * @return 0:成功, -1:失败
 */
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);

/**
 * @brief 获取关节输出力矩
 * @param vmc VMC控制器实例
 * @param tau_front 前髋关节力矩输出
 * @param tau_rear 后髋关节力矩输出
 * @return 0:成功, -1:失败
 */
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);

/**
 * @brief 重置VMC控制器状态
 * @param vmc VMC控制器实例
 */
void VMC_Reset(VMC_t *vmc);

/**
 * @brief 设置虚拟力和力矩
 * @param vmc VMC控制器实例
 * @param F_virtual 虚拟支撑力 (N)
 * @param T_virtual 虚拟摆动力矩 (N*m)
 */
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);

/**
 * @brief 计算雅可比矩阵
 * @param vmc VMC控制器实例
 * @return 0:成功, -1:失败
 */
int8_t VMC_ComputeJacobian(VMC_t *vmc);

#ifdef __cplusplus
}
#endif