#include <algorithm>

void WCPPID::PR3DCluster::do_rough_path(WCP::WCPointCloud<double>::WCPoint& first_wcp, WCP::WCPointCloud<double>::WCPoint& last_wcp){
  path_wcps.clear();
  
  Point test_p;
  // find the corresponding points in the Steiner Tree Point Cloud
  {
    test_p.x = first_wcp.x;
    test_p.y = first_wcp.y;
    test_p.z = first_wcp.z;
    first_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
    test_p.x = last_wcp.x;
    test_p.y = last_wcp.y;
    test_p.z = last_wcp.z;
    last_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
  }
  dijkstra_shortest_paths(first_wcp,2);
  cal_shortest_path(last_wcp,2);
}

WCP::Point WCPPID::PR3DCluster::adjust_rough_path(){
  
  Point test_p;

  test_p.x = fine_tracking_path.at(0).x;
  test_p.y = fine_tracking_path.at(0).y;
  test_p.z = fine_tracking_path.at(0).z;

  TVector3 drift_dir(1,0,0);

  int save_i = 0;
  bool flag_crawl = false;

  std::vector<double> refl_angles(fine_tracking_path.size(),0);
  std::vector<double> para_angles(fine_tracking_path.size(),0);
  
  for (size_t i=0;i!=fine_tracking_path.size(); i++){
    
    /* TVector3 v10(0,0,0); */
    /* TVector3 v20(0,0,0); */

    /* TVector3 v11(0,0,0); */
    /* TVector3 v21(0,0,0); */

    /* TVector3 v12(0,0,0); */
    /* TVector3 v22(0,0,0); */
    
    /* if (i>0) */
    /*   v10.SetXYZ(fine_tracking_path.at(i).x - fine_tracking_path.at(i-1).x, */
    /* 		 fine_tracking_path.at(i).y - fine_tracking_path.at(i-1).y, */
    /* 		 fine_tracking_path.at(i).z - fine_tracking_path.at(i-1).z); */
    /* if (i>1) */
    /*   v11.SetXYZ(fine_tracking_path.at(i).x - fine_tracking_path.at(i-2).x, */
    /* 		 fine_tracking_path.at(i).y - fine_tracking_path.at(i-2).y, */
    /* 		 fine_tracking_path.at(i).z - fine_tracking_path.at(i-2).z); */
    /* if (i>2) */
    /*   v12.SetXYZ(fine_tracking_path.at(i).x - fine_tracking_path.at(i-3).x, */
    /* 		 fine_tracking_path.at(i).y - fine_tracking_path.at(i-3).y, */
    /* 		 fine_tracking_path.at(i).z - fine_tracking_path.at(i-3).z); */
    
    /* if (i+1<fine_tracking_path.size()) */
    /*   v20.SetXYZ(fine_tracking_path.at(i+1).x - fine_tracking_path.at(i).x, */
    /* 		 fine_tracking_path.at(i+1).y - fine_tracking_path.at(i).y, */
    /* 		 fine_tracking_path.at(i+1).z - fine_tracking_path.at(i).z); */
    /* if (i+2<fine_tracking_path.size()) */
    /*   v21.SetXYZ(fine_tracking_path.at(i+2).x - fine_tracking_path.at(i).x, */
    /* 		 fine_tracking_path.at(i+2).y - fine_tracking_path.at(i).y, */
    /* 		 fine_tracking_path.at(i+2).z - fine_tracking_path.at(i).z); */
    /* if (i+3<fine_tracking_path.size()) */
    /*   v22.SetXYZ(fine_tracking_path.at(i+3).x - fine_tracking_path.at(i).x, */
    /* 		 fine_tracking_path.at(i+3).y - fine_tracking_path.at(i).y, */
    /* 		 fine_tracking_path.at(i+3).z - fine_tracking_path.at(i).z); */
    
    /* double angle1 = v10.Angle(v20)/3.1415926*180.; */
    /* angle1 = std::min(v11.Angle(v21)/3.1415926*180.,angle1); */
    /* angle1 = std::min(v12.Angle(v22)/3.1415926*180., angle1); */

    /* TVector3 v30 = v10+v20; */
    /* double angle2 = fabs(v30.Angle(drift_dir)/3.1415926*180.-90); */
    /* v30 = v11 + v21; */
    /* angle2 = std::min(fabs(v30.Angle(drift_dir)/3.1415926*180.-90),angle2); */
    /* v30 = v12 + v22; */
    /* angle2 = std::min(fabs(v30.Angle(drift_dir)/3.1415926*180.-90),angle2); */
    

    double angle1 = 0;
    double angle2 = 0;

    for (int j=0;j!=6;j++){    
      TVector3 v10(0,0,0);
      TVector3 v20(0,0,0);
      if (i>j)
	v10.SetXYZ(fine_tracking_path.at(i).x - fine_tracking_path.at(i-j-1).x,
		   fine_tracking_path.at(i).y - fine_tracking_path.at(i-j-1).y,
		   fine_tracking_path.at(i).z - fine_tracking_path.at(i-j-1).z);
      
      if (i+j+1<fine_tracking_path.size())
	v20.SetXYZ(fine_tracking_path.at(i+j+1).x - fine_tracking_path.at(i).x,
		   fine_tracking_path.at(i+j+1).y - fine_tracking_path.at(i).y,
		   fine_tracking_path.at(i+j+1).z - fine_tracking_path.at(i).z);
      
      if (j==0){
	angle1 = v10.Angle(v20)/3.1415926*180.;
	angle2 = std::max(fabs(v10.Angle(drift_dir)/3.1415926*180.-90.),
			  fabs(v20.Angle(drift_dir)/3.1415926*180.-90.));
      }else{
	if (v10.Mag()!=0 && v20.Mag()!=0){
	  angle1 = std::min(v10.Angle(v20)/3.1415926*180., angle1);
	  angle2 = std::min(std::max(fabs(v10.Angle(drift_dir)/3.1415926*180.-90.),
				     fabs(v20.Angle(drift_dir)/3.1415926*180.-90.)),angle2);
	}
      }
    }
    
    refl_angles.at(i) = angle1;
    para_angles.at(i) = angle2;
    //    std::cout << i << " " << angle2 << " " << angle1 << " " << min_dQ_dx << std::endl;
  }


   for (int i=0;i!=fine_tracking_path.size();i++){
     double min_dQ_dx = dQ.at(i)/dx.at(i);
     for (size_t j = 1;j!=6;j++){
       if (i+j<fine_tracking_path.size())
	 if (dQ.at(i+j)/dx.at(i+j) < min_dQ_dx)
	   min_dQ_dx = dQ.at(i+j)/dx.at(i+j);
     }
     
    double sum_angles = 0;
    double nsum = 0;

    for (int j = -2; j!=3;j++){
      if (i+j>=0 && i+j<fine_tracking_path.size()){
	if (para_angles.at(i+j)>10){
	  sum_angles += pow(refl_angles.at(i+j),2);
	  nsum ++;
	}
      }
    }
    if (nsum!=0) sum_angles=sqrt(sum_angles/nsum);
       
    
    if (min_dQ_dx < 1000 && para_angles.at(i) > 10 && refl_angles.at(i) > 25){
      std::cout << "Mid_Point_Break: " << i << " " << refl_angles.at(i) << " " << para_angles.at(i) << " " << min_dQ_dx << " " << fine_tracking_path.at(i).x << " " << fine_tracking_path.at(i).y << " " << fine_tracking_path.at(i).z << std::endl;
      flag_crawl = true;
      save_i = i;
      break;
    }else if (para_angles.at(i) > 15 && refl_angles.at(i) > 27 && sum_angles > 12.5){
       TVector3 v10(fine_tracking_path.at(i).x - fine_tracking_path.front().x,
		   fine_tracking_path.at(i).y - fine_tracking_path.front().y,
		   fine_tracking_path.at(i).z - fine_tracking_path.front().z);
      TVector3 v20(fine_tracking_path.back().x - fine_tracking_path.at(i).x,
		   fine_tracking_path.back().y - fine_tracking_path.at(i).y,
		   fine_tracking_path.back().z - fine_tracking_path.at(i).z);
      double angle3 = v10.Angle(v20)/3.1415926*180.;
      if (angle3 < 20) continue;
      
      std::cout << "Mid_Point_Break: " << i << " " << refl_angles.at(i) << " " << para_angles.at(i) << " " << angle3 << " " << min_dQ_dx << " " << fine_tracking_path.at(i).x << " " << fine_tracking_path.at(i).y << " " << fine_tracking_path.at(i).z << std::endl;
      flag_crawl = true;
      save_i = i;
      break;
    }
      //
  }

  if (flag_crawl){
    // Start to Crawl
    float step_dis = 1*units::cm;
    
    // initialization
    Point p(fine_tracking_path.at(save_i).x, fine_tracking_path.at(save_i).y, fine_tracking_path.at(save_i).z);
    WCP::WCPointCloud<double>::WCPoint curr_wcp = point_cloud_steiner->get_closest_wcpoint(p);
    WCP::WCPointCloud<double>::WCPoint next_wcp = curr_wcp;
    
    Point prev_p(0,0,0);
    int num_p = 0;
    for (size_t i=1;i!=6;i++){
      if (save_i>=i){
	prev_p.x += fine_tracking_path.at(save_i-i).x;
	prev_p.y += fine_tracking_path.at(save_i-i).y;
	prev_p.z += fine_tracking_path.at(save_i-i).z;
	num_p ++;
      }
    }
    prev_p.x /= num_p;
    prev_p.y /= num_p;
    prev_p.z /= num_p;
    
    TVector3 dir(p.x - prev_p.x, p.y - prev_p.y, p.z - prev_p.z);
    dir = dir.Unit();
    
    bool flag_continue = true;
    while(flag_continue){
      flag_continue = false;
      for (int i=0; i!=3; i++){
	test_p.x = curr_wcp.x + dir.X() * step_dis * (i+1);
	test_p.y = curr_wcp.y + dir.Y() * step_dis * (i+1);
	test_p.z = curr_wcp.z + dir.Z() * step_dis * (i+1);
	
	next_wcp = point_cloud->get_closest_wcpoint(test_p);
	TVector3 dir1(next_wcp.x - curr_wcp.x, next_wcp.y - curr_wcp.y, next_wcp.z - curr_wcp.z);
	
	/* std::cout << dir.X() << " " << dir.Y() << " " << dir.Z() << " " << dir1.X() << " " << dir1.Y() << " " << dir1.Z() << " " << drift_dir.Angle(dir)/3.1415926*180. << " " << drift_dir.Angle(dir1)/3.1415926*180. << std::endl; */
	/* std::cout <<curr_wcp.x << " " << curr_wcp.y << " " << curr_wcp.z << " " << i << " " << dir1.Mag() << " " << dir1.Angle(dir)/3.14151926*180. << " " << test_p << " " << next_wcp.x << " " << next_wcp.y << " " << next_wcp.z << std::endl; */
	
	if (dir1.Mag()!=0 && (dir1.Angle(dir)/3.1415926*180. < 30)){
	  //|| (fabs(drift_dir.Angle(dir)/3.1415926*180.-90.)<10. && fabs(drift_dir.Angle(dir1)/3.1415926*180.-90.)<10.) && dir1.Angle(dir)/3.1415926*180. < 60)){
	  flag_continue = true;
	  curr_wcp = next_wcp;
	  dir = dir1 + dir * 5 * units::cm; // momentum trick ...
	  dir = dir.Unit();
	  break;
	}
	
	next_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
	TVector3 dir2(next_wcp.x - curr_wcp.x, next_wcp.y - curr_wcp.y, next_wcp.z - curr_wcp.z);
		
	//	std::cout << dir2.Angle(dir)/3.1415926*180. << " " << i << " " << drift_dir.Angle(dir2)/3.1415926*180. << " "<< drift_dir.Angle(dir)/3.1415926*180. << std::endl;
	
	if (dir2.Mag()!=0 && (dir2.Angle(dir)/3.1415926*180. < 30)){
	    //|| (fabs(drift_dir.Angle(dir)/3.1415926*180.-90.)<10. && fabs(drift_dir.Angle(dir2)/3.1415926*180.-90.)<10.) && dir2.Angle(dir)/3.1415926*180. < 60)){
	  flag_continue = true;
	  curr_wcp = next_wcp;
	  dir = dir1 + dir * 5 * units::cm; // momentum trick ...
	  dir = dir.Unit();
	  break;
	}
	
      }
    }
    
    test_p.x = fine_tracking_path.front().x;
    test_p.y = fine_tracking_path.front().y;
    test_p.z = fine_tracking_path.front().z;
    WCP::WCPointCloud<double>::WCPoint& first_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);

    test_p.x = fine_tracking_path.back().x;
    test_p.y = fine_tracking_path.back().y;
    test_p.z = fine_tracking_path.back().z;
    WCP::WCPointCloud<double>::WCPoint& last_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);

    test_p.x = curr_wcp.x;
    test_p.y = curr_wcp.y;
    test_p.z = curr_wcp.z;
    curr_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
  
    std::cout << "First, Center: " << first_wcp.x << " " << first_wcp.y << " " << first_wcp.z << " " << curr_wcp.x << " " << curr_wcp.y << " " << curr_wcp.z << std::endl;
    
  
    double dis = sqrt(pow(curr_wcp.x-last_wcp.x,2) + pow(curr_wcp.y-last_wcp.y,2) + pow(curr_wcp.z-last_wcp.z,2));

    if (dis > 1*units::cm){
      
      dijkstra_shortest_paths(first_wcp,2);
      cal_shortest_path(curr_wcp,2);


      std::list<WCP::WCPointCloud<double>::WCPoint> temp_path_wcps = path_wcps;
      dijkstra_shortest_paths(curr_wcp,2);
      cal_shortest_path(last_wcp,2);
      
      auto it1 = temp_path_wcps.rbegin();
      int count = 0;
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
      	if ( (*it).index==(*it1).index){
      	  count ++;
      	  it1++;
      	}
      }
      for (int i=0;i!=count;i++){
      	if (i!=count-1){
      	  temp_path_wcps.pop_back();
      	}
      	path_wcps.pop_front();
      }
      
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
      	temp_path_wcps.push_back(*it);
      }
      
      path_wcps = temp_path_wcps;
      
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
      	//std::cout << (*it).x << " " << (*it).y << " " << (*it).z << std::endl;
	
      	if (path_mcells.size()==0){
      	  path_mcells.push_back( (*it).mcell);
      	}else{
      	  if ( (*it).mcell!=path_mcells.back())
      	    path_mcells.push_back( (*it).mcell);
      	}
      }
      
    }
    
  }
  
  
  return test_p;
}


