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

#include "leg_old.h"
#include "math.h"
#include "main.h"
#include "arm_math.h"

#include "cal_phi0_dot.h"


static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
static void calculate_leg_pos(Leg_State_t* leg_state);
static void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3,
                            float phi4, float F, float Tp, float T12[2]);
void VMC_getT(LegInstance* legInstance);

void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig)
{
    PIDInit(&legInstance->Length_PID,&legInitConfig->length_PID_conf);
    PIDInit(&legInstance->Phi0_PID,&legInitConfig->phi0_PID_conf);
    legInstance->target_L0 = legInitConfig->init_target_L0;
    legInstance->F_feedforward = legInitConfig->F_feedforward;

    arm_mat_init_f32(&legInstance->VMC_Jacobi,2,2,legInstance->VMC_Jacobi_data);
    arm_mat_init_f32(&legInstance->inv_VMC_Jacobi,2,2,legInstance->inv_VMC_Jacobi_data);
    // set rest of memory to 0
    DWT_GetDeltaT(&legInstance->legState.DWT_CNT);
}

void Leg_length_control_loop(LegInstance* legInstance)
{
    float F_pid_out = PIDCalculate(&legInstance->Length_PID,legInstance->legState.L0,legInstance->target_L0);
    float Tp_pid_out = PIDCalculate(&legInstance->Phi0_PID,legInstance->legState.phi0,legInstance->target_phi0);
    Leg_Input_update(&legInstance->legInput,F_pid_out-2.73f,Tp_pid_out);
    //Leg_calc_output(&legInstance->legState,&legInstance->legInput,&legInstance->legOutput);
    VMC_getT(legInstance);
}
void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot)
{
    Leg_State_t* leg_state = &legInstance->legState;
    if(phi1<=PI/2)
        phi1 = PI/2;
    if(phi4>=PI/2)
        phi4 = PI/2;

    leg_state->phi1 = phi1;
    leg_state->phi4 = phi4;
    float leg_pos[4];

    //calculate_leg_pos(leg_state->phi1,leg_state->phi4,leg_pos);
    calculate_leg_pos(leg_state);


    //对phi0求导
    leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
    //if(leg_state->last_phi0>=0)
        leg_state->phi0_dot_num = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
    //else
        //leg_state->phi0_dot_num = 0;
    leg_state->last_phi0 = leg_state->phi0;


    leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
    leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot);
}

void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp)
{
    leg_Input->F = F;
    leg_Input->Tp = Tp;
}

void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t* leg_output)
{
//    float T12[2] = {0,0};
//    vmcJacobian(leg_state->L0,leg_state->phi0,leg_state->phi1,
//                 leg_state->phi2,leg_state->phi3,leg_state->phi4,
//                 leg_input->F,leg_input->Tp,T12);
//    leg_output->T1 = T12[0];
//    leg_output->T2 = T12[1];
}


void calculate_leg_pos(Leg_State_t* leg_state)
{
    //以机体中点建立坐标系
    float xb = -l[4]/2+l[0]* arm_cos_f32(leg_state->phi1);
    float yb = l[0]* arm_sin_f32(leg_state->phi1);

    float xd = l[4]/2+l[3]* arm_cos_f32(leg_state->phi4);
    float yd = l[3]* arm_sin_f32(leg_state->phi4);

    float A0 = 2*l[1]*(xd-xb);
    float B0 = 2*l[1]*(yd-yb);

    float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
    float C0 = l[1]*l[1]+lbd2-l[2]*l[2];

    float tanPhi = 0;

    arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi);
    //tanPhi = (B0+tanPhi)/(A0+C0);

    float phi2 = 2* atan2f(B0+tanPhi,A0+C0);
    float xc = xb + l[1] * arm_cos_f32(phi2);
    float yc = yb + l[1] * arm_sin_f32(phi2);

    float phi3 = acosf(xc-xd/l[2]);
    float L0 = 0;
    arm_sqrt_f32((xc*xc + yc*yc),&L0);
    float phi0 = atan2f(yc,xc);

    //赋值
    leg_state->L0 = L0;
    leg_state->phi0 = phi0;
    leg_state->phi2 = phi2;

    leg_state->phi3 = phi3;
}

