#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->PitchContext  .is_init) { 
		  u->PitchContext .PitchConfig  = u->param ->PitchCfg  ;//赋值
      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新
      u->PitchContext .is_init = 1;
    }
   if(!u->PassContext.is_init)  {
		  u->PassContext .PassCfg  = u->param ->PassCfg  ;//赋值
      u->PassContext .PassState   = PASS_STOP;  //状态更新
      u->PassContext .is_init = 1;		  
	 }
	 u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
	 u->MID360Context.MID360Cfg = u->param->MID360Cfg;
	 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;
}


/*
  这里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;

}



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 );
	
	/*俯仰2006，双环，内使用oid角度环，外电机速度环
	  
	*/
	
	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 .Pitch_angle)+0.14)
	);

	GO_SendData(
	&u->motorfeedback.go_data,
	&u->go_cmd,u->motor_target .go_shoot,
	u->motor_target.go_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;

	
}


fp32 posss=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->PitchContext.PitchConfig;	
	LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
	up_motor_target_t *target=&u->motor_target ;
		
	/*config值限位*/
		abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
		abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);

	/*部分数据更新*/
	static int is_pitch=1;
	posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
//	if (u->PitchContext.Curve == CURVE_58) {
//    target->Pitch_angle = 58;
//    } else {
//    target->Pitch_angle = 66;
//    }
		u->vofa_send [2]  = c->pos;
		u->vofa_send [3]  =  LowPassFilter2p_Apply(&u->filled[0],c->pos);
		u->vofa_send [4]   =1;
  
    switch (c->CMD_CtrlType  )
	{
	 case RCcontrol:  //在手动模式下

 	 
   switch (c-> CMD_mode ) 
			{
				
		 case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
         /*投篮*/
		if(is_pitch){
		target->go_shoot = LaunchCfg->go_init ;
		target->Pitch_angle  =  LaunchCfg->Pitch_angle;		
		is_pitch=0;	
		} //初始上电
		//LaunchCfg->go_up_speed=15;
		target->Pitch_angle =LaunchCfg->Pitch_angle; 
		target->go_pull_speed=LaunchCfg->go_up_speed;	
		target->Shoot_M2006_angle=LaunchCfg->m2006_init;
		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新
    u->LaunchContext.LaunchState  = Launch_Stop;
				break;
			
		case Pitch :
				if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
				{
		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
				}

			 Pitch_Process(u,out,c);


				 break ;
		case UP_Adjust: //测试用,手动使用pitch下的cfg
			  LaunchCfg->go_up_speed=5;
			  pitch_cfg ->go_release_pos += c->Vx ;
	      LaunchCfg->Pitch_angle += c->Vy/100;
	
		    target->go_shoot=pitch_cfg ->go_release_pos;
		    target->Pitch_angle=LaunchCfg->Pitch_angle;
			break ;
				default:
			break;
	}
			break;
	case AUTO:
        UP_AUTO_Control(u,out,c);		
		break ;	
      case PASS_BALL:
				Pass_Process(u,out,c);
				break;
			case RELAXED:
//			target->go_shoot=	pitch_cfg .
			target->Pitch_angle = 58;
			target->go_shoot = -190.0f;
//			target->Pitch_angle = 58;
			
			
			break ;
	
}

return 0;
}




//复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
	
	switch(LaunchContext->LaunchState){
		case Launch_Stop: break;
		case Launch_PREPARE:
			u->motor_target.go_shoot = StartPos;
		 if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&& 
			 is_reached(u->motorfeedback.go_data.W,0,1.0f)){
				 //根据位置和速度判断是否到达初始位置
			 LaunchContext->LaunchState = Launch_START;
				
			 }break;
			 
		case Launch_START:
			u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
		  u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
			if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
			u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
		    
			LaunchContext->LaunchState = Launch_TRIGGER;
			}break;
		
		case Launch_TRIGGER:
			
		if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){  //当2006的总角度小于1，可以认为已经勾上,误差为1
      u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
		  u->motor_target.go_shoot = EndPos ;
		  if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
			LaunchContext->LaunchState = Launch_SHOOT_WAIT;
		}	break;
		
		case Launch_SHOOT_WAIT:
			if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
				LaunchContext->LaunchState = Launch_DONE;
			break;
		case Launch_DONE:
			break ;
	}
	
}



int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
	/*简化*/
	PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
	PitchState_t *state =&u->PitchContext .PitchState;
  up_motor_target_t *target=&u->motor_target ;
  LaunchContext_t *LaunchContext = &u->LaunchContext;
	// 可根据实际需要传入不同的起始和末位置,起始：当前位置
  Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);

    
	switch(*state){
		
		case PITCH_START:
			   LaunchContext->LaunchState = Launch_PREPARE;
		
		    *state=PITCH_WAIT;
		break;
		
		case PITCH_WAIT:
			 
    if(LaunchContext->LaunchState==Launch_DONE)
         *state=PITCH_COMPLETE;
		break;
		case PITCH_COMPLETE:
			
			break;
		default:
			break;
		
	}
  
	
    return 0;
	
}



int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
{

    PassCfg_t *PassCfg = &u->PassContext.PassCfg;
	  PassState_t *state = &u->PassContext.PassState;
	  up_motor_target_t *target=&u->motor_target ;
    LaunchContext_t *LaunchContext = &u->LaunchContext;
	
	  Pass_Sequence_Check(u,c);
	  
	if(c->pos) //
	{
	  PassCfg ->go_release_pos =
		CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
		
	}
    switch (*state) { //遥控器按键进行状态切换
			  case PASS_STOP:
					
				break;
				case PASS_IDLE:				 
				LaunchContext->LaunchState = Launch_PREPARE;
				break;
        case PASS_PREPARE:
				 target->go_pull_speed=PassCfg->go_up_speed;
				 
         Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
				  
				  
        break;
        case PASS_START:
					if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
 				 target->go_pull_speed=PassCfg->go_down_speed;
				 target->go_shoot = PassCfg->go_release_pos ;
					}
        break ;
				case PASS_POS_PREPARE:
         target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射   					
					break;
        case PASS_COMPLETE:
            break;
        default:
            break;
    }
    return 0;
}

int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
		/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
		  up_motor_target_t *target=&u->motor_target ;	
	    LaunchContext_t *LaunchContext = &u->LaunchContext;
	    MID360Context_t *MID360Context=&u->MID360Context;
	    MID360Cfg_t  *MID360Cfg = &u->MID360Context.MID360Cfg;
	    MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3.2,4.3,&u->MID360Context.Curve);
			if (u->MID360Context.Curve == CURVE_58) {
        target->Pitch_angle = 58;
				} else {
				target->Pitch_angle = 66;
				}
			
       LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
       LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
				
		
		switch(c-> CMD_mode)
		{
			
			case 	AUTO_MID360_Pitch:
				if(MID360Context->IsLaunch==0){
				MID360Context->IsLaunch=1;
				LaunchContext->LaunchState = Launch_PREPARE;
				}
			  Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
			
			break ;
				
			case AUTO_MID360:
					target->Shoot_M2006_angle=	LaunchContext->LaunchCfg.m2006_init;
				MID360Context->IsLaunch=0;
				break ;
					default:
			break;
			
		}
		
	}