/*
 * 云台模组
 模式自行选择：遥控器改变或自行初始化
 delta获取从遥控器自行获取
 example:
	Gimbal_t gimbal;
	Gimbal_IMU_t gimbal_imu;
	Gimbal_CMD_t gimbal_cmd;

	Gimbal_Init(&gimbal,&Config_GetRobotParam()->gimbal_param,GIMBAL_CTRL_FREQ);
 
	while(1){
	//这里写遥控器获取gimbal_cmd数据
		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0))
    Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
		
 		Gimbal_UpdateFeedback(&gimbal);
    Gimbal_Control(&gimbal,&gimbal_cmd);
    Gimbal_Output(&gimbal);
		}
 */

/* Includes ----------------------------------------------------------------- */
#include "gimbal.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_rm.h"

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */

/**
 * \brief 设置云台模式
 *
 * \param c 包含云台数据的结构体
 * \param mode 要设置的模式
 *
 * \return 函数运行结果
 */
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
  if (g == NULL)
    return -1;
  if (mode == g->mode)
    return GIMBAL_OK;

  PID_Reset(&g->pid.yaw_angle);
  PID_Reset(&g->pid.yaw_omega);
  PID_Reset(&g->pid.pit_angle);
  PID_Reset(&g->pid.pit_omega);
  LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
  LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);

//  MOTOR_DM_Enable(&(g->param->yaw_motor));
  
  AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
	
  g->setpoint.eulr.pit = g->feedback.imu.eulr.pit;
  g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
	g->setpoint.ecd.pit=g->feedback.motor.pit.rotor_abs_angle;
	g->setpoint.ecd.yaw=g->feedback.motor.yaw.rotor_abs_angle;
  g->mode = mode;
  return 0;
}

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

/**
 * \brief 初始化云台
 *
 * \param g 包含云台数据的结构体
 * \param param 包含云台参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
                   float target_freq) {
  if (g == NULL)
    return -1;

  g->param = param;
  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */

  /* 初始化云台电机控制PID和LPF */
  PID_Init(&(g->pid.yaw_angle), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.yaw_angle));
  PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.yaw_omega));
  PID_Init(&(g->pid.pit_angle), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.pit_angle));
  PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.pit_omega));
	
  PID_Init(&(g->pid.yaw_ecd_angle), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.yaw_ecd_angle));
  PID_Init(&(g->pid.yaw_velocity), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.yaw_velocity));
  PID_Init(&(g->pid.pit_ecd_angle), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.pit_ecd_angle));
  PID_Init(&(g->pid.pit_velocity), KPID_MODE_CALC_D, target_freq,
           &(g->param->pid.pit_velocity));
  LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
                       g->param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
                       g->param->low_pass_cutoff_freq.out);
//  g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
//  g->limit.yaw.min = g->param->mech_zero.yaw;
//  g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
//  g->limit.pit.min = g->param->mech_zero.pit;
	/*输出限位*/
	g->limit.set_pit.max=g->param->Set_Limit_t.pit_max;
	g->limit.set_pit.min=g->param->Set_Limit_t.pit_min;
	g->limit.set_yaw.max=g->param->Set_Limit_t.yaw_max;
	g->limit.set_yaw.min=g->param->Set_Limit_t.yaw_min;	
	g->limit.set_ecd_pit.max=g->param->Set_Limit_t.pit_ecd_max;
	g->limit.set_ecd_pit.min=g->param->Set_Limit_t.pit_ecd_min;
	g->limit.set_ecd_yaw.max=g->param->Set_Limit_t.yaw_ecd_max;
	g->limit.set_ecd_yaw.min=g->param->Set_Limit_t.yaw_ecd_min;
  BSP_CAN_Init();
  MOTOR_RM_Register(&(g->param->yaw_motor));
  MOTOR_RM_Register(&(g->param->pit_motor));
//  MOTOR_DM_Register(&(g->param->yaw_motor));
//  MOTOR_DM_Enable(&(g->param->yaw_motor));
  return 0;
}

/**
 * \brief 通过CAN设备更新云台反馈信息
 *
 * \param gimbal 云台
 * \param can CAN设备
 *
 * \return 函数运行结果
 */
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
  if (gimbal == NULL)
    return -1;

  /* 更新RM电机反馈数据（pitch轴） */
  MOTOR_RM_Update(&(gimbal->param->yaw_motor));
	MOTOR_RM_Update(&(gimbal->param->pit_motor));
  MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->yaw_motor));
	MOTOR_RM_t *rm_motor_pit = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
  if (rm_motor_pit||rm_motor_yaw != NULL) {
    gimbal->feedback.motor.yaw = rm_motor_yaw->feedback;
		gimbal->feedback.motor.pit = rm_motor_pit->feedback;
  }

  /* 更新DM电机反馈数据（yaw轴） */
//  MOTOR_DM_Update(&(gimbal->param->yaw_motor));
//  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
//  if (dm_motor != NULL) {
//    gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
//  }
  return 0;
}

int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){

  if (gimbal == NULL) {
    return -1;
  }
  gimbal->feedback.imu.gyro = imu->gyro;
  gimbal->feedback.imu.eulr = imu->eulr;
}

