/*
 * 伸缩云台模组
 模式自行选择：遥控器改变或自行初始化
 delta获取从遥控器自行获取
 */

/* Includes ----------------------------------------------------------------- */
#include "telescoping_gimal.h"
#include "gimbal.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include "device/motor_dm.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
float motor_add_angle(float current_angle){
	static int cirle;
	static float prev_angle;
	float delta = current_angle - prev_angle;
	prev_angle=current_angle;
	if(delta>M_PI){
		cirle-=1;
	}else if(delta<-M_PI){
			cirle+=1;
	}	
		current_angle+=cirle*M_2PI;

	return current_angle;
}


/**
 * \brief 设置伸缩模式
 *
 * \param c 包含云台数据的结构体
 * \param mode 要设置的模式
 *
 * \return 函数运行结果
 */
static int8_t Telescoping_SetMode(Telescoping_t *t, Telescoping_Mode_t mode) {
    if (t == NULL)
		return -1;
	  if (mode == t->mode)
    return TELESCOPING_OK;

  PID_Reset(&t->pid.telescoping_angle);
  PID_Reset(&t->pid.telescoping_velocity);

  LowPassFilter2p_Reset(&t->filter_out.telescoping, 0.0f);
		
  AHRS_ResetEulr(&(t->setpoint.telescoping)); /* 切换模式后重置设定值 */
	
	return 0;
}

/* Exported functions ------------------------------------------------------- */

/**
* \brief 初始化伸缩电机
 *
 * \param g 包含伸缩电机数据的结构体
 * \param param 包含伸缩电机参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
                   float target_freq) {
  if (t == NULL)
    return -1;
	t->param = param;
  t->mode = TELESCOPING_MODE_CLOCKWISE; /* 设置默认模式 */
	
  /* 初始化云台电机控制PID和LPF */
  PID_Init(&(t->pid.telescoping_angle), KPID_MODE_CALC_D, target_freq,
           &(t->param->pid.Telescoping_angle));
	
  PID_Init(&(t->pid.telescoping_velocity), KPID_MODE_CALC_D, target_freq,
           &(t->param->pid.Telescoping_velocity));	
	
	  LowPassFilter2p_Init(&t->filter_out.telescoping, target_freq,
                       t->param->low_pass_cutoff_freq.out);
	
	  BSP_CAN_Init();
		MOTOR_RM_Register(&(t->param->telescoping_motor));

  return 0;
}

/**
 * \brief 通过CAN设备更新云台反馈信息
 *
 * \param gimbal 云台
 * \param can CAN设备
 *
 * \return 函数运行结果
 */
int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping) {
  if (telescoping == NULL)
    return -1;
	
		MOTOR_RM_Update(&(telescoping->param->telescoping_motor));
		MOTOR_RM_t *telescoping_motor_yaw = MOTOR_RM_GetMotor(&(telescoping->param->telescoping_motor));
		telescoping->feedback.motor.telescoping = telescoping_motor_yaw->feedback;
		telescoping->feedback.motor.gearbox_total_angle=motor_add_angle(telescoping->feedback.motor.telescoping.rotor_abs_angle);
  return 0;
}


/**

 *
* \param t 包含伸缩电机数据的结构体
 * \param t_cmd 伸缩电机控制指令
 *
 * \return 函数运行结果
 */

int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd) {
  if (t == NULL || t_cmd == NULL) {
    return -1;
  }
  t->dt = (BSP_TIME_Get_us() - t->lask_wakeup) / 1000000.0f;
  t->lask_wakeup = BSP_TIME_Get_us();
//  Telescoping_SetMode(t, t_cmd->mode);
//		  switch (t->mode){
//			case TELESCOPING_MODE_RELAX:			
//				t->out.telescoping=0.0f;
//			break;
//			case TELESCOPING_MODE_CLOCKWISE:
//				/*顺时针*/
//				t->setpoint.telescoping=t->param->circle*M_2PI;
//			break;			
//			case TELESCOPING_MODE_ANTI_CLOCKWISE:
//				t->setpoint.telescoping=(-t->param->circle)*M_2PI;				
//				break;		
//		}
   /* 控制相关逻辑 */
	t->setpoint.telescoping=t->param->circle*M_2PI;
  float telescoping_t_set_point;
	
	telescoping_t_set_point = PID_Calc(&(t->pid.telescoping_angle), t->setpoint.telescoping,
                          t->feedback.motor.gearbox_total_angle, 0.0f, t->dt);
  t->out.telescoping = PID_Calc(&(t->pid.telescoping_velocity), telescoping_t_set_point,
                          t->feedback.motor.telescoping.rotor_speed, 0.f, t->dt);

		t->out.telescoping=(-t->out.telescoping);

	    /* 输出滤波 */
    t->out.telescoping = LowPassFilter2p_Apply(&t->filter_out.telescoping, t->out.telescoping);
}


void Telescoping_Output(Telescoping_t *t){

	MOTOR_RM_Ctrl(&t->param->telescoping_motor);
	MOTOR_RM_SetOutput(&t->param->telescoping_motor, t->out.telescoping);
}