void VMC_getT(LegInstance* legInstance)
{
    //arm_matrix_instance_f32 T_matrix;
    arm_matrix_instance_f32 F_Tp_matrix;
//    float T_array[4] = {0};
    float F_Tp_array[2] = {0};

    F_Tp_array[0] = legInstance->legInput.F;
    F_Tp_array[1] = legInstance->legInput.Tp;

    float sinPhi23 = arm_sin_f32(legInstance->legState.phi2 - legInstance->legState.phi3);
    float sinPhi12 = arm_sin_f32(legInstance->legState.phi1 - legInstance->legState.phi2);
    float sinPhi34 = arm_sin_f32(legInstance->legState.phi3 - legInstance->legState.phi4);

    float sinPhi03 = arm_sin_f32(legInstance->legState.phi0 - legInstance->legState.phi3);
    float sinPhi02 = arm_sin_f32(legInstance->legState.phi0 - legInstance->legState.phi2);
    float cosPhi03 = arm_cos_f32(legInstance->legState.phi0 - legInstance->legState.phi3);
    float cosPhi02 = arm_cos_f32(legInstance->legState.phi0 - legInstance->legState.phi2);

//    T_array[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
//    T_array[1] = -(l[0]* cosPhi03*sinPhi12)/(legInstance->legState.L0 * sinPhi23);
//
//    T_array[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
//    T_array[3] = -(l[3]* cosPhi02*sinPhi34)/(legInstance->legState.L0 * sinPhi23);
    legInstance->VMC_Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
    legInstance->VMC_Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legInstance->legState.L0 * sinPhi23);

    legInstance->VMC_Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
    legInstance->VMC_Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legInstance->legState.L0 * sinPhi23);

    //arm_mat_init_f32(&T_matrix,2,2,T_array);
    arm_mat_init_f32(&F_Tp_matrix,2,1,F_Tp_array);

    arm_matrix_instance_f32 T12_matrix;
    float T12[2] = {0};
    arm_mat_init_f32(&T12_matrix,2,1,T12);

    arm_mat_mult_f32(&legInstance->VMC_Jacobi,&F_Tp_matrix,&T12_matrix);
    legInstance->legOutput.T1 = T12_matrix.pData[0];
    legInstance->legOutput.T2 = T12_matrix.pData[1];
}

void Leg_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb)
{
    arm_mat_inverse_f32(&legInstance->VMC_Jacobi,&legInstance->inv_VMC_Jacobi);

    arm_matrix_instance_f32 F_Tp_matrix;
    float F_Tp_array[2] = {0};
    arm_mat_init_f32(&F_Tp_matrix,2,1,F_Tp_array);

    arm_matrix_instance_f32 T12_matrix;
    float T12[2] = {0};
    arm_mat_init_f32(&T12_matrix,2,1,T12);

    T12[0] = T1_fb; T12[1] = T2_fb;
    arm_mat_mult_f32(&legInstance->inv_VMC_Jacobi,&T12_matrix,&F_Tp_matrix);

    legInstance->legFeedback.F = F_Tp_array[0];
    legInstance->legFeedback.Tp = F_Tp_array[1];
}

void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3,
                  float phi4, float F, float Tp, float T12[2])
{
    float T_tmp;
    float b_T_tmp;
    float c_T_tmp;
    float d_T_tmp;
    float e_T_tmp;
    float f_T_tmp;
    T_tmp = phi0 - phi3;
    b_T_tmp = arm_sin_f32(phi1 - phi2);
    c_T_tmp = arm_sin_f32(phi2 - phi3);
    d_T_tmp = phi0 - phi2;
    e_T_tmp = arm_sin_f32(phi3 - phi4);
    f_T_tmp = L0 * c_T_tmp;
    T12[0] = -(0.15F * arm_sin_f32(T_tmp) * b_T_tmp) / c_T_tmp * F +
             -(0.15F * arm_cos_f32(T_tmp) * b_T_tmp) / f_T_tmp * Tp;
    T12[1] = -(0.15F * arm_sin_f32(d_T_tmp) * e_T_tmp) / c_T_tmp * F +
             -(0.15F * arm_cos_f32(d_T_tmp) * e_T_tmp) / f_T_tmp * Tp;
}