#include "module/arm.h"

#include "module/config.h"
#include "device/motor_dm.h"
#include "device/motor_lz.h"
#include "component/pid.h"
#include "component/user_math.h"
#include "bsp/dwt.h"
#include "component/cmd.h"







//用于机械臂重力补偿 lxqx:linkx质心位置对qx的偏导数 由sympy符号计算库计算生成
double compute_l5q2(double q1, double q2, double q3, double q4, double q5) {
    return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
}
double compute_l5q3(double q1, double q2, double q3, double q4, double q5) {
    return 0.063171000000000005*((-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(6.123233995736766e-17*sin(q2)*sin(q3) - 3.749399456654644e-33*sin(q2)*cos(q3) - 3.749399456654644e-33*sin(q3)*cos(q2) - 6.123233995736766e-17*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + 1.0*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) - 1.0*cos(q2)*cos(q3))*sin(q5 + 1.5707963267948966) + 0.31590000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.31590000000000001*cos(q2)*cos(q3);
}
double compute_l5q4(double q1, double q2, double q3, double q4, double q5) {
    return 0.063171000000000005*(-(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966))*cos(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966);
}
double compute_l5q5(double q1, double q2, double q3, double q4, double q5) {
  q3+=0.01;
    return -0.063171000000000005*((-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*cos(q4 + 1.5707963267948966) + (-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*sin(q4 + 1.5707963267948966))*sin(q5 + 1.5707963267948966) + 0.063171000000000005*(-6.123233995736766e-17*(-1.0*sin(q2)*sin(q3) + 6.123233995736766e-17*sin(q2)*cos(q3) + 6.123233995736766e-17*sin(q3)*cos(q2) + 1.0*cos(q2)*cos(q3))*sin(q4 + 1.5707963267948966) + 6.123233995736766e-17*(-3.749399456654644e-33*sin(q2)*sin(q3) - 6.123233995736766e-17*sin(q2)*cos(q3) - 6.123233995736766e-17*sin(q3)*cos(q2) + 3.749399456654644e-33*cos(q2)*cos(q3) - 6.123233995736766e-17)*cos(q4 + 1.5707963267948966) - 6.123233995736766e-17*sin(q2)*sin(q3) - 1.0*sin(q2)*cos(q3) - 1.0*sin(q3)*cos(q2) + 6.123233995736766e-17*cos(q2)*cos(q3) + 3.749399456654644e-33)*cos(q5 + 1.5707963267948966);
}
double compute_l4q2(double q1, double q2, double q3, double q4, double q5) {
    return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
}
double compute_l4q3(double q1, double q2, double q3, double q4, double q5) {
    return 0.22925000000000001*sin(q2)*sin(q3) - 0.10370000000000001*sin(q2)*cos(q3) - 0.10370000000000001*sin(q3)*cos(q2) - 0.22925000000000001*cos(q2)*cos(q3);
}
double compute_l3q2(double q1, double q2, double q3, double q4, double q5) {
    return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3) + 0.40200000000000002*cos(q2);
}
double compute_l3q3(double q1, double q2, double q3, double q4, double q5) {
  
    return -0.0048250000000000003*sin(q2)*sin(q3) - 0.11803*sin(q2)*cos(q3) - 0.11803*sin(q3)*cos(q2) + 0.0048250000000000003*cos(q2)*cos(q3);
}
double compute_l2q2(double q1, double q2, double q3, double q4, double q5) {
    return 0.27905999999999997*cos(q2+0.07);
}






//机械臂初始化
void arm_init(arm_t* arm,Config_RobotParam_t *cfg)
{
    assert_param(!(cfg==NULL||arm==NULL));

  for (int i=0; i<JOINT_NUM; i++) {
    switch (cfg->arm_param.joint[i].motor.type) {
      case DM_MOTOR:
        MOTOR_DM_Register(&cfg->arm_param.joint[i].motor.param.dm_param);
        arm->joint[i].motor.handle=MOTOR_DM_GetMotor(&cfg->arm_param.joint[i].motor.param.dm_param);
        MOTOR_DM_Enable(&cfg->arm_param.joint[i].motor.param.dm_param);
      break;
      case LZ_MOTOR:
        MOTOR_LZ_Register(&cfg->arm_param.joint[i].motor.param.lz_param);
        arm->joint[i].motor.handle=MOTOR_LZ_GetMotor(&cfg->arm_param.joint[i].motor.param.lz_param);
        MOTOR_LZ_Enable(&cfg->arm_param.joint[i].motor.param.lz_param);
      break;
    }
    arm->joint[i].motor.type=cfg->arm_param.joint[i].motor.type;
    PID_Init(&arm->joint[i].controler.Ppid,KPID_MODE_SET_D,1000,&cfg->arm_param.joint[i].pid.Ppid);
    PID_Init(&arm->joint[i].controler.Vpid,KPID_MODE_NO_D,1000,&cfg->arm_param.joint[i].pid.Vpid);
    arm->joint[i].status.angle_limit[0]=cfg->arm_param.joint[i].angle_limit[0];
    arm->joint[i].status.angle_limit[1]=cfg->arm_param.joint[i].angle_limit[1];


  arm->mass[i]=cfg->arm_param.mass[i];
  }
}

//机械臂重力补偿计算
void arm_gravity_calcu(arm_t* arm)
{
    assert_param(!(arm==NULL));
  arm->joint[4].ref.torque += -1 *
               compute_l5q5(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
               0.403 * 9.81;
  arm->joint[3].ref.torque += -1 *
               compute_l5q4(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
               0.403 * 9.81;
  arm->joint[2].ref.torque += compute_l5q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   0.403 * 9.81 +
               compute_l4q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   0.720 * 9.81 +
               compute_l3q3(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   1.410 * 9.81;
  arm->joint[1].ref.torque += compute_l5q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   0.403 * 9.81 +
               compute_l4q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   0.720 * 9.81 +
               compute_l3q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   1.410 * 9.81 +
               compute_l2q2(arm->joint[0].feedback.position ,arm->joint[1].feedback.position,arm->joint[2].feedback.position,arm->joint[3].feedback.position,
                            arm->joint[5].feedback.position) *
                   1.9889 * 9.81;
}


//机械臂数据更新
void arm_data_updata(arm_t* arm)
{
    assert_param(!(arm==NULL));
  MOTOR_LZ_UpdateAll();
  MOTOR_DM_UpdateAll();
  for (uint8_t i=0; i<JOINT_NUM; i++) {
    assert_param(!(arm->joint[i].motor.handle == NULL));
    float nangle=0;
    float nspeed=0;
    nangle = (arm->joint[i].motor.type==DM_MOTOR)?((__dm_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_abs_angle):((__lz_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_abs_angle);
    nspeed = (arm->joint[i].motor.type==DM_MOTOR)?((__dm_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_speed):((__lz_handle(arm->joint[i].motor.handle))->motor.feedback.rotor_speed);
    if(nangle- arm->joint[i].status.last_angle >M_2PI/2)
    {
      arm->joint[i].status.circles++;
    }
    else if(nangle- arm->joint[i].status.last_angle < -M_2PI/2)
    {
      arm->joint[i].status.circles--;
    }
    arm->joint[i].status.last_angle = nangle;
    arm->joint[i].feedback.velocity = nspeed;
    arm->joint[i].feedback.position = arm->joint[i].status.circles*M_2PI+nangle;
  }

  arm->joint[2].feedback.velocity = arm->joint[2].feedback.velocity - arm->joint[1].feedback.velocity;
  arm->joint[2].feedback.position = arm->joint[2].feedback.position - arm->joint[1].feedback.position;
  
}

//机械臂pid计算
void arm_pid_calcu(arm_t* arm)
{
    assert_param(!(arm==NULL));
  for (uint8_t i=0; i<JOINT_NUM; i++) {
    float now_time = DWT_GetTimeline_s();
    arm->joint[i].ref.velocity+= PID_Calc(&arm->joint[i].controler.Ppid, arm->joint[i].ref.position, arm->joint[i].feedback.position,arm->joint[i].feedback.velocity,now_time-arm->joint[i].status.last_control_time);
    arm->joint[i].ref.torque  += PID_Calc(&arm->joint[i].controler.Vpid, arm->joint[i].ref.velocity, arm->joint[i].feedback.velocity, 0, now_time-arm->joint[i].status.last_control_time);
    arm->joint[i].status.last_control_time = now_time;
  }
}

//机械臂控制输出
void arm_control_apply(arm_t* arm)
{
    assert_param(!(arm==NULL));
  for (uint8_t i=0; i<JOINT_NUM; i++) {
    assert_param(!(arm->joint[i].motor.handle == NULL));
    if(arm->joint[i].feedback.position<arm->joint[i].status.angle_limit[0]||arm->joint[i].feedback.position>arm->joint[i].status.angle_limit[1])
    {
      arm->joint[i].ref.torque=0;
    }
    switch (arm->joint[i].motor.type) {
      case DM_MOTOR:
        {
          MOTOR_DM_t* handle= __dm_handle(arm->joint[i].motor.handle);
          MOTOR_MIT_Output_t out;
          out.torque = arm->joint[i].ref.torque;
          MOTOR_DM_MITCtrl(&handle->param, &out);
          break;
        }
      case LZ_MOTOR:
        {
          MOTOR_LZ_t* handle= __lz_handle(arm->joint[i].motor.handle);
          MOTOR_LZ_MotionParam_t out;
          out.torque = arm->joint[i].ref.torque;
          MOTOR_LZ_MotionControl(&handle->param, &out);
          break;
        }
    }
    arm->joint[i].ref.torque=0;
    arm->joint[i].ref.position=0;
    arm->joint[i].ref.velocity=0;
  }
}
void arm_relax(arm_t* arm)
{
  assert_param(!(arm==NULL));

  for (uint8_t i=0; i<JOINT_NUM; i++) {
    if(arm->joint[i].feedback.position<arm->joint[i].status.angle_limit[0]||arm->joint[i].feedback.position>arm->joint[i].status.angle_limit[1])
    {
      arm->joint[i].ref.torque=0;
    }
    switch (arm->joint[i].motor.type) {
      case DM_MOTOR:
        {
          MOTOR_DM_t* handle= __dm_handle(arm->joint[i].motor.handle);
          MOTOR_DM_Relax(&handle->param);
          break;
        }
      case LZ_MOTOR:
        {
          MOTOR_LZ_t* handle= __lz_handle(arm->joint[i].motor.handle);
          MOTOR_LZ_Relax(&handle->param);
          break;
        }
    }
  }
}


void arm_path_start(arm_t* arm,float* target_angle)
{
  assert_param(!(arm==NULL));
  float time=0;
  float delta[JOINT_NUM];
  float max_delta=0;
  uint32_t max_index=0;

  for (uint8_t i=0; i<JOINT_NUM; i++) {
    delta[i]= target_angle[i]-arm->joint[i].feedback.position;
  }
  arm_absmax_f32(delta, JOINT_NUM,&max_delta,&max_index); //arm-cmsis函数
  time = max_delta/10.0f;

  for (uint8_t i=0; i<JOINT_NUM; i++) {
    PathStart(&arm->joint[i].controler.path,arm->joint[i].feedback.position,target_angle[i],0,0,time,DWT_GetTimeline_s());
  }
}

void arm_path_process(arm_t* arm)
{
  assert_param(!(arm==NULL));
  int finish_num=0;
  for(int i=0;i<JOINT_NUM;i++)
  {
    PathCalcu(&arm->joint[i].controler.path, DWT_GetTimeline_s());
    arm->joint[i].ref.position += arm->joint[i].controler.path.pout;   //当前路径位置
    arm->joint[i].ref.velocity += arm->joint[i].controler.path.vout;   //当前路径速度前馈

    finish_num+= arm->joint[i].controler.path.state;
  }
  if(finish_num==0)
  {
    arm->status.path_finish=1;
  }
}


void arm_status_reset(arm_t* arm)
{
  assert_param(!(arm==NULL));
  arm->status.cmd_prepare=0;
  arm->status.busy=0;
  arm->status.step=0;
}

void arm_follow_control(arm_t* arm,CMD_Arm_t* data)
{
  assert_param(!(arm==NULL||data==NULL));
  switch (arm->status.step) {
    case 0:  //步骤一
    {
        if(arm->status.busy==0)  //检测是否非忙碌,并进行第一次初始化
        {
          arm_path_start(arm, data->data.ref_angle); //初始化路径规划,设定终点为控制器当前值
          arm->status.busy=1;  //开始路径，进入忙碌
        }
        arm_path_process(arm);  //计算并设定一次路径点
        if(arm->status.path_finish==1)  //检测是否到达路径终点
        {
          arm->status.path_finish=0;
          arm->status.step=1;  //进入下一步
        }
    break;
    }
    case 1:
    {
        arm->status.busy=0;  //退出忙碌，可以接收指令
        for (int i=0; i<JOINT_NUM; i++) {
          arm->joint[i].ref.position += data->data.ref_angle[i];  //跟踪控制器
        }
    break;
    }
  }
}

void arm_controler_process(arm_t* arm,CMD_Arm_t* data) //自定义控制器指令处理
{
  assert_param(!(arm==NULL||data==NULL));

  if(arm->status.busy == 0&&data->cmd_prepare==1)  //指令接收准备信号
  {
    arm->status.cmd_prepare=1;  //进入指令接收准备状态
  }

  if(data->cmd_prepare==0&&arm->status.cmd_prepare==1)  //接收准备信号结束，并且已处于接收准备状态
  {
    arm_status_reset(arm); //清除状态
    switch (data->mode) {
      case SET_ARM_MODE_RELAX:
          arm->mode = RELAX_MODE;
      break;
      case SET_ARM_MODE_FOLLOW:
          arm->mode = FOLLOW_MODE;
      break;
      case SET_ARM_MODE_STORAGE:
          arm->mode=STORAGE_MODE;
      break;
      case SET_ARM_MODE_TAKE:
          arm->mode=TAKE_MODE;
      break;
      case SET_ARM_MODE_POSITION:
          arm->mode=SET_ARM_MODE_POSITION;
      break;
    }
  }
}

void arm_control_process(arm_t* arm,CMD_Arm_t* data)
{
  switch (arm->mode) {
    case RELAX_MODE:
      arm_relax(arm);
    break;
    case FOLLOW_MODE:
      arm_follow_control(arm, data);
    break;
    case STORAGE_MODE:
    break;
    case TAKE_MODE:
    break;
    case POSITION_MODE:
    break;
  }
}

