#include "navi.h"

//导航初始化一系列参数
int8_t Navi_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos){
	
	if(o ==NULL)  return DEVICE_ERR;
	if(param == NULL) return DEVICE_ERR;

	o->param = param;
	
	//底盘路径速度pid初始化
	PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
	/*全场定位pid初始化*/
	//全场定位跑路径的xy方向速度环pid
	PID_init(&o->pid_PosSpeed_xy, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_xy_param));
	 /* 初始化角度纠正PID */
	PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
	PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环

	PID_init(&o->pid_OutAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_hold_param));//外环
	PID_init(&o->pid_InnerAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_hold_param));//内环
	//底盘xy方向位置环pid初始化
	PID_init(&o->pid_pos_xy_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_inner_param));
	PID_init(&o->pid_pos_xy_outer,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_outer_param));
	//sick
	PID_init(&o->pid_sick_inner,PID_POSITION,(&o->param->ops_pid.pid_sick_inner_param));
	PID_init(&o->pid_sick_outer,PID_POSITION,(&o->param->ops_pid.pid_sick_out_param));
	
	//全场定位底盘坐标初始化
	o->chassis_pos->pos_x=0;
	o->chassis_pos->pos_y=0;	
	o->chassis_pos->pos_lastX=0;
	o->chassis_pos->pos_lastY=0;

  //码盘反馈的坐标
	o->chassis_pos = pos;
	//经过sick调整后码盘反馈的坐标
	o->chassis_pos_offest->pos_x = pos->pos_x + o->sick_offest.x;
	o->chassis_pos_offest->pos_y = pos->pos_y + o->sick_offest.y;

  //底盘获取地图
	o->chassis_map =  get_map_pointer();
	//设置全场定位允许的误差范围
	o->chassis_map->pos_mistake = 30;
	//小量程sick1分米大概变30，大量程sick1分米大概变60
	o->chassis_map->sick_mistake = 15;
	//o->state.points_num = param->path_num;
	//标志位初始化
	o->chassis_map->points_flag = 0;
	o->navi_flag = 0;
	o->sick_flag = 0;
	
  //初始化sick微分数据
  for(int i=0 ;i<4 ; i++){
		
	o->sick_d.sick_distance[i] = 0;	
	o->sick_d.sick_distance_last[i] = 0;	
  o->sick_d.sick_derivative[i] = 0;   
  o->sick_d.status[i] = normal;
	}
	
	return DEVICE_OK;
}

/*判断是否寻迹完成函数(全场定位)*/ 
bool isArrive_path(point_t p,ops_t *o)
{
	uint16_t xArrive = 0, yArrive = 0;
	xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->chassis_map->pos_mistake ? 1:0;
	yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->chassis_map->pos_mistake ? 1:0;

	if(xArrive && yArrive ) return true;
	else return false;
}

//利用C板imu纠正角度
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle,pid_type_def *outer_yaw_pid_set,pid_type_def *inner_yaw_pid_set){
	
	fp32 delta_angle,delta_wz;
	if(hope_angle - o->chassis_pos->pos_yaw >180){
	  hope_angle -= 360;
	} 
	else if(hope_angle - o->chassis_pos->pos_yaw <-180){
	  hope_angle += 360;
	 }
	//外环角度纠正
	delta_angle = PID_calc(outer_yaw_pid_set,o->chassis_pos->pos_yaw,hope_angle);
	//内环速度纠正
	delta_wz = PID_calc(inner_yaw_pid_set,o->ops_gyro.z,delta_angle);
	 
	return delta_wz;
} 

//底盘xy速度计算(全场定位)
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
	o->final_out.action.vx = PID_calc(&o->pid_PosSpeed_xy, o->chassis_pos->pos_Vx,vx_set);
	o->final_out.action.vy = PID_calc(&o->pid_PosSpeed_xy, o->chassis_pos->pos_Vy,vy_set);
	o->final_out.action.vz = ops9_AngleCorr(o,yaw_angle_set,&o->pid_OutAngle,&o->pid_InnerAngle);
} 

//定点控制
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set){
	
	fp32 delta_x,delta_y;
	fp32 delta_vx,delta_vy;
	fp32 hold_vx,hold_vy;
	
	//x
	delta_x = PID_calc(&o->pid_pos_xy_outer,o->chassis_pos->pos_x,p.x);
	delta_vx = PID_calc(&o->pid_pos_xy_inner,o->chassis_pos->pos_Vx,delta_x);
	
	//y
	delta_y = PID_calc(&o->pid_pos_xy_outer,o->chassis_pos->pos_y,p.y);
	delta_vy = PID_calc(&o->pid_pos_xy_inner,o->chassis_pos->pos_Vy,delta_y);
	
	//目标速度
	hold_vx = delta_vy*cos((o->chassis_pos->pos_yaw)*PI/180.0f) - delta_vx*sin((o->chassis_pos->pos_yaw)*PI/180.0f);
	hold_vy = delta_vy*sin((o->chassis_pos->pos_yaw)*PI/180.0f) + delta_vx*cos((o->chassis_pos->pos_yaw)*PI/180.0f);
	
	//底盘速度
	o->final_out.action.vx = hold_vx;
	o->final_out.action.vy = hold_vy;
	o->final_out.action.vz = ops9_AngleCorr(o,yaw_angle_set,&o->pid_OutAngle_hold,&o->pid_InnerAngle_hold);

}

