#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 RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (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; 	/*初始化参数 */
	/*go电机初始化*/
	GO_M8010_init();
	/*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 .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param  ));
	PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
	
	PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param  ));
	PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
	
	PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
	PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
		
  PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
//	for(int i=0;i<2;i++){		 //go初始位置设置为0 
//	    GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );		
//	}

	
	// 初始化上层状态机
	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;
    }


}

/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
		for(int i=0;i<2;i++){		     //go初始位置设置为0 
	  u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
	}
	
	u->motorfeedback .M2006 .motor =M2006;
	u->motorfeedback .M3508 .motor =M3508;

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

	u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
	u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed  ;
	
	for(int i=0;i<4;i++){
	u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
	u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
	}
	
	u->cmd =c;
	
	return 0;
}



/*上层大疆电机角度控制,使用can1的id1和2*/
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
    // 获取当前编码器角度
	int8_t cnt=0;
	fp32 angle ,delta;

	switch(motor)
	{
		case M2006 :  			
    angle = u->motorfeedback.M3508_angle[0]; 
    if (u->motorfeedback .M2006 .init_cnt < 50) {
        u->motorfeedback .M2006.orig_angle = angle;    // 记录初始编码器值
        u->motorfeedback .M2006.last_angle = angle;    
			
        u->motorfeedback .M2006.init_cnt++;            // 初始化计数器递增
        return 0;                       
    }
		
		    delta = angle - u->motorfeedback .M2006.last_angle;
	    	delta = angle - u->motorfeedback .M2006.last_angle;
    if (delta > 4096) {
        u->motorfeedback .M2006.round_cnt--;           // 逆时针跨圈
    } else if (delta < -4096) {
        u->motorfeedback .M2006.round_cnt++;           // 顺时针跨圈
    }
        u->motorfeedback.M2006.last_angle = angle;
		    // 计算总角度
    float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
    u->motorfeedback.M2006.total_angle =total_angle;
    
    float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
    float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
    u->motor_target.M2006_angle = target_angle;
    u->final_out .final_3508out [0] =delta_speed;

		break ;
		
		case M3508 : 
			
    angle = u->motorfeedback.M3508_angle[1]; 
    if (u->motorfeedback .M3508 .init_cnt < 50) {
        u->motorfeedback .M3508.orig_angle = angle;    // 记录初始编码器值
        u->motorfeedback .M3508.last_angle = angle;    
			
        u->motorfeedback .M3508.init_cnt++;            // 初始化计数器递增
        return 0;                       
    }
		
		    delta = angle - u->motorfeedback .M3508.last_angle;
	    	delta = angle - u->motorfeedback .M3508.last_angle;
    if (delta > 4096) {
        u->motorfeedback .M3508.round_cnt--;           // 逆时针跨圈
    } else if (delta < -4096) {
        u->motorfeedback .M3508.round_cnt++;           // 顺时针跨圈
    }
        u->motorfeedback.M3508.last_angle = angle;
		    // 计算总角度
     total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; 
    u->motorfeedback.M3508.total_angle =total_angle;
    
     delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
     delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
		
    u->motor_target.M3508_angle  = target_angle;
    u->final_out .final_3508out[1] =delta_speed;

		break ;  

	}
    return 0;
}

