//
// Created by SJQ on 2024/3/2.
//

#include "balance.h"
void get_lqr_k(float L0, float K[12]);

balance::balance(Balance_Init_config_s *InitConfig):
leg_r_(&InitConfig->legInitConfig),leg_l_(&InitConfig->legInitConfig)
{
    PIDInit(&leg_coordination_PID_,&InitConfig->leg_cor_PID_config);
    PIDInit(&leg_length_PID_,&InitConfig->legInitConfig.length_PID_conf);
    memset(state_,0,6);
    memset(target_state_,0,6);
}

void balance::feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z)
{
    leg_r_.state_update(leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
    leg_l_.state_update(leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);

    Matrixf<4,1> leg_r_state = leg_r_.get_state();
    Matrixf<4,1> leg_l_state = leg_l_.get_state();
    float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
    float phi_average = (leg_r_state[1][0] + leg_l_state[1][0])/2;
    float phi_dot_average = (leg_r_state[3][0] + leg_l_state[3][0])/2;

    float theta = PI/2 - phi_average - body_phi;
    float theta_dot = -phi_dot_average - body_phi_dot;
    state_[0] = theta;
    state_[1] = theta_dot;
    state_[2] = x; state_[3] = x_dot;
    state_[4] = body_phi; state_[5] = body_phi_dot;

    float  K_mat[12];
    get_lqr_k(L_average,K_mat);
    balance_LQR_.set_k(K_mat);
}

void balance::control_loop() {
    balance_LQR_.update(state_,target_state_);

    Matrixf<2,1> control = balance_LQR_.get_control();
    Matrixf<4,1> leg_r_state = leg_r_.get_state();
    Matrixf<4,1> leg_l_state = leg_l_.get_state();

    float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
    float F_pid_out=  PIDCalculate(&leg_length_PID_,L_average,target_length_);
    float coordination_err = leg_r_state[1][0] - leg_l_state[1][0];
    float cor_Tp = PIDCalculate(&leg_coordination_PID_,coordination_err,0);

    leg_r_.input_update(F_pid_out + leg_r_.F_feedforward_ + 20,control[1][0] + cor_Tp);
    leg_l_.input_update(F_pid_out + leg_l_.F_feedforward_ + 20,control[1][0] - cor_Tp);
}

void balance::target_set(float x) {
    target_state_[2] = x;
}

Matrixf<2,1> balance::get_control() {
    return balance_LQR_.get_control();
}

void get_lqr_k(float L0, float K[12])
{
    float t2;
    float t3;

    float K_temp[12];
    /*     This function was generated by the Symbolic Math Toolbox version 9.2.
     */
    t2 = L0 * L0;
    t3 = t2 * L0;
    /*     24-Jan-2024 14:05:09 */
    K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
    K_temp[1] =
            ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F;
    K_temp[2] =
            ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F;
    K_temp[3] =
            ((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F;
    K_temp[4] =
            ((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F;
    K_temp[5] =
            ((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F;
    K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
                10.4576302F;
    K_temp[7] =
            ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F;
    K_temp[8] =
            ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F;
    K_temp[9] =
            ((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F;
    K_temp[10] =
            ((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F;
    K_temp[11] =
            ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F;

    //matlab赋值顺序不同
    for (int i = 0; i < 6; ++i) {
        K[i] = K_temp[2*i];
    }
    for (int i = 0; i < 6; ++i) {
        K[6+i] = K_temp[2*i+1];
    }
}