//sick卡点
void sick_point(ops_t *o){
	
	//x是x轴的sick,y是y轴的sick，每个轴各两个
	fp32 delta_x,delta_y;
	fp32 delta_vx,delta_vy;
	//x轴
	if(o->sick_d.status[0] == normal) {	 
		delta_x = -PID_calc(&o->pid_sick_outer,o->sick_distance[0],o->chassis_map->sick->sick_qian);
	  delta_vx = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vx,delta_x);
	}
	else if(o->sick_d.status[2] == normal){
		delta_x = -PID_calc(&o->pid_sick_outer,o->sick_distance[2],o->chassis_map->sick->sick_hou);
	  delta_vx = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vx,delta_x);	
	}
	else{
		delta_vx = 0;
	}
	//y轴
	if(o->sick_d.status[1] == normal){
	  delta_y = -PID_calc(&o->pid_sick_outer,o->sick_distance[1],o->chassis_map->sick->sick_you);
	  delta_vy = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vy,delta_y);	
	}
	else if(o->sick_d.status[3] == normal){
	  delta_y = -PID_calc(&o->pid_sick_outer,o->sick_distance[3],o->chassis_map->sick->sick_zuo);
	  delta_vy = PID_calc(&o->pid_sick_inner,o->chassis_pos->pos_Vy,delta_y);		
	}
	else{
		delta_vy = 0;
	}

	//底盘速度
	o->final_out.sick.vx = delta_vx;
	o->final_out.sick.vy = delta_vy;
	o->final_out.sick.vz = 0;//启用sick时不需要底盘旋转
}
 
// 计算sick微分函数
void sick_calculate_d(ops_t *o){

	static uint32_t last_calc_time =0;
  uint32_t current_time = HAL_GetTick();
    
  // 计算实际时间差(ms)
  uint32_t time_diff = current_time - last_calc_time;
    
  // 至少400ms才计算一次                
	//采样频率不能太高！！
  if (time_diff < 400) {
     return;
  }
    
  // 转换为秒
  float dt = time_diff / 1000.0f;
  last_calc_time = current_time;
    
  for(int i=0; i<4; i++){
        
    o->sick_d.sick_distance[i] = o->sick_distance[i];
    o->sick_d.sick_distance_diff[i] = o->sick_d.sick_distance[i] - o->sick_d.sick_distance_last[i];        
    o->sick_d.sick_derivative[i] = (float)o->sick_d.sick_distance_diff[i] / dt;			
		o->sick_d.sick_distance_last[i] = o->sick_distance[i];
		
	switch(o->sick_d.status[i]){
		
	/*目前这套switch能用，但是没有考虑到一种情况，即有车挡住了sick使标志位变成了block，
	但它缓慢后退然后平移离开导致标志位不能变回normal，后期可以加一个如果底盘没有动微分值改变
  说明被阻挡*/
  	case normal:
      // 检查是否达到最大测量值   小量程sick测量最大值：3238，大量程sick测量最大值：6487
      if(o->sick_d.sick_derivative[i] == 0 && 
      ((o->sick_d.sick_distance[i] > 3200 && o->sick_d.sick_distance[i] < 3300) || 
      (o->sick_d.sick_distance[i] > 6400 && o->sick_d.sick_distance[i] < 6500))){
        
				o->sick_d.status[i] = max_reached;
      }
      // 检查是否被阻挡
      else if(o->sick_d.sick_derivative[i] < -1500){
      
				o->sick_d.status[i] = blocked;
      }
      break;
                
    case blocked:
      // 阻挡解除
      if(o->sick_d.sick_derivative[i] > 1500){
        
				o->sick_d.status[i] = normal; 
      }
      break;
                
    case max_reached:
      // 从最大测量值恢复的条件
      if(o->sick_d.sick_derivative[i] != 0 ){
					
        if(o->sick_d.sick_derivative[i] < -1500){
				 
           o->sick_d.status[i] = blocked;
        }
        else{		
						
					 o->sick_d.status[i] = normal;
				}
      }
      break;
   }		
 }
}