WCP::Point WCPPID::PR3DCluster::do_stm_crawl(WCP::WCPointCloud<double>::WCPoint& first_wcp, WCP::WCPointCloud<double>::WCPoint& last_wcp, int flag_end){
  path_wcps.clear();
  
  Point test_p;
  // find the corresponding points in the Steiner Tree Point Cloud
  {
    test_p.x = first_wcp.x;
    test_p.y = first_wcp.y;
    test_p.z = first_wcp.z;
    first_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
    test_p.x = last_wcp.x;
    test_p.y = last_wcp.y;
    test_p.z = last_wcp.z;
    last_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
  }

  TVector3 drift_dir(1,0,0);
  
  //Crawl in point cloud 
  float step_dis = 1*units::cm;
  
  // initialization
  WCP::WCPointCloud<double>::WCPoint curr_wcp = first_wcp;
  WCP::WCPointCloud<double>::WCPoint next_wcp = first_wcp;
  Point p(first_wcp.x, first_wcp.y, first_wcp.z);
  TVector3 dir = VHoughTrans(p, 15*units::cm); // start direction
  
  
  bool flag_continue = true;
  while(flag_continue){
    flag_continue = false;
    for (int i=0; i!=3; i++){
      test_p.x = curr_wcp.x + dir.X() * step_dis * (i+1);
      test_p.y = curr_wcp.y + dir.Y() * step_dis * (i+1);
      test_p.z = curr_wcp.z + dir.Z() * step_dis * (i+1);

      next_wcp = point_cloud->get_closest_wcpoint(test_p);
      TVector3 dir1(next_wcp.x - curr_wcp.x, next_wcp.y - curr_wcp.y, next_wcp.z - curr_wcp.z);

      std::cout << dir.X() << " " << dir.Y() << " " << dir.Z() << " " << dir1.X() << " " << dir1.Y() << " " << dir1.Z() << " " << drift_dir.Angle(dir)/3.1415926*180. << " " << drift_dir.Angle(dir1)/3.1415926*180. << std::endl;
      std::cout <<curr_wcp.x << " " << curr_wcp.y << " " << curr_wcp.z << " " << i << " " << dir1.Mag() << " " << dir1.Angle(dir)/3.14151926*180. << " " << test_p << " " << next_wcp.x << " " << next_wcp.y << " " << next_wcp.z << std::endl;

      if (dir1.Mag()!=0 && (dir1.Angle(dir)/3.1415926*180. < 30 || (fabs(drift_dir.Angle(dir)/3.1415926*180.-90.)<10. && fabs(drift_dir.Angle(dir1)/3.1415926*180.-90.)<10.) && fabs(drift_dir.Angle(dir)-drift_dir.Angle(dir1))/3.1415926*180. < 20 && dir1.Angle(dir)/3.1415926*180. < 60)){
	flag_continue = true;
	curr_wcp = next_wcp;
      	dir = dir1 + dir * 5 * units::cm; // momentum trick ...
	dir = dir.Unit();
	break;
      }
      
      next_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);
      TVector3 dir2(next_wcp.x - curr_wcp.x, next_wcp.y - curr_wcp.y, next_wcp.z - curr_wcp.z);

      std::cout << dir2.Angle(dir)/3.1415926*180. << " " << i << " " << drift_dir.Angle(dir2)/3.1415926*180. << " "<< drift_dir.Angle(dir)/3.1415926*180. << std::endl;
      
      if (dir2.Mag()!=0 && (dir2.Angle(dir)/3.1415926*180. < 30 || (fabs(drift_dir.Angle(dir)/3.1415926*180.-90.)<10. && fabs(drift_dir.Angle(dir2)/3.1415926*180.-90.)<10.) && fabs(drift_dir.Angle(dir) - drift_dir.Angle(dir2))/3.1415926*180. < 20 && dir2.Angle(dir)/3.1415926*180. < 60)){
	flag_continue = true;
	curr_wcp = next_wcp;
      	dir = dir1 + dir * 5 * units::cm; // momentum trick ...
	dir = dir.Unit();
	break;
      }
      
    }
  }
  test_p.x = curr_wcp.x;
  test_p.y = curr_wcp.y;
  test_p.z = curr_wcp.z;
  curr_wcp = point_cloud_steiner->get_closest_wcpoint(test_p);

  
  //std::cout << first_wcp.x << " " << first_wcp.y << " " << first_wcp.z << " " << curr_wcp.x << " " << curr_wcp.y << " " << curr_wcp.z << std::endl;

  double dis = sqrt(pow(curr_wcp.x-last_wcp.x,2) + pow(curr_wcp.y-last_wcp.y,2) + pow(curr_wcp.z-last_wcp.z,2));
  // Find the shortest path between first and middle point
  
  // If middle point is far away from last_wcp, find the shortest path
  if (dis < 1*units::cm){
    
    dijkstra_shortest_paths(first_wcp,2);
    cal_shortest_path(last_wcp,2);

  }else{
    //    double dis1 = sqrt(pow(curr_wcp.x - last_wcp.x,2)+pow(curr_wcp.y - last_wcp.y,2)+pow(curr_wcp.z - last_wcp.z,2));
    //    double dis2 = sqrt(pow(first_wcp.x - last_wcp.x,2)+pow(first_wcp.y - last_wcp.y,2)+pow(first_wcp.z - last_wcp.z,2));

    std::cout << first_wcp.x << " " << first_wcp.y << " " << first_wcp.z << std::endl;
    std::cout << curr_wcp.x << " " << curr_wcp.y << " " << curr_wcp.z << std::endl;
    std::cout << last_wcp.x << " " << last_wcp.y << " " << last_wcp.z << std::endl;
    // std::cout << dis1 << " " << dis2 << std::endl;

    dijkstra_shortest_paths(first_wcp,2);
    cal_shortest_path(curr_wcp,2);

    
    if (flag_end == 1 ){
      std::list<WCP::WCPointCloud<double>::WCPoint> temp_path_wcps = path_wcps;
      
      dijkstra_shortest_paths(curr_wcp,2);
      cal_shortest_path(last_wcp,2);
      
      auto it1 = temp_path_wcps.rbegin();
      int count = 0;
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
	if ( (*it).index==(*it1).index){
	  count ++;
	  it1++;
	}
      }
      for (int i=0;i!=count;i++){
	if (i!=count-1){
	  temp_path_wcps.pop_back();
	}
	path_wcps.pop_front();
      }
      
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
	temp_path_wcps.push_back(*it);
      }
      
      path_wcps = temp_path_wcps;
     
      for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
	//std::cout << (*it).x << " " << (*it).y << " " << (*it).z << std::endl;
	
	if (path_mcells.size()==0){
	  path_mcells.push_back( (*it).mcell);
	}else{
	  if ( (*it).mcell!=path_mcells.back())
	    path_mcells.push_back( (*it).mcell);
	}
      }
    }
    
  }
       
  // for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
  //  std::cout << (*it).x << " " << (*it).y << " " << (*it).z << std::endl;
  // }
  
  test_p.x = curr_wcp.x;
  test_p.y = curr_wcp.y;
  test_p.z = curr_wcp.z;
  
  return test_p;
  
 
}
