#include "up.h"
#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*接线


*/

#define GEAR_RATIO_2006         (36)   // 2006减速比
#define GEAR_RATIO_3508         (19)

#define CAN_MOTOR_ENC_RES  8191   // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_2006))   //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_3508))   //3508编码值转轴角度

#define CAN2_G3508_ID (0x200)


// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)

int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
	u->param = param; 	/*初始化参数 */

	/*pid初始化*/
	PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param  ));
	PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));

	PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param  ));
	PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
	
	PID_init (&u->pid .Pitch_Angle_M2006_speed  ,PID_POSITION ,&(u->param->Pitch_Angle_M2006_speed_param));
	
	PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param  ));
	PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
	
	u->go_cmd =u->param ->go_cmd ;
	LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);  


	if (!u->Pitch_Cfg  .is_init) { 
		  u->Pitch_Cfg .PitchConfig  = u->param ->PitchCfg  ;//赋值
      u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
      u->Pitch_Cfg .is_init = 1;
    }

     BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层

}

/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {

	

	u->motorfeedback .VESC.VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
	u->motorfeedback .VESC.VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;

	u->motorfeedback .Encoder .angle = can ->Oid.angle ;
	u->motorfeedback .Encoder .ecd   = can ->Oid.ecd ;

	for(int i=0;i<4;i++){  //范围0x201--0x204
	u->motorfeedback .DJmotor_feedback[i].id =i+CAN2_G3508_ID+1;	
	u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_dribble_motor3508 .as_array [i].rotor_speed ;
  u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_dribble_motor3508.as_array [i].rotor_ecd ;
	DJ_processdata(&u->motorfeedback .DJmotor_feedback [i],	MOTOR3508_ECD_TO_ANGLE);
	}	
	for(int i=0;i<2;i++){  //范围0x205--0x206
	u->motorfeedback .DJmotor_feedback[i+4].id =i+CAN2_G3508_ID+4+1;			
	u->motorfeedback .DJmotor_feedback[i+4].rpm =can ->motor .up_shoot_motor3508 .as_array [i].rotor_speed ;
  u->motorfeedback .DJmotor_feedback[i+4].ecd =can ->motor .up_shoot_motor3508.as_array [i].rotor_ecd ;
	DJ_processdata(&u->motorfeedback .DJmotor_feedback [i+4],	MOTOR2006_ECD_TO_ANGLE);
	}	

	u->cmd =c;

	return 0;
}

int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
	  int8_t cnt=0;
	  fp32 angle ,delta;
 			
    angle = f->ecd;  
    if (f->init_cnt < 50) {
		f->orig_angle= angle;    
		f->last_angle = angle;
		f->init_cnt++;            
     return 0;			
    }
		
	
	  delta = angle - f->last_angle;
    if (delta > 4096) {
        f->round_cnt--;           
    } else if (delta < -4096) {
        f->round_cnt++;          
    }
	    f->last_angle = angle;
		  f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
	
}
/*
  这里id范围为1-4，
*/
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle)
{
	fp32 delta_angle,delta_speed;
  delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
  delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
	
	u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed;
	return 0;
}
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed)
{
	
	fp32 delta_speed;
  delta_speed = PID_calc(Speed_pid , f->rpm , target_speed);	
	u->final_out .DJfinal_out [id-CAN2_G3508_ID-1]=delta_speed;
	return 0;
}
int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
{
	u->motor_target .VESC_5065_M1_rpm =speed;
	u->motor_target .VESC_5065_M2_rpm =speed;
	
	u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
  u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
	
	return 0;

}



/*go电机控制*/	 

