#include "component/kinematics.h"
#include <math.h>



/**
  * func dh参数计算齐次变换矩阵
  * para dh参数  输出矩阵
*/
void DH_to_A(DH_parameter_t* dh_parm ,mat_t* dst_mat)
{
  float ct = arm_cos_f32(dh_parm->theta+dh_parm->theta_offset);
  float st = arm_sin_f32(dh_parm->theta+dh_parm->theta_offset);
  float ca = arm_cos_f32(dh_parm->alpha);
  float sa = arm_sin_f32(dh_parm->alpha);

  float32_t buf[16] = {
     ct,   -st*ca,  st*sa,  dh_parm->a*ct ,
     st,    ct*ca, -ct*sa,  dh_parm->a*st ,
     0,       sa,     ca,    dh_parm->d   ,
     0,       0,      0,        1          };
  memcpy(dst_mat->pData, buf, 16*sizeof(float32_t));

}




/**
 * 机械臂初始化
 * 机械臂结构体 dh参数表 齐次变换矩阵组 矩阵内存 连杆数量
 */
void robot_init(robot_t* robot,DH_parameter_t* link,mat_t* T_mat,float* mat_pdata,int linknum)
{
  robot->LinkNum=linknum;
  robot->Link=link;
  for (int a=0; a<linknum; a++) {
    T_mat[a].pData=  mat_pdata + a*16;
    T_mat[a].numCols =4;
    T_mat[a].numRows =4;
  }
  robot->T_mat = T_mat;
  for (int a=0; a<linknum; a++) {
    DH_to_A(&(robot->Link[a]), &(robot->T_mat[a]));
  
  }

}



/*位姿提取*/
void A_to_pose(mat_t* A_mat, pose_t *pose) {
  float *m = A_mat->pData;

  // 位置
  pose->x = m[3];
  pose->y = m[7];
  pose->z = m[11];

  // 旋转矩阵元素
  float r11 = m[0], r12 = m[1], r13 = m[2];
  float r21 = m[4], r22 = m[5], r23 = m[6];
  float r31 = m[8], r32 = m[9], r33 = m[10];

  pose->pitch = asinf(-r31);
  if (fabsf(cosf(pose->pitch)) > 1e-6f) {
    pose->yaw = atan2f(r21, r11);
    pose->roll = atan2f(r32, r33);
  } else {
    // 万向节锁死（pitch ≈ ±90°）时的处理
    pose->yaw = atan2f(-r12, r22);
    pose->roll = 0.0f;
  }
}

/**
  * func 正向运动学解算
* para robot结构 位置姿态结构
*/

void fkine(robot_t* robot)
{
  float pdata_buf[16];
  mat_t T_pose={.numCols=4,.numRows=4,.pData =pdata_buf};
  float pdata_buf1[16];
  mat_t T_pose1={.numCols=4,.numRows=4,.pData =pdata_buf1};
  mat_mul(&(robot->T_mat[0]),&(robot->T_mat[1]),&T_pose);

  for(int i =2;i<robot->LinkNum;i++)
      {
        mat_mul(&(T_pose),&(robot->T_mat[i]),&T_pose1);
        memcpy(T_pose.pData, T_pose1.pData, 16*sizeof(float));
      }
  A_to_pose(&T_pose, &(robot->pose));
}

/** 
* func 设定并启动一次轨迹规划
*/

void PathStart(path_param_t* path_param,float q0,float qf,float v0,float vf,float time,float t0)
{
  if(path_param->state ==0)
  {
    path_param->qf = qf;
    path_param->t0 = t0;
    path_param->a0 = q0;
    path_param->a1 = v0;
    path_param->a2 = (3*(qf- q0) - (2*v0 + vf)*time)/(time*time);
    path_param->a3 = (2*(q0 -qf) + (v0 + vf)*time)/(time*time*time);
    path_param->state =1;
  }
}


/**
  * func 计算当前位置和速度
*/
void PathCalcu(path_param_t* path_param,float t0)
{
  if(path_param->state)
  {
    float time = t0 - path_param->t0;
    path_param->pout = path_param->a0 + path_param->a1*time + path_param->a2*time*time + path_param->a3*time*time*time;
    path_param->vout = path_param->a1 + 2*path_param->a2*time + 3*path_param->a3*time*time;
    if(fabs(path_param->pout- path_param->qf)<0.001)
    {
      path_param->state =0;
    }
  }
}


