#include "Chassis.h"

/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处，进debug将四个轮子编码器朝左查看6020反馈值，
将6020反馈值放入motor_offset中*/
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){

	 c->mode =ctrl->C_cmd.mode;
   c->pos =ctrl->C_cmd.pos;	   
	 return 0;
}

//底盘解算   
void Chassis_speed_calculate(Chassis_t *c,Action_POS_t*pos) {
	 
	// RC模式下松开遥控器防止6020回到默认位置导致侧翻
  if (c->mode == RC&&fabs(c->move_vec.Vx) < 3000 && fabs(c->move_vec.Vy) < 3000 && fabs(c->move_vec.Vw) < 3000) {
    // 如果之前不处于保持模式，则记录当前角度
    if (!c->keeping_angle_flag) {
       for (uint8_t i = 0; i < 4; i++) {
         c->keep_angle[i] = c->motorfeedback.rotor_angle6020[i];
       }
       c->keeping_angle_flag = 1;  // 进入保持模式
    }
        
    // 使用保持的角度，而不是实时反馈值
    for (uint8_t i = 0; i < 4; i++) {
      c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
    }
        
    // 速度设为 0
    for (uint8_t i = 0; i < 4; i++) {
    c->hopemotorout.rotor5065_jiesuan_1[i] = 0;
    }
  } else {
      // 如果有速度输入，则退出保持模式
      c->keeping_angle_flag = 0;
      switch(c->mode){	
				case RC: //RC模式下遥控器控制底盘运动，操控为机器人坐标系  x朝前，y朝右
				  //解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
					c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
					(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
  
					c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
					(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
 
					c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
					(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));	
 
					c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
					(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
	
					//解算得到6020角度（-180°——+180°）
					c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
	     break;
	     case STOP: //停止模式下轮子锁死
		     for(int i =0 ; i < 4 ; i++){
			   c->hopemotorout.rotor5065_jiesuan_1[i]=0;	
		     }
			
		     c->hopemotorout.rotor6020_jiesuan_1[0]=315;
		     c->hopemotorout.rotor6020_jiesuan_1[1]=45;
		     c->hopemotorout.rotor6020_jiesuan_1[2]=45;
		     c->hopemotorout.rotor6020_jiesuan_1[3]=315;   
		   break;
	     case NAVI: //导航模式下操控为世界坐标系，能实现舵轮小陀螺前进
				 
//			   c->chassis_yaw = pos->pos_yaw* (M_PI / 180.0f);
//			   float cos_yaw = cosf(c->chassis_yaw);
//         float sin_yaw = sinf(c->chassis_yaw);
//			 
//			   // 将速度从世界坐标系转换到底盘坐标系
//         float Vx_local = c->move_vec.Vx * cos_yaw + c->move_vec.Vy * sin_yaw;
//         float Vy_local = -c->move_vec.Vx * sin_yaw + c->move_vec.Vy * cos_yaw;
// 
//			   //解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
//	       c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
//	       (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
//         (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
//  
//				 c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
//	       (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
//         (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
// 
//	       c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
//	       (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
//         (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
// 
//         c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
//	       (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
//         (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
//	
//		     //解算得到6020角度（-180°——+180°）
//         c->hopemotorout.rotor6020_jiesuan_1[0]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
//																			 (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
//         c->hopemotorout.rotor6020_jiesuan_1[1]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
//				                               (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
//         c->hopemotorout.rotor6020_jiesuan_1[2]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
//				                               (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
// 	       c->hopemotorout.rotor6020_jiesuan_1[3]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
//				                               (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);

           //相机纠正时就用正常解算
	         c->hopemotorout.rotor5065_jiesuan_1[0]=sqrt(
					(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
  
					c->hopemotorout.rotor5065_jiesuan_1[1]=-sqrt(
					(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
 
					c->hopemotorout.rotor5065_jiesuan_1[2]=sqrt(
					(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));	
 
					c->hopemotorout.rotor5065_jiesuan_1[3]=-sqrt(
					(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
					(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
	
					//解算得到6020角度（-180°——+180°）
					c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
					c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																				(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
		     break;	
       }	
     }
		 //角度归化到0°——360°
		 for (uint8_t i = 0; i < 4; i++) {
			
			 if(c->hopemotorout.rotor6020_jiesuan_1[i]<0){
 				 c->hopemotorout.rotor6020_jiesuan_1[i]+=360;
				}	
 			}
		
     //舵向电机就近转位
     float angle_error[4];//角度误差         
     for (uint8_t i = 0; i < 4; i++) {
       angle_error[i]=c->hopemotorout.rotor6020_jiesuan_1[i]-c->motorfeedback.rotor_angle6020[i];    
       //误差角度归化到-180°——+180°
       while (angle_error[i] > 180) angle_error[i] -= 360;
       while (angle_error[i] < -180) angle_error[i] += 360;
                
       if(angle_error[i]>90&&angle_error[i]<=180){
          c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]-180;
        }
         
        else if(angle_error[i]<-90&&angle_error[i]>=-180){
          c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]+180;
        }
         
        else{
          c->hopemotorout.rotor5065_jiesuan_2[i]=c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i];
        }
     }
		 
}

//该函数用来更新can任务获得的电机反馈值
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {

  for (uint8_t i = 0; i < 4; i++) {
		
		c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
		c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
	 
    c->motorfeedback.rotor_rpm6020[i] = can->motor.chassis6020.as_array[i].rotor_speed;
		c->motorfeedback.rotor_current6020[i] = can->motor.chassis6020.as_array[i].torque_current;
		c->motorfeedback.rotor_angle6020[i] = can->motor.chassis6020.as_array[i].rotor_angle;
		//由于安装不能保证0点朝向我们想要朝向的方向，所以进行零点偏移
		c->motorfeedback.rotor_angle6020[i] = fmod(can->motor.chassis6020.as_array[i].rotor_angle - 
		                                                    c->motoroffset.MOTOR_OFFSET[i], 360.0);
		if(c->motorfeedback.rotor_angle6020[i]<0){
		c->motorfeedback.rotor_angle6020[i]+=360;
		}
		
	  c->motorfeedback.rotor_rpm5065[i] = can->motor.chassis5065.as_array[i].rotor_speed;
	  c->motorfeedback.torque_current5065[i] = can->motor.chassis5065.as_array[i].torque_current;

 }

  return CHASSIS_OK;
}
//底盘初始化
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq){
	 c->param = param; 	/*初始化参数 */

    //舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向左面   
	 MotorOffset_t motor_offset = { {209.281338, 329.226851, 328.404256, 208.594717} };	 
   c->motoroffset = motor_offset;  // 将 motor_offset 的值赋给 c->motoroffset
		
	 PID_init(&(c->pid.chassis_6020OmegaPid), PID_POSITION,&(c->param->C6020Omega_param));
	 PID_init(&(c->pid.chassis_6020anglePid), PID_POSITION,&(c->param->C6020Angle_param)); 		
	 PID_init(&(c->pid.chassis_CameraanglePID),PID_POSITION,&(c->param->CameraAngle_param));
	 PID_init(&(c->pid.chassis_CameraspeedPID),PID_POSITION,&(c->param->CameraSpeed_param));
	
	 LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
	 LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f);  //给y做滤波	
	 LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
	 
	 return CHASSIS_OK;
}
//底盘控制
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out, Action_POS_t*pos){    
	
	Chassis_SetCtrl(c,ctrl);

	fp32 chassis6020_detangle[4];
	
	//对相机发送的像素点偏移量进行双环pid
	fp32 camera_detangle;
	camera_detangle = -PID_calc(&(c->pid.chassis_CameraanglePID),ctrl->camera_yaw,0);
  c->chassis_yaw_pid = PID_calc(&(c->pid.chassis_CameraspeedPID),c->pos088.bmi088.gyro.z,camera_detangle);	
	
  switch (c->mode){	
		case RC:
	   c->move_vec.Vx = ctrl->Vx*95000;
     c->move_vec.Vy = ctrl->Vy*95000;
	   c->move_vec.Vw = ctrl->Vw*95000;
	   break;	
	  case STOP:
		 c->move_vec.Vx =0;
		 c->move_vec.Vy =0;
		 c->move_vec.Vw =0;
		 break; 
	  case NAVI:
	   c->move_vec.Vx =ctrl->C_navi.vx ;
     c->move_vec.Vy =ctrl->C_navi.vy ;    
	   c->move_vec.Vw =ctrl->C_navi.wz ;
	
//     //像素点偏移量	
//	   if(ctrl->camera_yaw != 0){

////		 if(c->chassis_yaw_pid<500 && c->chassis_yaw_pid>-500){
////		 c->chassis_yaw_pid = 0;
////		 }				 
////		 if(c->chassis_yaw_pid>500){
////		c->chassis_yaw_pid	= map_fp32(c->chassis_yaw_pid,0.0,7000.0,4000.0,7000.0); 
////		 }
////     else if(c->chassis_yaw_pid<-500){
////		 c->chassis_yaw_pid	= map_fp32(c->chassis_yaw_pid,-7000.0,0.0,-7000.0,-4000.0);
////		 }
//		 
//		  c->move_vec.Vw = c->chassis_yaw_pid;
//     }	   
	   break;
	}	  
        //进行滤波
    c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
	  c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
	  c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);

	  Chassis_speed_calculate(c,pos);

  for (uint8_t i = 0 ; i <4 ; i++){
    
	  c->hopemotorout.motor6020_target[i]=c->hopemotorout.rotor6020_jiesuan_2[i];
	  chassis6020_detangle[i] = PID_calc(&(c->pid.chassis_6020anglePid),c->motorfeedback.rotor_angle6020[i],
		                                                                 c->hopemotorout.motor6020_target[i]);
	  c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid),c->motorfeedback.rotor_rpm6020[i],
		                                                                                 chassis6020_detangle[i]);
	  out->chassis6020.as_array[i] = c->final_out.final_6020out[i];
		
	  c->hopemotorout.motor5065_target[i]=c->hopemotorout.rotor5065_jiesuan_2[i];
		c->final_out.final_5065out[i]=c->hopemotorout.motor5065_target[i];
		out->chassis5065.erpm[i] = c->final_out.final_5065out[i];
        
	}	
//	 c->vofa_send[0] =c->motorfeedback.rotor_angle6020[0];
//	 c->vofa_send[1] =c->hopemotorout.motor6020_target[0];
//	 c->vofa_send[2] =;
//	 c->vofa_send[3] =;
//	 c->vofa_send[4] =;
//	 c->vofa_send[5] =;
//	 c->vofa_send[6] =;
//	 c->vofa_send[7] =;	
return CHASSIS_OK;
}