/*
此module是负责武林秘籍组装代码模块
*/

/* Includes ----------------------------------------------------------------- */
#include "module/assemble.h"
#include "bsp/can.h"
#include "component/pid.h"
#include "device/motor.h"
#include <stdint.h>
#include "bsp/time.h"


/* Private typedef ---------------------------------------------------------- */

/* Private define ----------------------------------------------------------- */

/* Private macro ------------------------------------------------------------ */

/* Private variables -------------------------------------------------------- */

/* Private function prototypes ---------------------------------------------- */

/*Export function
 * --------------------------------------------------------------*/

int8_t Assemble_Init(Assemble_t *a, Assemble_param_t *param,float target_freq) {
  if (a == NULL || param == NULL) {
    return ASSEMBLE_ERR; // 参数错误
  }

  a->param = param;
  /*CAN初始化*/
  BSP_CAN_Init();
  /*注册电机*/
  for (int i = 0; i < 3; i++) {
    MOTOR_DM_Register(&a->param->motor_param.motor_4310_param[i]);
  }
  /*初始化pid*/
  for (int i = 0; i < 3; i++) {
    PID_Init(&a->pid.assemble_4310_Angle_pid[i], KPID_MODE_CALC_D, 
    target_freq,&a->param->pid_param.assemble_4310_Angle_param[i]);
    PID_Init(&a->pid.assemble_4310_Omega_pid[i], KPID_MODE_CALC_D,
    target_freq,&a->param->pid_param.assemble_4310_Omega_param[i]);
  }
  for (int i = 0; i < 3; i++) {
    MOTOR_DM_Enable(&a->param->motor_param.motor_4310_param[i]);
  }
  return ASSEMBLE_OK;
}

int8_t Assemble_Update(Assemble_t *a) {
  if (a == NULL) {
    return ASSEMBLE_ERR; // 参数错误
  }

  /*更新所有电机数据*/
  MOTOR_DM_UpdateAll();

  /*更新电机反馈*/
  MOTOR_DM_t *motor_4310_1 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[0]);
  if (motor_4310_1 != NULL) {
    a->feedback.motor_4310_angle[0] =
        MOTOR_GetRotorAbsAngle(&motor_4310_1->motor);
    a->feedback.motor_4310_speed[0] = MOTOR_GetRotorSpeed(&motor_4310_1->motor);
  }

  MOTOR_DM_t *motor_4310_2 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[1]);
  if (motor_4310_2 != NULL) {
    a->feedback.motor_4310_angle[1] =
        MOTOR_GetRotorAbsAngle(&motor_4310_2->motor);
    a->feedback.motor_4310_speed[1] = MOTOR_GetRotorSpeed(&motor_4310_2->motor);
  }

  MOTOR_DM_t *motor_4310_3 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[2]);
  if (motor_4310_3 != NULL) {
    a->feedback.motor_4310_angle[2] =
        MOTOR_GetRotorAbsAngle(&motor_4310_3->motor);
    a->feedback.motor_4310_speed[2] = MOTOR_GetRotorSpeed(&motor_4310_3->motor);
  }

  return ASSEMBLE_OK;
}

int8_t Assemble_Calc(Assemble_t *a,Assemble_CMD_t cmd_assemble) 
{
  if (a == NULL) {
    return ASSEMBLE_ERR; // 参数错误
  }

a->dt = (BSP_TIME_Get_us() - a->last_wakeup) / 1000000.0f;
  a->last_wakeup = BSP_TIME_Get_us();

    a->setpoint.motor_4310_setpoint[0] = cmd_assemble.setpoint.motor_4310_setpoint[0];
    a->setpoint.motor_4310_setpoint[1] = cmd_assemble.setpoint.motor_4310_setpoint[1];
    a->setpoint.motor_4310_setpoint[2] = cmd_assemble.setpoint.motor_4310_setpoint[2];

  float angle_output_1 = PID_Calc(&a->pid.assemble_4310_Angle_pid[0],
                                  a->setpoint.motor_4310_setpoint[0],
                                  a->feedback.motor_4310_angle[0], 0.0f, a->dt);
  a->assemble_out.motor_4310_out[0] =
      PID_Calc(&a->pid.assemble_4310_Omega_pid[0], angle_output_1,
               a->feedback.motor_4310_speed[0], 0.0f, a->dt);

  float angle_output_2 = PID_Calc(&a->pid.assemble_4310_Angle_pid[1],
                                  a->setpoint.motor_4310_setpoint[1],
                                  a->feedback.motor_4310_angle[1], 0.0f, a->dt);
  a->assemble_out.motor_4310_out[1] =
      PID_Calc(&a->pid.assemble_4310_Omega_pid[1], angle_output_2,
               a->feedback.motor_4310_speed[1], 0.0f, a->dt);

  float angle_output_3 = PID_Calc(&a->pid.assemble_4310_Angle_pid[2],
                                  a->setpoint.motor_4310_setpoint[2],
                                  a->feedback.motor_4310_angle[2], 0.0f, a->dt);
  a->assemble_out.motor_4310_out[2] =
      PID_Calc(&a->pid.assemble_4310_Omega_pid[2], angle_output_3,
               a->feedback.motor_4310_speed[2], 0.0f, a->dt);

  return ASSEMBLE_OK;
}

int8_t Assemble_SetOutput(Assemble_t *a) {
  MOTOR_MIT_Output_t MOTOR_4310_output_1 = {0};
  MOTOR_4310_output_1.torque = a->assemble_out.motor_4310_out[0] * 3.0f;
  MOTOR_4310_output_1.kd=0.1f;

MOTOR_MIT_Output_t MOTOR_4310_output_2 = {0};
  MOTOR_4310_output_2.torque = a->assemble_out.motor_4310_out[1] * 9.0f;
  MOTOR_4310_output_2.kd=0.1f;

  MOTOR_MIT_Output_t MOTOR_4310_output_3 = {0};
  MOTOR_4310_output_3.torque = a->assemble_out.motor_4310_out[2] * 3.0f;
  MOTOR_4310_output_3.kd=0.1f;

    MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[0], &MOTOR_4310_output_1);
    MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[1], &MOTOR_4310_output_2);
    MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[2], &MOTOR_4310_output_3);

  return ASSEMBLE_OK;
}