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

上层用到三个光电
1.PE9 发射用 输出高电平 LIGHTA
2.PE11 运球上3508 输出高电平 LIGHTB
3.PE13 三摩擦检测球 输出高电平LIGHTC


*/

#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 ));

	PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param  ));
	PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param  ));
	for(int i=0;i<3;i++){
	PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
	}
	
	u->go_cmd =u->param ->go_cmd ;
	LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);  

	
	// 初始化上层状态机
	if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
		  u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
      u->DribbleContext .DribbleState  = DRIBBLE_PREPARE;  
      u->DribbleContext .is_initialized = 1;
		
    }

	if (!u->PitchContext  .is_initialized) { 
		  u->PitchContext .PitchConfig  = u->param ->PitchConfig_Config  ;//赋值
      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
      u->PitchContext .is_initialized = 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;
	/*光电状态更新*/
	u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port  ,LIGHT_C_Pin);
	u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port  ,LIGHT_B_Pin);


	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; //误差量

//	GO_MotorData_t *GO_calibration;//校准前，原始数据
	   u->motorfeedback .go_data = get_GO_measure_point() ;
//	if(is_calibration==0)
//	{
//		is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
//		u->go_cmd.Pos = -50; //上电之后跑		
//	  error= GO_calibration->Pos ;
//	}	  
//    u->motorfeedback .go_data  = GO_calibration;
//	  u->motorfeedback .go_data ->Pos= error  ;
//    u->go_cmd.Pos = pos;

    // 读取参数
    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_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
	DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
	DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
   DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
	                 &u->pid .Dribble_translate_M3508_angle ,
	                 &u->pid .Dribble_translate_M3508_speed ,
	                 u->motor_target .Dribble_translate_M3508_angle );
  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_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
//	                 &u->pid .Pitch_M2006_angle ,
//	                 &u->pid .Pitch_M2006_speed ,
//	                 u->motor_target .Pitch_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,5 );

	
	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;

	
}


int a=0;
int b=0;


int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
	
	if(u ==NULL)  return 0;
	static int is_pitch=1;
	
	

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

		    /*运球*/
		
		RELAY1_TOGGLE(0);//关闭气缸
			for(int i=0;i<3;i++){
	    	u->motor_target.Dribble_M3508_speed[i]=0;
			}
    u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle;
	  u->DribbleContext .DribbleState  = DRIBBLE_PREPARE; //重置最初状态
			
			

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

			 Pitch_Process(u,out);
//				if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
//				{
//				     u->CoContext .CoState =CoTRANSLATE_OUT;
//				}		
//        Co_Process(u,out);

				 break ;
		case UP_Adjust: //测试用
			
				u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ;
	      u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
	
		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
		    u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
			break ;
		case Dribble:
		{

			if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
				u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;			
			}
			//光电状态更新
			 Dribble_Process(u,out);
		}break ;

						
	
	default:
		break;
	}
	case AUTO:
		switch(c-> CMD_mode)
		{
			case 	AUTO_MID360_Pitch:
		 u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);

			if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
				{
		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
				}
				/*根据距离实时计算所需pos*/
				
//       u->PitchContext .PitchConfig .go1_release_threshold=c->pos;
			 Pitch_Process(u,out);
			break ;
				
			case AUTO_MID360:
						u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;

						u->PitchContext .PitchState   = PITCH_PREPARE;  

			
			
				break ;
			
		}
			
			
return 0;

	
}


}











