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

#ifndef WHEEL_LEGGED_LEG_H
#define WHEEL_LEGGED_LEG_H

#include <matrix.h>
#include "controller.h"

typedef struct
{
    float L0;
    float phi0;

    float phi1;
    float phi4;

    float phi2;
    float phi3;

    uint32_t DWT_CNT;
    float dt;        //用于计算时间并求导
    float last_phi0;
    float phi0_dot;

    float L0_dot;

}Leg_State_t;

typedef struct
{
    PID_Init_Config_s length_PID_conf;
    PID_Init_Config_s phi0_PID_conf;
    float init_target_L0;    //初始腿长目标值
    float F_feedforward;
}Leg_init_config_s;

class leg
{
public:
    leg(Leg_init_config_s* legInitConfig);
    void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
    void input_update(float F,float Tp);
    void feedback_update(float T1_fb,float T2_fb);
    void calculate_output();
    Matrixf<4,1> get_state();
    float calculate_FN(float MotionAccel_z,float theta,float theta_dot);

    float F_feedforward_; //支持力前馈补偿
private:
    Leg_State_t legState_;
    Matrixf<2,1> legInput_;
    Matrixf<2,1> legOutput_;
    Matrixf<2,1> legFeedback_;

    Matrixf<2,1> State_dot_; // L0 和 phi0 的导数

    float target_L0_;
    float target_phi0_;
    PIDInstance Length_PID_;
    PIDInstance Phi0_PID_;

    Matrixf<2,2> VMC_Jacobi_;

    void get_dot(float phi1_dot,float phi4_dot);
};

#endif //WHEEL_LEGGED_LEG_H