int8_t GO_SendData(UP_t *u, float pos, float limit)
{ 
	static int is_calibration=0;
	static fp32 error=0; //误差量

	   u->motorfeedback .go_data = get_GO_measure_point() ;


    // 读取参数
    float tff = u->go_cmd.T;          // 前馈力矩
    float kp = u->go_cmd.K_P;         // 位置刚度
    float kd = u->go_cmd.K_W;         // 速度阻尼
    float q_desired = u->go_cmd.Pos;  // 期望位置（rad）
    float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
    float dq_desired = u->go_cmd.W;  // 期望角速度（rad/s）
    float dq_current = u->motorfeedback.go_data->W;  // 当前角速度（rad/s）

    // 计算输出力矩 tau
    float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);

	  /*限制最大输入来限制最大输出*/
	if (pos - q_current > limit) {
		u->go_cmd.Pos = q_current + limit; // 限制位置
	}else if (pos - q_current < -limit) {
		u->go_cmd.Pos = q_current - limit; // 限制位置
	}else {
		u->go_cmd.Pos = pos; // 允许位置
	}

    // 发送数据
    GO_M8010_send_data(&u->go_cmd);


    return 0;
}

int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
	//电机控制	，传进can
  DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
	                 &u->pid .Shoot_M2006angle ,
	                 &u->pid .Shoot_M2006speed ,
	                 u->motor_target .Shoot_M2006_angle );
	DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
	(PID_calc (&(u->pid .Pitch_M2006_angle ),
	u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
	);
  
	GO_SendData(u,
	u->motor_target .go_shoot,
	u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度

	
	for(int i=0;i<4;i++){
	out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
	}
	for(int i=0;i<2;i++){
	out ->motor_UP3508_shoot.as_array[i]=u->final_out.DJfinal_out [i+4] ;
	}	
	out ->chassis5065 .erpm [0]=  u->final_out .final_VESC_5065_M1out ;
	out ->chassis5065 .erpm [1]=  -u->final_out .final_VESC_5065_M2out ;
	
	return 0;

	
}



int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
	
	if(u ==NULL)  return 0;
		if(out ==NULL)  return 0;
		if(c ==NULL)  return 0;
	
	/*指针简化*/
	PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;	
	
	up_motor_target_t *target=&u->motor_target ;
	static int is_pitch=1;
	
	/*config值限位*/
		abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f);


    switch (c->CMD_CtrlType  )
	{
	 case RCcontrol:  //在手动模式下
		 	 
   switch (c-> CMD_mode ) 
			{
				
		case Normal :
         /*投篮*/
		if(is_pitch){
		target->go_shoot =pitch_cfg->go_init ;
		target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
		is_pitch=0;	
			} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
		target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
		u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球

				break;
			
		case Pitch :
				if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
				{
		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
				}

			 Pitch_Process(u,out);


				 break ;
		case UP_Adjust: //测试用
			  pitch_cfg ->pull_speed=5;
			  pitch_cfg ->go_release_pos += c->Vx ;
	      pitch_cfg->Pitch_angle += c->Vy/100;
	
		    target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
		    target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
			break ;
				
	
	default:
		break;
	}
	case AUTO:
		switch(c-> CMD_mode)
		{
			case 	AUTO_MID360_Pitch:
		      pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos);

			if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
				{
		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
				}
				/*根据距离实时计算所需pos*/			
        pitch_cfg ->go_release_pos=c->pos;


			 Pitch_Process(u,out);
			break ;
				
			case AUTO_MID360:
						target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;

						u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  
				
				break ;
			
		}
			
			
return 0;

	
}


}







int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{
	
	PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
	PitchState_t *state =&u->Pitch_Cfg .PitchState;
  up_motor_target_t *target=&u->motor_target ;
	
	switch(*state){
	
		case PITCH_START:
//			u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 
      cfg->pull_speed=10;
			target->go_shoot = cfg->go_pull_pos;
		
		if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
			target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
		  u->Pitch_Cfg .PitchConfig .pull_speed=6;
			*state=PITCH_PULL_TRIGGER;
}//更改标志位

			break ;
		case PITCH_PULL_TRIGGER:
			
		
			if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1)  //当2006的总角度小于1，可以认为已经勾上,误差为1
			{
         target->go_shoot=cfg->go_release_pos; 
				if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f))
			  {
				*state=PITCH_LAUNCHING;			
			  }
			}
			break ;
		
		case PITCH_LAUNCHING: //等待发射	
//			*state=PITCH_COMPLETE;	
		break ;
		case PITCH_COMPLETE:  //发射完成
			
		
	  break ;
		
	
	}
		
return 0;
	
}




