#include "balance.h"
#include "stdint.h"
#include "arm_math.h"

/**
 * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
 * @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
 *
 */
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
    float k[12][3] = {85.842511,-168.176021,-4.383994,
                        -8.011354,-27.544550,0.946791,
                        40.658824,-34.245857,-13.443024,
                        33.057883,-37.645803,-8.872245,
                        186.414265,-182.258244,59.110130,
                        12.371459,-12.979501,4.575691,
                        -2.044517,-13.973090,28.443243,
                        -8.073717,8.553946,2.899594,
                        109.977348,-109.218563,37.018016,
                        67.432105,-68.632617,25.883284,
                        -210.173934,175.310058,96.350481,
                        -15.827060,13.395116,3.049615};
    float T[2] = {0};       // 0 T_wheel  1 T_hip
    float l = p->leg_len;
    float lsqr = l * l;

    // float dist_limit = abs(chassis->target_dist - chassis->dist) > MAX_DIST_TRACK ? sign(chassis->target_dist - chassis->dist) * MAX_DIST_TRACK : (chassis->target_dist - chassis->dist); // todo设置值
    // float vel_limit = abs(chassis->target_v - chassis->vel) > MAX_VEL_TRACK ? sign(chassis->target_v - chassis->vel) * MAX_VEL_TRACK : (chassis->target_v - chassis->vel);

    for (uint8_t i = 0; i < 2; ++i)
    {
        uint8_t j = i * 6;
        T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
               (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
               (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
               (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
               (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
               (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w;
    }
    p->T_wheel = T[0];
    p->T_hip = T[1];
}