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

#include "kinematics.h"
#include "RobotAlgorithmModule.h"
#include "arm_math.h"
#include "dm_motor_ctrl.h"
Link_Parameter_s Link[4];
double curr = -2.8,curr_3=-0.5;
double 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() {
    //计算通用其次变换矩阵
    int num = 0;
    Link[0].angle = motor[Motor1].para.pos;
    Link[1].angle = motor[Motor2].para.pos;
    Link[2].angle = motor[Motor3].para.pos;
    Link[3].angle = motor[Motor4].para.pos;
    for (num = 0; num<4; num++) {
        Link[num].transformation_matrix[0][0] = cos(Link[num].angle);
        Link[num].transformation_matrix[0][1] = -sin(Link[num].angle) * cos(Link[num].twist);
        Link[num].transformation_matrix[0][2] = sin(Link[num].angle) * sin(Link[num].twist);
        Link[num].transformation_matrix[0][3] = Link[num].length * cos(Link[num].angle);
        Link[num].transformation_matrix[1][0] = sin(Link[num].angle);
        Link[num].transformation_matrix[1][1] = cos(Link[num].angle) * cos(Link[num].twist);
        Link[num].transformation_matrix[1][2] = -cos(Link[num].angle) * sin(Link[num].twist);
        Link[num].transformation_matrix[1][3] = Link[num].length * sin(Link[num].angle);
        Link[num].transformation_matrix[2][0] = 0;
        Link[num].transformation_matrix[2][1] = sin(Link[num].twist);
        Link[num].transformation_matrix[2][2] = cos(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
    double m = Total_Transformation_Matrix[1][3] - Link[3].length * Total_Transformation_Matrix[1][0] -
              Link[3].offset * Total_Transformation_Matrix[1][2];
    double n = Total_Transformation_Matrix[0][3] - Link[3].length * Total_Transformation_Matrix[0][0] -
              Link[3].offset * Total_Transformation_Matrix[0][2];

    Link[0].angle_set = atan2(m, n) - atan2(Link[1].offset, 0);


    //关节3
    Link[2].angle_set = acos(Total_Transformation_Matrix[1][2] * cos(Link[0].angle_set) -
                              Total_Transformation_Matrix[0][2] * sin(Link[0].angle));
    Link[2].angle_set2 = -acos(Total_Transformation_Matrix[1][2] * cos(Link[0].angle_set) -
                               Total_Transformation_Matrix[0][2] * sin(Link[0].angle));

    //关节4
    double mm = -Total_Transformation_Matrix[1][2];
    double nn = Total_Transformation_Matrix[0][2];
    Link[3].angle_set2    = atan2(mm, nn) - atan2(cos(Link[1].angle_set), sqrt(mm * mm + nn * nn -
                                                                                cos(Link[1].angle_set) *
                                                                                cos(Link[1].angle_set)));
    Link[3].angle_set = atan2(mm, nn) - atan2(sin(Link[1].angle_set), -sqrt(mm * mm + nn * nn -
                                                                                cos(Link[1].angle_set) *
                                                                                cos(Link[1].angle_set)));

    //关节2
    Link[1].angle_set = asin(Total_Transformation_Matrix[2][2] / sin(Link[2].angle_set));
    Link[1].angle_set2 = -asin(Total_Transformation_Matrix[2][2] / sin(Link[2].angle_set));


    motor[Motor1].ctrl.pos_set = Link[0].angle_set;

    if(Link[2].angle>=0) motor[Motor2].ctrl.pos_set = Link[1].angle_set;
    else motor[Motor2].ctrl.pos_set = Link[1].angle_set2;

    if(Link[2].angle>=0) motor[Motor3].ctrl.pos_set = Link[2].angle_set;
    else motor[Motor3].ctrl.pos_set = Link[2].angle_set2;
    motor[Motor4].ctrl.pos_set = Link[3].angle_set;

    motor[Motor1].ctrl.tor_set = curr * sin(Link[0].angle);
    motor[Motor3].ctrl.tor_set = curr_3 * sin(Link[2].angle);

}