/*
    control Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/kinematics.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"
#include "component/pid.h"
#include "component/filter.h"
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
DH_parameter_t LINK[7] = {
{ .a = 0,     .alpha = pi / 2, .d = 0.0495, .theta = 0, .theta_offset = 0},
{ .a = 0.402, .alpha = 0,      .d = 0,      .theta = 0, .theta_offset = 0},   
{.a = 0,      .alpha = -pi/2,  .d = 0,      .theta = 0, .theta_offset = 0},   
{.a = -0.0729,.alpha = pi/2,   .d = 0.1037, .theta = 0, .theta_offset = 0},   
{.a = 0,      .alpha = -pi/2,  .d = 0,      .theta = 0, .theta_offset = pi/2},
{.a = 0,      .alpha = pi/2,   .d = 0.243,  .theta = 0, .theta_offset =pi/2},
{.a = 0.050,  .alpha = 0,      .d = 0,      .theta = 0, .theta_offset =pi/2}
};
mat_t T_mat[7];
float32_t mat_pdata[6*4*4];

robot_t hand;




KPID_Params_t pid_pram = {.k = 1,
                          .d = 0.2,
                          .i = 0,
                          .p = 15,
                          .i_limit = 12,
                          .d_cutoff_freq = 100,
                          .out_limit = 12,
                          .range = M_2PI};
KPID_t pid;
MOTOR_DM_t motor = {.param = {.can = BSP_CAN_1,
                              .can_id = 0x02,
                              .master_id = 0x12,
                              .module = MOTOR_DM_J4310,
                              .reverse = false}};
MOTOR_LZ_t lzmotor={
  .param ={.can = BSP_CAN_1,
    .motor_id=126,
    .host_id =128,
    .module = MOTOR_LZ_RSO3,
    .reverse=0,
    .mode = MOTOR_LZ_MODE_MOTION
  }
};
MOTOR_LZ_MotionParam_t control={
  .torque=1
};
MOTOR_MIT_Output_t motor_out;



MOTOR_Feedback_t fb;
float tp = 0;
LowPassFilter2p_t filter;

typedef struct {
  float alpha;  // 滤波系数
  float y_last; // 上一次输出
} LowPassFilter;

// 初始化
void LPF_Init(LowPassFilter *lpf, float dt, float tau) {
  lpf->alpha = dt / (tau + dt);
  lpf->y_last = 0.0f;
}

// 执行滤波
float LPF_Update(LowPassFilter *lpf, float x) {
  float y = lpf->alpha * x + (1.0f - lpf->alpha) * lpf->y_last;
  lpf->y_last = y;
  return y;
}
LowPassFilter ff;




MOTOR_LZ_t* a;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_control(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / CONTROL_FREQ;

  osDelay(CONTROL_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */

  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
    
    // MOTOR_LZ_UpdateAll();
    MOTOR_LZ_Update(&lzmotor.param);
    MOTOR_LZ_MotionControl(&lzmotor.param, &control);
    // MOTOR_DM_Update(&motor.param);
    //
    // MOTOR_DM_t *m = MOTOR_DM_GetMotor(&motor.param);
    // fb = m->motor.feedback;
    //
    // motor_out.angle = tp;
    // sp = PID_Calc(&pid, tp, fb.rotor_abs_angle, fb.rotor_speed, 0.001);
    // // sp = LowPassFilter2p_Apply(&filter, sp);
    // // sp = LPF_Update(&ff, sp);
    // motor_out.torque = sp;
    // MOTOR_DM_MITCtrl(&motor.param, &motor_out);
    
    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}
