#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; 	/*初始化参数 */

	/*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_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 ;
	
	// 初始化上层状态机
	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++){
	u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_motor3508 .as_array [i].rotor_speed ;
	u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ;
	DJ_processdata(&u->motorfeedback .DJmotor_feedback [i],	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-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_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
	                 &u->pid .Shoot_M2006angle ,
	                 &u->pid .Shoot_M2006speed ,
	                 u->motor_target .Shoot_M2006_angle );
	DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
	                 &u->pid .Pitch_M2006_angle ,
	                 &u->pid .Pitch_M2006_speed ,
	                 u->motor_target .Pitch_M2006_angle );
	GO_SendData(u,u->motor_target .go_shoot,5 );

	
	for(int i=0;i<4;i++){
	out ->motor_UP3508.as_array[i]=u->final_out.DJfinal_out [i] ;
	}
	
	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;
	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 ;
		is_pitch=0;	
		}
		u->motor_target .Shoot_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 UP_Adjust:
				u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
	      u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;

		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
		    u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
			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:
			break ;
//        Dribbl_transfer_a(u,out);
			}
			break;
						
	
	default:
		break;
	}

			
			
return 0;

	
}














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

//		go1_position=u->PitchContext .PitchConfig .go1_init_position ;
//		M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;


	switch(u->PitchContext .PitchState){
	
		case PITCH_START:
			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 [0].total_angle  >-1)    //当2006的总角度小于1，可以认为已经勾上,误差为1
			{
         u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; 
//				 u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
//	       u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
	
			}
		
			break ;

		
	
	
	}
		
return 0;
	
}





int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
	
	u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
	u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
			
}
























//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;

//		}	
//}