int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{

	switch(u->PitchContext .PitchState){
	
		case PITCH_START:
//					u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 

			u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
		
		if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
			u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
		
			u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
}//更改标志位

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









int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
  static int common_speed_flag=0;//是否共速
  static int common_speed_reverse_flag=0;//是否共速
	

	switch (u->DribbleContext.DribbleState) {

		case DRIBBLE_TRANSLATE:
		u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 

		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
		{
			u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
		}
			break;
		case DRIBBLE_PROCESS_DOWN:
			
			u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
			u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
			u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;

		  if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
				u->motorfeedback .DJmotor_feedback [1].rpm,
		   	u->motorfeedback .DJmotor_feedback [2].rpm,
			  u->motor_target.Dribble_M3508_speed [0],
			  u->motor_target.Dribble_M3508_speed [1],
				u->motor_target.Dribble_M3508_speed[2],	
			  50.0f,50)
			) {
   		  	RELAY1_TOGGLE(1);  //速度达到后打开气缸
				  common_speed_flag =1;
			}
			if(common_speed_flag){
				if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
				u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
				u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
				u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;

			  u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;

				}
				
			} 

			
			break;
		case DRIBBLE_PROCESS_UP:
				 common_speed_flag =0;

			  if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&			
			     (u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
			     (u->motorfeedback .DJmotor_feedback [2].rpm<-500) 
			   ){
		        common_speed_reverse_flag=1;
				  
				}
				 if(common_speed_reverse_flag){
					 if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
			     u->DribbleContext .DribbleState=DRIBBLE_DONE;
				   RELAY1_TOGGLE(0);  //关闭气缸
					 }
				 }
				 break ;
		case DRIBBLE_DONE:
			common_speed_reverse_flag=0;
			for(int i=0;i<3;i++){
			  u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
			}
		
        /*标志位清零*/
		    u->DribbleContext.DribbleConfig.light_ball_flag=0;
		    u->DribbleContext.DribbleConfig.light_3508_flag=0;
		
		
			break;
		default:
			// 处理未知状态
			break;
	}
    return 0;
}
//    CoPREPARE,            // 准备阶段
//	  CoTRANSLATE_OUT,//上方平移,去运球
//	  CoDRIBBLE,       //运球阶段
//	  CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
//	  CoPITCH,       //发射
//	  CoDONE,        
	  
int8_t Co_Process(UP_t *u, CAN_Output_t *out)
{
	switch(u->CoContext .CoState )
	{
		case CoPREPARE:
			
		break ;
		case CoTRANSLATE_OUT:
			u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 

		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
			{
				u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
				u->PitchContext .PitchState = PITCH_START;

				u->CoContext .CoState =CoDRIBBLE;
			}
		break;

		case CoDRIBBLE:
			 Dribble_Process(u,out);  //状态停在DRIBBLE_DONE
			 Pitch_Process(u,out);    //状态停在PITCH_PULL_TRIGGER
		//状态停在对应位置时,平移去给发射送球
		if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING)
		{
			u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;			
		}
		if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,1.0f))
		{
			u->motor_target.Dribble_M3508_speed[0]=1000;
			u->motor_target.Dribble_M3508_speed[1]=-1000;
			u->motor_target.Dribble_M3508_speed[2]=-1000;

			u->CoContext .CoState =CoTRANSLATE_IN;
		}
		
    break ;	
		case CoTRANSLATE_IN:
			
				if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
				u->motorfeedback .DJmotor_feedback [1].rpm,
		   	u->motorfeedback .DJmotor_feedback [2].rpm,
			  u->motor_target.Dribble_M3508_speed [0],
			  u->motor_target.Dribble_M3508_speed [1],
				u->motor_target.Dribble_M3508_speed[2],	
			  50.0f,100)
			) {
   		  	RELAY1_TOGGLE(1);  //速度达到后打开气缸,送给发射
			}
			if(u->DribbleContext .DribbleConfig.light_ball_flag == 0)
			{
			  u->motor_target.Dribble_M3508_speed[0]=0;
			  u->motor_target.Dribble_M3508_speed[1]=0;
			  u->motor_target.Dribble_M3508_speed[2]=0;	
				u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
			
			}
			
		  if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端
			{
				u->CoContext .CoState =CoPITCH;
			}
    break ;
			
    case CoPITCH:
			
		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; 

			
			break ;

	}
	
	
	


}