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

#ifndef WHEEL_LEGGED_LEG_OLD_H
#define WHEEL_LEGGED_LEG_OLD_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 phi0_dot_num;

    float L0_dot;
    float L0_dot_num;

}Leg_State_t;

typedef struct
{
    float F;
    float Tp;
}Leg_Input_t;

typedef struct
{
    float T1;
    float T2;
}Leg_Output_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;
typedef struct
{
    Leg_State_t legState;
    Leg_Input_t legInput;
    Leg_Input_t legFeedback;
    Leg_Output_t legOutput;

    float target_L0;
    float target_phi0;
    PIDInstance Length_PID;
    PIDInstance Phi0_PID;

    float F_feedforward; //支持力前馈补偿

    arm_matrix_instance_f32 VMC_Jacobi;
    float VMC_Jacobi_data[4];
    arm_matrix_instance_f32 inv_VMC_Jacobi;
    float inv_VMC_Jacobi_data[4];
}LegInstance;



void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot);
void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t* leg_output);
void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp);
void Leg_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb);

void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig);
void Leg_length_control_loop(LegInstance* legInstance);
void VMC_getT(LegInstance* legInstance);


#endif //WHEEL_LEGGED_LEG_OLD_H