#pragma once

#include "component/pid.h"
#ifdef __cplusplus
extern "C" {
#endif

#include "bsp/struct_typedef.h"
#include "device/device.h"
#include "device/motor_dm.h"
#include "module/cmd.h"

#define ASSEMBLE_OK (0)
#define ASSEMBLE_ERR (-1)
#define ASSEMBLE_ERR_NULL (-2)
#define ASSEMBLE_ERR_MODE (-3)
#define ASSEMBLE_ERR_TYPE (-4) 

typedef struct {
  float motor_4310_out[3];
} Assemble_out_t;

typedef struct {
  struct
  {
     MOTOR_DM_Param_t motor_4310_param[3];
  }motor_param;
  
  struct{
  KPID_Params_t assemble_4310_Angle_param[3];
  KPID_Params_t assemble_4310_Omega_param[3];
  }pid_param;

} Assemble_param_t;

typedef struct {
  uint32_t last_wakeup;
  float dt;

struct{
 fp32 motor_4310_setpoint[3];
}setpoint;

  struct {
    fp32 motor_4310_speed[3];
    fp32 motor_4310_angle[3];
  } feedback;

  struct {
    KPID_t assemble_4310_Angle_pid[3];
    KPID_t assemble_4310_Omega_pid[3];
  } pid;

  Assemble_param_t *param;
  Assemble_out_t assemble_out;
} Assemble_t;


int8_t Assemble_Init(Assemble_t *a, Assemble_param_t *param,
                     float target_freq);
int8_t Assemble_Update(Assemble_t *a);
int8_t Assemble_Calc(Assemble_t *a,Assemble_CMD_t cmd_assemble);
int8_t Assemble_SetOutput(Assemble_t *a);

#ifdef __cplusplus
}
#endif