/*
    ctrl_gimbal Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "device/motor_rm.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
#include "device/motor.h"
#include "component/pid.h"
#include <stdbool.h>

/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_DM_Param_t dm_4310_param = {
    .can = BSP_CAN_1,
    .master_id = 0x11,    // 主站ID
    .can_id = 0x01,       // 反馈ID 10进制是85
    .module = MOTOR_DM_J4310,
    .reverse = false,
};

// MOTOR_RM_Param_t m2006 = {
//     .can = BSP_CAN_1,
//     .id = 0x201,
//     .module = MOTOR_M2006,
//     .reverse = false,
//     .gear = true,
// };

MOTOR_MIT_Output_t gimbal_output = {
    .angle = 0.0f,
    .velocity = 0.0f,
    .torque = 0.0f,
    .kp = 0.0f,
    .kd = 0.0f,
};
MOTOR_DM_t *dm_4310_motor = NULL;

// MOTOR_Feedback_t m2006_fb;
MOTOR_Feedback_t dm_4310_fb;

// KPID_Params_t gimbal_angle_pid = {
//   .leg_theta={
//       .k=1.0f,
//       .p=5.0f,           /* 摆角比例系数 */
//       .i=0.0f,           /* 摆角积分系数 */
//       .d=0.0f,           /* 摆角微分系数 */
//       .i_limit=0.0f,     /* 积分限幅 */
//       .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */
//       .d_cutoff_freq=30.0f, /* 微分截止频率 */
//       .range=-1.0f,      /* 不使用循环误差处理 */
// };
/* USER STRUCT END */

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

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

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

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

  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
    // MOTOR_DM_Update(&dm_4310_param);
    // MOTOR_DM_t *dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
    // if (dm_4310_motor) {
    //     dm_4310_fb = dm_4310_motor->motor.feedback;
    // }
    // if (dm_4310_motor->motor.header.online == false) {
    //   MOTOR_DM_Enable(&dm_4310_param); // 使能电机
    // } else {
    //   MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
    // }
    

    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}