//
// Created by SJQ on 2023/12/30.
//

#ifndef WHEEL_LEGGED_BALANCE_H
#define WHEEL_LEGGED_BALANCE_H

#include "leg.h"
#include "LQR_old.h"
#include "controller.h"
#include "kalman_filter.h"
#include "estimator.h"
#include "observer.h"
typedef struct
{
    LegInstance leg_right;
    LegInstance leg_left;
    PIDInstance leg_coordination_PID;  //双腿协调PD控制

    PIDInstance leg_length_PID; //平均腿长控制

    float state_r[6];
    float target_state_r[6];
    float state_l[6];
    float target_state_l[6];

    float v_est[4];

    float target_L0; //平均腿长目标值
    float FN_left; //地面支持力
    float FN_right;

    //双腿LQR分开计算
    LQRInstance LQR_l;
    LQRInstance LQR_r;
    estimator_t v_estimator;
    BalanceObserver state_observer;

    uint8_t jump_flag;//跳跃标志
    uint8_t fly_flag_r; //右腿离地标志
    uint8_t fly_flag_l; //左腿离地标志

}Balance_Control_t;

typedef struct
{
    Leg_init_config_s legInitConfig;
    PID_Init_Config_s leg_cor_PID_config;

    int LQR_state_num;
    int LQR_control_num;
}Balance_Init_config_s;

void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
//void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,const float x[2],const float x_dot[2],const float MotionAccel[3]);
void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag);
void target_x_set(Balance_Control_t* balanceControl,float add_x);
void target_L_set(Balance_Control_t* balanceControl,float add_L);
void state_clear(Balance_Control_t* balanceControl);

#endif //WHEEL_LEGGED_BALANCE_H