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

#include "leg.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;

    // 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 = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
//    else
//        leg_state->phi0_dot = 0;
//    leg_state->last_phi0 = leg_state->phi0;
    leg_state->phi0_dot = get_dphi0(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};
//    VMC_jacobian(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);

    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(&T_matrix,&F_Tp_matrix,&T12_matrix);
    legInstance->legOutput.T1 = T12_matrix.pData[0];
    legInstance->legOutput.T2 = T12_matrix.pData[1];
}


////此段代码为matlab生成
//static void calculate_leg_pos(const float phi1, const float phi4, float leg_pos[4])
//{
//    float A0;
//    float B0;
//    float C0;
//    float L0;
//    float phi2;
//    float scale;
//    float xc;
//    float xd;
//    float yb_tmp;
//    /*  以机体中点建立坐标系 */
//    L0 = 0.15F * cosf(phi1);
//    yb_tmp = 0.15F * sinf(phi1);
//    xd = 0.15F * cosf(phi4) + 0.075F;
//    A0 = 0.54F * (xd - (L0 - 0.075F));
//    C0 = 0.15F * sinf(phi4) - yb_tmp;
//    B0 = 0.54F * C0;
//    scale = (0.15F * cosf(phi4) + 0.075F) - (L0 - 0.075F);
//    C0 = ((scale * scale + C0 * C0) + 0.0729000047F) - 0.0729000047F;
//    phi2 = 2.0F * atanf((B0 + sqrtf((A0 * A0 + B0 * B0) - C0 * C0)) / (A0 + C0));
//    xc = L0 + 0.27F * cosf(phi2);
//    C0 = yb_tmp + 0.27F * sinf(phi2);
//    scale = 1.29246971E-26F;
//    A0 = fabsf(xc);
//    if (A0 > 1.29246971E-26F) {
//        L0 = 1.0F;
//        scale = A0;
//    } else {
//        B0 = A0 / 1.29246971E-26F;
//        L0 = B0 * B0;
//    }
//    A0 = fabsf(C0);
//    if (A0 > scale) {
//        B0 = scale / A0;
//        L0 = L0 * B0 * B0 + 1.0F;
//        scale = A0;
//    } else {
//        B0 = A0 / scale;
//        L0 += B0 * B0;
//    }
//    L0 = scale * sqrtf(L0);
//    leg_pos[0] = L0;
//    leg_pos[1] = atanf(C0 / xc);
//    leg_pos[2] = phi2;
//    leg_pos[3] = acosf(xc - xd / 0.27F);
//}
//
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;
}