/**
 * \brief 运行云台控制逻辑
 *
 * \param g 包含云台数据的结构体
 * \param g_cmd 云台控制指令
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
  if (g == NULL || g_cmd == NULL) {
    return -1;
  }

  g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
  g->lask_wakeup = BSP_TIME_Get_us();
  Gimbal_SetMode(g, g_cmd->mode);

  /* 欧拉角控制相关逻辑 */
  float yaw_omega_set_point, pit_omega_set_point,delta_yaw;
	/* 电机角度控制相关逻辑 */
	float yaw_velocity_set_point, pit_velocity_set_point,delta_ecd_yaw;
	
  switch (g->mode) {
  case GIMBAL_MODE_RELAX:/*放松模式*/
    g->out.yaw = 0.0f;
    g->out.pit = 0.0f;
    break;
  case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
		delta_yaw = g_cmd->delta_yaw*g->dt;
		g->setpoint.eulr.yaw+=delta_yaw;	
	
		if (g->param->travel.yaw > 0){
	if(g->setpoint.eulr.yaw>g->limit.set_yaw.max)g->setpoint.eulr.yaw=g->limit.set_yaw.max;
	if(g->setpoint.eulr.yaw<g->limit.set_yaw.min)g->setpoint.eulr.yaw=g->limit.set_yaw.min;			
}
		else{
		CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
		/*限制在-3.14~3.14*/
		if (g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
		}
			
  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
  float delta_pit = g_cmd->delta_pit*g->dt;
	g->setpoint.eulr.pit+=delta_pit;
		
  if (g->param->travel.pit > 0){
  /* 限制控制命令 */
	if(g->setpoint.eulr.pit>g->limit.set_pit.max)g->setpoint.eulr.pit=g->limit.set_pit.max;
	if(g->setpoint.eulr.pit<g->limit.set_pit.min)g->setpoint.eulr.pit=g->limit.set_pit.min;		
	}
    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
                          g->feedback.imu.gyro.z, 0.f, g->dt);

		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
                          g->feedback.imu.gyro.x, 0.f, g->dt);
    break;
		case GIMBAL_MODE_RELATIVE:
			/*将它的角度控制再-3.14~3.14*/
		if(g->feedback.motor.yaw.rotor_abs_angle>M_2PI)
		g->feedback.motor.yaw.rotor_abs_angle-=M_2PI;
		g->setpoint.eulr.yaw = g->feedback.motor.yaw.rotor_abs_angle; 
		
    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
                          g->feedback.imu.gyro.z, 0.f, g->dt);

		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
                          g->feedback.imu.gyro.x, 0.f, g->dt);
		
		break;
	case GIMBAL_MODE_MOTOR:
		
  delta_ecd_yaw = g_cmd->delta_yaw*g->dt;
	g->setpoint.ecd.yaw+=delta_ecd_yaw;
	
	if(g->param->travel.yaw > 0){

	if(g->setpoint.ecd.yaw>g->limit.set_ecd_yaw.max)g->setpoint.ecd.pit=g->limit.set_ecd_yaw.max;
	if(g->setpoint.ecd.yaw<g->limit.set_ecd_yaw.min)g->setpoint.ecd.pit=g->limit.set_ecd_yaw.min;	
	}
	else{
	/*限制yaw在0~6.28*/
	CircleAdd(&(g->setpoint.ecd.yaw), delta_ecd_yaw, M_2PI);
	}
	
  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
  float delta_ecd_pit = g_cmd->delta_pit*g->dt;
	g->setpoint.ecd.pit+=delta_pit;
	
  if (g->param->travel.pit > 0) {
		/* 限制控制命令 */
	if(g->setpoint.ecd.pit>g->limit.set_ecd_pit.max)g->setpoint.ecd.pit=g->limit.set_ecd_pit.max;
	if(g->setpoint.ecd.pit<g->limit.set_ecd_pit.min)g->setpoint.ecd.pit=g->limit.set_ecd_pit.min;	
  }
	 
    yaw_velocity_set_point = PID_Calc(&(g->pid.yaw_ecd_angle), g->setpoint.ecd.yaw,
                                   g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
    g->out.yaw = PID_Calc(&(g->pid.yaw_velocity), yaw_velocity_set_point,
                          g->feedback.motor.yaw.rotor_speed, 0.f, g->dt);

		pit_velocity_set_point = PID_Calc(&(g->pid.pit_ecd_angle), g->setpoint.ecd.pit,
                                   g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
    g->out.pit = PID_Calc(&(g->pid.pit_velocity), pit_velocity_set_point,
                          g->feedback.motor.pit.rotor_speed, 0.f, g->dt);	
		break;
    /* 输出滤波 */
    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);

    return 0;
  }
}

  /**
   * \brief 云台输出
   *
   * \param s 包含云台数据的结构体
   * \param out CAN设备云台输出结构体
   */
void Gimbal_Output(Gimbal_t *g){

    MOTOR_RM_Ctrl(&g->param->pit_motor);
		MOTOR_RM_Ctrl(&g->param->yaw_motor);
		MOTOR_RM_SetOutput(&g->param->yaw_motor, g->out.yaw);
	  MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);//
  }