/*
 * LQR控制器测试程序
 */

#include "lqr.h"
#include <stdio.h>
#include <assert.h>

/* 测试用的增益矩阵 */
extern LQR_GainMatrix_t example_lqr_gains;

/* 测试函数 */
void test_lqr_init(void);
void test_lqr_gain_calculation(void);
void test_lqr_control(void);
void test_lqr_polynomial_calc(void);

int main(void) {
    printf("开始LQR控制器测试...\n");
    
    test_lqr_polynomial_calc();
    test_lqr_init();
    test_lqr_gain_calculation();
    test_lqr_control();
    
    printf("所有测试通过！\n");
    return 0;
}

void test_lqr_polynomial_calc(void) {
    printf("测试多项式计算...\n");
    
    /* 测试多项式计算函数 */
    float coeff[4] = {1.0f, 2.0f, 3.0f, 4.0f}; // L^3 + 2*L^2 + 3*L + 4
    float L = 0.25f;
    
    float expected = 1.0f * 0.25f * 0.25f * 0.25f + 
                     2.0f * 0.25f * 0.25f + 
                     3.0f * 0.25f + 
                     4.0f;
                     
    float result = LQR_PolynomialCalc(coeff, L);
    
    printf("  多项式计算: L=%.2f, 期望=%.4f, 实际=%.4f\n", L, expected, result);
    assert(fabsf(result - expected) < 1e-4f);
    
    printf("  多项式计算测试通过\n");
}

void test_lqr_init(void) {
    printf("测试LQR初始化...\n");
    
    LQR_Controller_t lqr;
    int8_t result = LQR_Init(&lqr, 10.0f, 5.0f);
    
    assert(result == 0);
    assert(lqr.initialized == true);
    assert(lqr.param.max_wheel_torque == 10.0f);
    assert(lqr.param.max_joint_torque == 5.0f);
    assert(lqr.current_leg_length == 0.25f);
    
    printf("  LQR初始化测试通过\n");
}

void test_lqr_gain_calculation(void) {
    printf("测试增益矩阵计算...\n");
    
    LQR_Controller_t lqr;
    LQR_Init(&lqr, 10.0f, 5.0f);
    LQR_SetGainMatrix(&lqr, &example_lqr_gains);
    
    /* 测试不同腿长的增益计算 */
    float test_lengths[] = {0.15f, 0.25f, 0.35f};
    int num_tests = sizeof(test_lengths) / sizeof(test_lengths[0]);
    
    for (int i = 0; i < num_tests; i++) {
        float L = test_lengths[i];
        int8_t result = LQR_CalculateGainMatrix(&lqr, L);
        
        assert(result == 0);
        assert(lqr.current_leg_length == L);
        
        /* 检查增益矩阵是否合理 */
        for (int j = 0; j < LQR_INPUT_DIM; j++) {
            for (int k = 0; k < LQR_STATE_DIM; k++) {
                assert(!isnan(lqr.K_matrix[j][k]));
                assert(!isinf(lqr.K_matrix[j][k]));
            }
        }
        
        printf("  腿长%.2fm的增益矩阵计算通过\n", L);
        printf("    K11=%.2f, K12=%.2f, K13=%.2f\n", 
               lqr.K_matrix[0][0], lqr.K_matrix[0][1], lqr.K_matrix[0][2]);
    }
    
    printf("  增益矩阵计算测试通过\n");
}

void test_lqr_control(void) {
    printf("测试LQR控制计算...\n");
    
    LQR_Controller_t lqr;
    LQR_Init(&lqr, 10.0f, 5.0f);
    LQR_SetGainMatrix(&lqr, &example_lqr_gains);
    
    /* 设置测试状态 */
    LQR_State_t test_state = {
        .theta = 0.1f,      /* 摆杆角度 */
        .d_theta = 0.05f,   /* 摆杆角速度 */
        .x = 0.02f,         /* 位置 */
        .d_x = 0.1f,        /* 速度 */
        .phi = 0.05f,       /* 俯仰角 */
        .d_phi = 0.02f      /* 俯仰角速度 */
    };
    
    LQR_State_t target_state = {0}; /* 平衡状态 */
    
    /* 更新状态 */
    int8_t result = LQR_UpdateState(&lqr, &test_state);
    assert(result == 0);
    
    result = LQR_SetTargetState(&lqr, &target_state);
    assert(result == 0);
    
    /* 执行控制计算 */
    result = LQR_Control(&lqr);
    assert(result == 0);
    
    /* 获取控制输出 */
    LQR_Input_t output;
    result = LQR_GetOutput(&lqr, &output);
    assert(result == 0);
    
    /* 检查输出是否合理 */
    assert(!isnan(output.T));
    assert(!isnan(output.Tp));
    assert(!isinf(output.T));
    assert(!isinf(output.Tp));
    assert(fabsf(output.T) <= lqr.param.max_wheel_torque);
    assert(fabsf(output.Tp) <= lqr.param.max_joint_torque);
    
    printf("  控制输出: T=%.3f N·m, Tp=%.3f N·m\n", output.T, output.Tp);
    printf("  LQR控制计算测试通过\n");
}

/* 如果没有math.h中的函数，提供简单实现 */
#ifndef __has_include
#define __has_include(x) 0
#endif

#if !__has_include(<math.h>)
float fabsf(float x) {
    return x < 0 ? -x : x;
}

int isnan(float x) {
    return x != x;
}

int isinf(float x) {
    return (x == x) && ((x - x) != 0);
}
#endif