//
// Created by 24871 on 2024/10/2.
//

#include "kinematics.h"
#include "RobotAlgorithmModule.h"
#include "arm_math.h"
Link_Parameter_s Link[4];
float Total_Transformation_Matrix[4][4],Process1_Transformation_Matrix[4][4],Process2_Transformation_Matrix[4][4];
void Kinematic_Init()
{
    Link[0].length = LINK_LENGTH1;
    Link[1].length = LINK_LENGTH2;
    Link[2].length = LINK_LENGTH3;
    Link[3].length = LINK_LENGTH4;

    Link[0].offset = LINK_OFFSET1;
    Link[1].offset = LINK_OFFSET2;
    Link[2].offset = LINK_OFFSET3;
    Link[3].offset = LINK_OFFSET4;

    Link[0].twist = LINK_TWIST1;
    Link[1].twist = LINK_TWIST2;
    Link[2].twist = LINK_TWIST3;
    Link[3].twist = LINK_TWIST4;
}

void Kinematic_Calculate()
{
    //计算通用其次变换矩阵
    for(int num=0;num++;num<4)
    {
        Link[num].transformation_matrix[0][0]=cosf(Link[num].angle);
        Link[num].transformation_matrix[0][1]=-sinf(Link[num].angle)*cosf(Link[num].twist);
        Link[num].transformation_matrix[0][2]=sinf(Link[num].angle)*sinf(Link[num].twist);
        Link[num].transformation_matrix[0][3]=Link[num].length*cosf(Link[num].angle);
        Link[num].transformation_matrix[1][0]=sinf(Link[num].angle);
        Link[num].transformation_matrix[1][1]=cosf(Link[num].angle)*cosf(Link[num].twist);
        Link[num].transformation_matrix[1][2]=-cosf(Link[num].angle)*sinf(Link[num].twist);
        Link[num].transformation_matrix[1][3]=Link[num].length*sinf(Link[num].angle);
        Link[num].transformation_matrix[2][0]=0;
        Link[num].transformation_matrix[2][1]=sinf(Link[num].twist);
        Link[num].transformation_matrix[2][2]=cosf(Link[num].twist);
        Link[num].transformation_matrix[2][3]=Link[num].offset;
        Link[num].transformation_matrix[3][0]=0;
        Link[num].transformation_matrix[3][1]=0;
        Link[num].transformation_matrix[3][2]=0;
        Link[num].transformation_matrix[3][3]=1;


    }
    //求出总变换（正运动学）
    Matrix4Mult(Link[0].transformation_matrix,Link[1].transformation_matrix,Process1_Transformation_Matrix);
    Matrix4Mult(Process1_Transformation_Matrix,Link[2].transformation_matrix,Process2_Transformation_Matrix);
    Matrix4Mult(Process2_Transformation_Matrix,Link[3].transformation_matrix,Total_Transformation_Matrix);

    //逆运动学
    //关节1
    float m=Total_Transformation_Matrix[1][3]-Link[3].length*Total_Transformation_Matrix[1][0]-Link[3].offset*Total_Transformation_Matrix[1][2];
    float n=Total_Transformation_Matrix[0][3]-Link[3].length*Total_Transformation_Matrix[0][0]-Link[3].offset*Total_Transformation_Matrix[0][2];
    //TODO:多解，需要一个判断位来选择解
    Link[0].angle_set = atan2f(m,n)-atan2f(Link[1].offset, sqrtf(m*m+n*n-Link[2].offset*Link[2].offset));
    Link[0].angle_set = atan2f(m,n)-atan2f(Link[1].offset, -sqrtf(m*m+n*n-Link[2].offset*Link[2].offset));

    //关节3
    //TODO:多解问题
    Link[2].angle_set = acosf(Total_Transformation_Matrix[1][2]*cosf(Link[0].angle_set)-Total_Transformation_Matrix[0][2]*sinf(Link[0].angle));
    Link[2].angle_set = -acosf(Total_Transformation_Matrix[1][2]*cosf(Link[0].angle_set)-Total_Transformation_Matrix[0][2]*sinf(Link[0].angle));




}