#include <stdio.h>
#include <math.h>
#include "User/component/vmc.h"

int main() {
    // 测试VMC计算
    VMC_t vmc;
    VMC_Param_t param = {
        .hip_length = 0.1f,      // 10cm髋关节间距
        .leg_1 = 0.15f,         // 15cm大腿前端
        .leg_2 = 0.15f,         // 15cm大腿后端
        .leg_3 = 0.15f,         // 15cm小腿
        .leg_4 = 0.15f,         // 15cm小腿前端
        .wheel_radius = 0.05f,   // 5cm轮子半径
        .wheel_mass = 1.0f       // 1kg轮子质量
    };
    
    if (VMC_Init(&vmc, &param, 1000.0f) != 0) {
        printf("VMC初始化失败\n");
        return -1;
    }
    
    printf("测试VMC腿长计算\n");
    printf("髋关节间距: %.3f\n", param.hip_length);
    printf("腿段长度: L1=%.3f, L2=%.3f, L3=%.3f, L4=%.3f\n", 
           param.leg_1, param.leg_2, param.leg_3, param.leg_4);
    printf("\n");
    
    // 测试不同的关节角度配置
    float test_angles[][2] = {
        {0.0f, 0.0f},           // 水平伸展
        {0.5f, -0.5f},          // 轻微弯曲
        {1.0f, -1.0f},          // 中等弯曲
        {1.5f, -1.5f},          // 较大弯曲
        {-0.5f, 0.5f},          // 反向弯曲
    };
    
    int num_tests = sizeof(test_angles) / sizeof(test_angles[0]);
    
    for (int i = 0; i < num_tests; i++) {
        float phi1 = test_angles[i][0];
        float phi4 = test_angles[i][1];
        float body_pitch = 0.0f;
        float body_pitch_rate = 0.0f;
        
        if (VMC_ForwardSolve(&vmc, phi1, phi4, body_pitch, body_pitch_rate) == 0) {
            float length, angle, d_length, d_angle;
            VMC_GetVirtualLegState(&vmc, &length, &angle, &d_length, &d_angle);
            
            float foot_x, foot_y;
            VMC_GetFootPosition(&vmc, &foot_x, &foot_y);
            
            printf("测试 %d: phi1=%.2f, phi4=%.2f\n", i+1, phi1, phi4);
            printf("  足端位置: (%.3f, %.3f)\n", foot_x, foot_y);
            printf("  虚拟腿长: %.3f\n", length);
            printf("  虚拟摆角: %.3f (%.1f度)\n", angle, angle * 180.0f / M_PI);
            printf("  内部变量: XC=%.3f, YC=%.3f, phi2=%.3f, phi3=%.3f\n", 
                   vmc.leg.XC, vmc.leg.YC, vmc.leg.phi2, vmc.leg.phi3);
            printf("\n");
        } else {
            printf("测试 %d: 求解失败 (phi1=%.2f, phi4=%.2f)\n", i+1, phi1, phi4);
        }
    }
    
    return 0;
}