//寻迹函数
void go_path(ops_t *o,CMD_ACTION_t *ops_out){	
	
	//检查码盘是否正常
	if(o->chassis_pos->Action_ready == 1){
     //路径未完成
		 if(o->chassis_map->points_flag == 0){
		
	     //计算距离
	     float delta_x = o->chassis_map->target->x - o->chassis_pos->pos_x;
	     float delta_y = o->chassis_map->target->y - o->chassis_pos->pos_y;
	     float distance = sqrtf(delta_x * delta_x + delta_y * delta_y);
		
	     //计算速度
	     //o->chassis_speed_set = o->chassis_map->target->v;//此处设定为固定的期望速度
       o->chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);
		
       //速度分解
			 float chassis_vx,chassis_vy;//底盘xy分速度	
       if (distance > 1e-6f) {  // 避免除零
         chassis_vx = o->chassis_speed_set * (delta_x / distance);
         chassis_vy = o->chassis_speed_set * (delta_y / distance);
       } 
			 else{
          chassis_vx = 0;
          chassis_vy = 0;
       }	
       o->navi_flag = 0;	

			 //未到达导航点附近
			 if(!isArrive_path(*(o->chassis_map->target),o)){	
 
		    POS_chassis_set(o,chassis_vx,chassis_vy,o->chassis_map->target->angle);	
				o->navi_flag = 1;
				 
	     }
			 else{
				//已到达目标点附近，进行微调
			  hold_point(o,*(o->chassis_map->target),o->chassis_map->target->angle);			
			  o->navi_flag = 2;			 
			 }         
	       
	     ops_out->out.Vx = o->final_out.action.vx;
	     ops_out->out.Vy = o->final_out.action.vy;
	     ops_out->out.Vw = o->final_out.action.vz;  
			 
	     //路径完成,具体的判断值根据之后导航速度提上来之后再定
	     if (fabsf(o->final_out.action.vx) < 1000.0f && 
           fabsf(o->final_out.action.vy) < 1000.0f && 
           fabsf(o->final_out.action.vz) < 1000.0f){
				
//        o->chassis_map->points_flag = 1;
        o->sick_flag = 0;						 
       }		 	
	   }
//  	 else if(o->chassis_map->points_flag == 1 &&o->chassis_map->sick->sick_flag == 1){
//		   if(o->sick_flag != 1){
//				
//			  o->navi_flag = 3;
//				 
//		    bool x_has_valid_sick = false;  // X 轴是否有可用的 Sick
//        bool y_has_valid_sick = false;  // Y 轴是否有可用的 Sick
//        bool x_adjusted = false;       // X 轴是否达标
//        bool y_adjusted = false;       // Y 轴是否达标

//        // X 轴检查
//        if (o->sick_d.status[0] == normal) {  // 前 Sick 可用
//            x_adjusted = (abs_float_double(o->sick_distance[0], o->chassis_map->sick->sick_qian) < o->chassis_map->sick_mistake);
//            x_has_valid_sick = true;
//        } 
//        else if (o->sick_d.status[2] == normal) {  // 后 Sick 可用
//            x_adjusted = (abs_float_double(o->sick_distance[2], o->chassis_map->sick->sick_hou) < o->chassis_map->sick_mistake);
//            x_has_valid_sick = true;
//        }
//        // 如果 X 轴两个 Sick 都不可用，x_has_valid_sick 保持 false

//        // Y 轴检查
//        if (o->sick_d.status[1] == normal) {  // 右 Sick 可用
//            y_adjusted = (abs_float_double(o->sick_distance[1], o->chassis_map->sick->sick_you) < o->chassis_map->sick_mistake);
//            y_has_valid_sick = true;
//        } 
//        else if (o->sick_d.status[3] == normal) {  // 左 Sick 可用
//            y_adjusted = (abs_float_double(o->sick_distance[3], o->chassis_map->sick->sick_zuo) < o->chassis_map->sick_mistake);
//            y_has_valid_sick = true;
//        }
//        // 如果 Y 轴两个 Sick 都不可用，y_has_valid_sick 保持 false

//        // 只有 X 轴和 Y 轴各至少有一个 Sick 可用，且所有可用 Sick 都达标时，才标记完成
//        if (x_has_valid_sick && y_has_valid_sick && x_adjusted && y_adjusted) {
//					
//            o->sick_flag = 1;
//            o->sick_offest.x = o->chassis_map->target->x - o->chassis_pos->pos_x;
//            o->sick_offest.y = o->chassis_map->target->y - o->chassis_pos->pos_y;
//            o->navi_flag = 4;
//        }
//        
//				sick_point(o);
//				 
////		    ops_out->out.Vx = o->final_out.sick.vx;
////	      ops_out->out.Vy = o->final_out.sick.vy;
////	      ops_out->out.Vw = o->final_out.sick.vz;				
//		   }
//       else{
//				 ops_out->out.Vx = 0;
//	       ops_out->out.Vy = 0;
//	       ops_out->out.Vw = 0; 
//		 	 } 			 
//	   }
	 }else{
	   ops_out->out.Vx = 0;
	   ops_out->out.Vy = 0;
	   ops_out->out.Vw = 0; 
    }
}