#include "navi.h"
#include "define.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->pid.path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
	
	/*全场定位pid初始化*/
	/*xy远距离pid*/
	for(int i=0;i<2;i++){
	  PID_init(&o->pid.pid_PosSpeed_xy[i], PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_xy_param));	
	} 
	/*远距离角度纠正pid */
	PID_init(&o->pid.pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));
	PID_init(&o->pid.pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));
	
	/*xy近距离pid*/
	PID_init(&o->pid.pid_pos_xy_outer,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_outer_param));
	PID_init(&o->pid.pid_pos_xy_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_xy_inner_param));	
	/*近距离角度纠正pid*/
	PID_init(&o->pid.pid_OutAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_hold_param));//外环
	PID_init(&o->pid.pid_InnerAngle_hold,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_hold_param));//内环
	
	//sick
	PID_init(&o->pid.pid_sick_outer,PID_POSITION,(&o->param->ops_pid.pid_sick_out_param));
	PID_init(&o->pid.pid_sick_inner,PID_POSITION,(&o->param->ops_pid.pid_sick_inner_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 = 20;
	//小量程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.pid_PosSpeed_xy[0], o->chassis_pos->pos_Vx,vx_set);
	o->final_out.action.vy = PID_calc(&o->pid.pid_PosSpeed_xy[1], o->chassis_pos->pos_Vy,vy_set);
	o->final_out.action.vz = ops9_AngleCorr(o,yaw_angle_set,&o->pid.pid_OutAngle,&o->pid.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.pid_pos_xy_outer,o->chassis_pos->pos_x,p.x);
	delta_vx = PID_calc(&o->pid.pid_pos_xy_inner,o->chassis_pos->pos_Vx,delta_x);
	
	//y
	delta_y = PID_calc(&o->pid.pid_pos_xy_outer,o->chassis_pos->pos_y,p.y);
	delta_vy = PID_calc(&o->pid.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.pid_OutAngle_hold,&o->pid.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.pid_sick_outer,o->sick_distance[0],o->chassis_map->sick->sick_qian);
	  delta_vx = PID_calc(&o->pid.pid_sick_inner,o->chassis_pos->pos_Vx,delta_x);
	}
	else if(o->sick_d.status[2] == normal){
		delta_x = -PID_calc(&o->pid.pid_sick_outer,o->sick_distance[2],o->chassis_map->sick->sick_hou);
	  delta_vx = PID_calc(&o->pid.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.pid_sick_outer,o->sick_distance[1],o->chassis_map->sick->sick_you);
	  delta_vy = PID_calc(&o->pid.pid_sick_inner,o->chassis_pos->pos_Vy,delta_y);	
	}
	else if(o->sick_d.status[3] == normal){
	  delta_y = -PID_calc(&o->pid.pid_sick_outer,o->sick_distance[3],o->chassis_map->sick->sick_zuo);
	  delta_vy = PID_calc(&o->pid.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,const CAN_t *can){

	//接收can发送的sick数据
#ifdef sick4
	for (uint8_t i = 0; i < 4; i++) {
		o->sick_distance[i] = can->sickfed.as_array[i].sick_distance/10; 
	}	
#elif defined(sick_mini) 
	for (uint8_t i = 0; i < 4; i++) {
	  o->sick_distance[i] = can->sicked_mini.sick_distance[i]/10; 
	}	
#endif				
		 		 
	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] > 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 = 12000;//此处设定为固定的期望速度
       //o->chassis_speed_set = PID_calc(&o->pid.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;
	     ops_out->out.Vx = o->final_out.action.vx/fabsf(o->final_out.action.vx)*6000;
	     ops_out->out.Vy = o->final_out.action.vy/fabsf(o->final_out.action.vy)*6000;
	     ops_out->out.Vw = o->final_out.action.vz; 				 
	     }
			 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{
				 //sick纠正完毕
				 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; 
    }
}
