#pragma once
#include "user_math.h"

#define pi M_PI
/**
 * DH参数结构体
 */
typedef struct DH_parameter_t {
  float a;
  float alpha;
  float d;
  float theta;
  float theta_offset;
} DH_parameter_t;

/**
 * 位置姿态结构体
 */
typedef struct pose_t{
  float x;
  float y;
  float z;
  float yaw;
  float pitch;
  float roll;
}pose_t;


/**
* robot结构体
*/
typedef struct robot_t {
  int LinkNum; //连杆数量
  DH_parameter_t* Link; //dh参数
  mat_t* T_mat;
  pose_t pose; //末端位姿
}robot_t;

/**
* 轨迹规划器 结构体
*/
typedef struct path_param_t{
  float qf;
  float a0;
  float a1;
  float a2;
  float a3;
  float t0;
  float state;
  float pout;
  float vout;
}path_param_t;


void robot_init(robot_t* robot,DH_parameter_t* link,mat_t* T_mat,float* mat_pdata,int linknum);
void DH_to_A(DH_parameter_t* dh_parm ,mat_t* dst_mat);
void A_to_pose(mat_t* A_mat, pose_t *pose);
void fkine(robot_t* robot);
void PathStart(path_param_t* path_param,float q0,float qf,float v0,float vf,float time,float t0);
void PathCalcu(path_param_t* path_param,float t0);
