/*
 * 伸缩模组
 */

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define TELESCOPING_OK (0)        /* 运行正常 */
#define TELESCOPING_ERR (-1)      /* 运行时发现了其他错误 */
#define TELESCOPING_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define TELESCOPING_ERR_MODE (-3) /* 运行时配置了错误的Telescoping_CMD_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
  TELESCOPING_MODE_RELAX,    /* 放松模式，电机不输出。 */
  TELESCOPING_MODE_CLOCKWISE, /*顺时针模式  */
  TELESCOPING_MODE_ANTI_CLOCKWISE, /*逆时针模式 */
} Telescoping_Mode_t;

typedef struct {
  Telescoping_Mode_t mode;
  float angle;
} Telescoping_CMD_t;

/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct {
	
	float circle;
	
	MOTOR_RM_Param_t telescoping_motor;
  struct {
		KPID_Params_t Telescoping_velocity;
    KPID_Params_t Telescoping_angle; 		
  } pid;

  /* 低通滤波器截止频率 */
  struct {
    float out;  /* 电机输出 */
  } low_pass_cutoff_freq;

} Telescoping_Params_t;



typedef struct {
  struct {
    MOTOR_Feedback_t telescoping; /*电机反馈 */
		float gearbox_total_angle;
  } motor;
} Telescoping_Feedback_t;

typedef struct {
  float telescoping;

} Telescoping_Output_t;

typedef struct {
  uint64_t lask_wakeup;
  float dt;

	
  Telescoping_Params_t *param; /* Init设定 */

  /* 模块通用 */
  Telescoping_Mode_t mode; /* 模式 */

  /* PID计算的目标值 */
  struct {
		float telescoping;
  } setpoint;

  struct {
		KPID_t telescoping_velocity; /* 电机速度环PID参数 */
    KPID_t telescoping_angle; 		/* 电机位置环PID参数 */
  } pid;

	
  struct {
    LowPassFilter2p_t telescoping;
  } filter_out;

  Telescoping_Output_t out;        /* 云台输出 */
  Telescoping_Feedback_t feedback; /* 反馈 */

} Telescoping_t;

int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
                   float target_freq);

int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping);

int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd);

void Telescoping_Output(Telescoping_t *t);

#ifdef __cplusplus
}
#endif