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


#include "leg.h"
#include <cmath>


static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};

leg::leg(Leg_init_config_s* legInitConfig)
{
    PIDInit(&Length_PID_,&legInitConfig->length_PID_conf);
    PIDInit(&Phi0_PID_,&legInitConfig->phi0_PID_conf);

    target_L0_ = legInitConfig->init_target_L0;
    F_feedforward_ = legInitConfig->F_feedforward;
}

void leg::input_update(float F, float Tp)
{
    legInput_ = Matrixf<2,1>(new (float[2]){F,Tp});
}

void leg::state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot)
{
    if(phi1<=PI/2)
        phi1 = PI/2;
    if(phi4>=PI/2)
        phi4 = PI/2;

    legState_.phi1 = phi1;
    legState_.phi4 = phi4;

    //以机体中点建立坐标系
    float xb = -l[4]/2+l[0]* arm_cos_f32(legState_.phi1);
    float yb = l[0]* arm_sin_f32(legState_.phi1);

    float xd = l[4]/2+l[3]* arm_cos_f32(legState_.phi4);
    float yd = l[3]* arm_sin_f32(legState_.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);

    //赋值
    legState_.L0 = L0;
    legState_.phi0 = phi0;
    legState_.phi2 = phi2;
    legState_.phi3 = phi3;

    get_dot(phi1_dot,phi4_dot);
}

void leg::calculate_output() {
    float sinPhi23 = arm_sin_f32(legState_.phi2 - legState_.phi3);
    float sinPhi12 = arm_sin_f32(legState_.phi1 - legState_.phi2);
    float sinPhi34 = arm_sin_f32(legState_.phi3 - legState_.phi4);

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

    float Jacobi_data[4] = {0};
    Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
    Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legState_.L0 * sinPhi23);

    Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
    Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legState_.L0 * sinPhi23);

    VMC_Jacobi_ = Matrixf<2,2>(Jacobi_data);
    legOutput_ = VMC_Jacobi_ * legInput_;
}

void leg::feedback_update(float T1_fb, float T2_fb) {
    Matrixf<2,1> T12(new (float[2]){T1_fb,T2_fb});
    legFeedback_ = matrixf::inv(VMC_Jacobi_) * T12;
}

void leg::get_dot(float phi1_dot,float phi4_dot)
{
    //多项式拟合方法获得雅可比矩阵  待测试
    float phi_1 = legState_.phi1;
    float phi_4 = legState_.phi4;

    float phi1_sq = phi_1*phi_1;
    float phi4_sq = phi_4*phi_4;
    float pose_Jacobi[4] = {0};
    float temp = sqrtf(powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2));

    pose_Jacobi[0] =
            (0.5f*(2.0f*(0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - 2.0f*(0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
    pose_Jacobi[1] =
            (0.5f*(2.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + 2.0f*(0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;

    float temp2 = (powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + 1.0f);

    pose_Jacobi[2] =
            ((0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + ((0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2))/temp2;
    pose_Jacobi[3] =
            -(1.0f*((0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - (1.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2)))/temp2;
    Matrixf<2,2> pose_J(pose_Jacobi);

    State_dot_ = pose_J * Matrixf<2,1>(new (float[2]){phi1_dot,phi4_dot});
}

Matrixf<4, 1> leg::get_state() {
    return Matrixf<4, 1>(new (float[4]) {legState_.L0,legState_.phi0,*State_dot_[0],*State_dot_[1]});
}

float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
    //求导所用时间
    static uint32_t dot_cnt = 0;
    static float last_theta_dot,last_L_dot;
    float L = legState_.L0;
    float L_dot = State_dot_[0][0];

    float dot_dt = DWT_GetDeltaT(&dot_cnt);
    float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
    float L_dot2 = (last_L_dot - L_dot) / dot_dt;

    float cos_theta = arm_cos_f32(theta);
    float sin_theta = arm_sin_f32(theta);
    float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta
                    + L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta;

    float P = legFeedback_[0][0]*cos_theta + legFeedback_[1][0]*sin_theta/L;

    float F_N = P + 0.8f * (zw_dot2 + 9.8f);

    return F_N;
}