//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
//{
//	u->motor_target .M3508_speed [] =speed;
//	for(int i=0;i<3;i++){
//	    u->final_out .final_3508out [i] =
//	        PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );	
//	}
//	
//}


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 GM6020_control(UP_t *u,fp32 angle)
{	
	fp32 delat_speed;
//	Clip(&angle,90,270);

	delat_speed =
	PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
	u->final_out .final_pitchout =
	PID_calc (&(u->pid .GM6020_speed  ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
	u->motor_target .rotor_pit6020angle =angle ;
	return 0;

}

/*go电机控制*/
int8_t GO_SendData(UP_t *u,int id,float pos)
{
	 fp32 deal_pos;
	 
//	 deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,pos);
	 GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id].T  ,u->param->go_param[id] .W ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
   return 0;

}



int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
	//电机控制	，传进can
	
	UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
	UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
	
	

	GO_SendData(u,0 ,u->motor_target .go_shoot );

	
	for(int i=0;i<4;i++){
	out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
	}
	
	out ->chassis5065 .erpm [0]=  u->final_out .final_VESC_5065_M1out ;
	out ->chassis5065 .erpm [1]=  -u->final_out .final_VESC_5065_M2out ;
	out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
	
	return 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 UP_RCcontrol:  //在手动模式下
		
   switch (c-> CMD_UP_mode ) 
			{
				
		case Normal :
         /*投篮*/
		if(is_pitch){
		u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
		is_pitch=0;	
		}
		u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球

//		    /*运球*/
//		u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
//		u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
//	  u->DribbleContext .DribbleState  = Dribble_PREPARE; //重置最初状态


//      	RELAY1_TOGGLE (1);//夹球，0夹1开
//      	RELAY2_TOGGLE (0);//球，0接1收
//    				
				
				break;
				
		case Pitch :
				if (u->PitchContext .PitchState  ==PITCH_PREPARE) //首次启动
				{
		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
				}

			 Pitch_Process(u,out,c);

				 break ;
		 
		case Dribble:
		{
			/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收		*/

			if(u->DribbleContext.DribbleState== Dribble_PREPARE){
				
				u->DribbleContext .DribbleState =STATE_GRAB_BALL;
			}
			 Dribble_Process(u,out);


		}break ;
		case Dribbl_transfer:
			
        Dribbl_transfer_a(u,out);
			}
			break;
						
	
	default:
		break;
	}

			
			
return 0;

	
}














int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
	static fp32 go1_position,M2006_Screw_position ;
	static int is_initialize=1;
	if(is_initialize)
	{
		go1_position=u->PitchContext .PitchConfig .go1_init_position ;
		M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
		is_initialize=0;
	}

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

			break ;
		case PITCH_PULL_TRIGGER:
			
		
			if(u->motorfeedback .M2006 .total_angle >-1)    //当2006的总角度小于1，可以认为已经勾上,误差为1
			{
				u->motor_target .go_shoot = go1_position;//go下拉
		    u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006
				go1_position = go1_position + c->Vx ;
		    M2006_Screw_position=M2006_Screw_position+ c->Vy;
		
				
			}
		
			break ;

		
	
	
	}
		
return 0;
	
}





























int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
    
    switch (u->DribbleContext.DribbleState) {
    case STATE_GRAB_BALL://开始
			
			RELAY1_TOGGLE (0);//夹球
		
			u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
			u->motor_target.go_spin     =u->DribbleContext .DribbleConfig .go2_flip_angle;

		
		if((u->motorfeedback .M3508 .total_angle >400)) {
				RELAY2_TOGGLE (1); 
		

		if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
			
				u->DribbleContext .DribbleState  = STATE_RELEASE_BALL;  //当go2到标准位置，标志位改变		
			}
		
		}
      break;


    case STATE_RELEASE_BALL:
				RELAY1_TOGGLE (1);//松球

		
		
		
		if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
				u->motor_target.go_spin     =-40; //恢复go2位置	
      	u->DribbleContext .DribbleState  = STATE_CATCH_PREP;  //当go2到标准位置，标志位改变		

			}

        break;

    case STATE_CATCH_PREP:
			if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
				u->motor_target.M3508_angle =0 ;  //当go2到初始位置，3508降       
		
			  RELAY2_TOGGLE (0);//接球
			}
			if(u->motorfeedback .M3508 .total_angle <51.0f){
				      	RELAY1_TOGGLE (0);//夹球，0夹1开		

			  u->DribbleContext .DribbleState  = STATE_TRANSFER;
			}
			
        break;
			

		
			
		break ;



    default:
        break;
    }

    return 0;
}

void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
{
    switch (u->DribbleContext.DribbleState) {
			
			
			
			case STATE_TRANSFER:
			if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
			{
				
        u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;			
			}
			
			if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9) 
			{
	      	RELAY1_TOGGLE (1);//夹球，0夹1开		
				
				if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
				{
	      	u->DribbleContext .DribbleState  = STATE_CATCH_DONE;  //当go2到标准位置，标志位改变		
				}
			}
		
			
		break ;

    case STATE_CATCH_DONE:
			
		    RELAY1_TOGGLE (1);//夹球，0夹1开
      	RELAY2_TOGGLE (0);//夹球，0接1收

		    	u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;

        break;

        break;

		}	
}