bool WCPPID::PR3DCluster::proto_break_tracks(WCP::WCPointCloud<double>::WCPoint& first_wcp, WCP::WCPointCloud<double>::WCPoint& curr_wcp, WCP::WCPointCloud<double>::WCPoint& last_wcp, std::list<WCP::WCPointCloud<double>::WCPoint>& wcps_list1, std::list<WCP::WCPointCloud<double>::WCPoint>& wcps_list2, bool flag_pass_check){

  double dis1 = sqrt(pow(curr_wcp.x-first_wcp.x,2) + pow(curr_wcp.y-first_wcp.y,2) + pow(curr_wcp.z-first_wcp.z,2));
  double dis2 = 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));

  //std::cout << first_wcp.index << " " << curr_wcp.index << " " << last_wcp.index << " " << dis1/units::cm << " " << dis2/units::cm << " " << flag_pass_check << std::endl;
  
  if (dis1 > 1*units::cm && dis2 > 1*units::cm || flag_pass_check){
    dijkstra_shortest_paths(first_wcp,2);
    //std::cout << "xixi0 " << std::endl;
    cal_shortest_path(curr_wcp,2);

    wcps_list1 = path_wcps;

    //    std::cout << "xixi1 " << std::endl;
    
    dijkstra_shortest_paths(curr_wcp,2);
    cal_shortest_path(last_wcp,2);
    wcps_list2 = path_wcps;
    
    // std::cout << "xixi2 " << std::endl;
    
    auto it1 = wcps_list1.rbegin();
    int count = 0;
    for (auto it = wcps_list2.begin(); it!=wcps_list2.end(); it++){
      if ( (*it).index==(*it1).index){
	count ++;
	it1++;
      }
    }
    for (int i=0;i!=count;i++){
      if (i+1!=count){
	wcps_list1.pop_back();
	wcps_list2.pop_front();
      }
    }
    curr_wcp = wcps_list1.back();

    if (wcps_list1.size()<=1 || wcps_list2.size()<=1) return false;
    
    return true;
  }else {
    return false;
  }
}



WCP::WCPointCloud<double>::WCPoint WCPPID::PR3DCluster::proto_extend_point(WCP::Point& p, TVector3& dir, TVector3& dir_other, bool flag_continue){

  float step_dis = 1*units::cm;
  
  WCP::WCPointCloud<double>::WCPoint curr_wcp = point_cloud_steiner->get_closest_wcpoint(p);
  WCP::WCPointCloud<double>::WCPoint next_wcp = curr_wcp;

  WCP::WCPointCloud<double>::WCPoint saved_start_wcp = curr_wcp; // saved start position
  TVector3 saved_dir = dir; // saved start direction
  
  WCP::Point test_p;
  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_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. << " " << next_wcp.index_u << " " << next_wcp.index_v << " " << next_wcp.index_w << " " << next_wcp.index << std::endl;
      if (dir2.Mag()!=0 && (dir2.Angle(dir)/3.1415926*180. < 25)){
	//|| (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 = dir2 + dir * 5 * units::cm; // momentum trick ...
	dir = dir.Unit();
	break;
      }
            
      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; 
      //std::cout <<  "A: " << dir1.Angle(dir)/3.1415926*180. << std::endl;
      if (dir1.Mag()!=0 && (dir1.Angle(dir)/3.1415926*180. < 17.5)){
      	//|| (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;
      }
    } 
  }

  /* if (curr_wcp.index != saved_start_wcp.index || sqrt(pow(curr_wcp.x - saved_start_wcp.x,2) + pow(curr_wcp.y - saved_start_wcp.y,2) + pow(curr_wcp.z - saved_start_wcp.z,2)) > 0.01*units::cm){ */
    // forward point ...
    //    std::cout << "Forward search" << std::endl;


  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 << curr_wcp.index << std::endl;
  
  /* }else{ */
  /*   // backward search ... */
  /*   //std::cout << "Backward search" << std::endl; */
    
  /*   while(flag_continue){ */
  /*     flag_continue = false; */
      
  /*     for (int i=0; i!=3; i++){ */
  /* 	test_p.x = curr_wcp.x + dir_other.X() * step_dis * (i+1); */
  /* 	test_p.y = curr_wcp.y + dir_other.Y() * step_dis * (i+1); */
  /* 	test_p.z = curr_wcp.z + dir_other.Z() * step_dis * (i+1); */
      
  /* 	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); */
  /* 	TVector3 dir3(saved_start_wcp.x - next_wcp.x, saved_start_wcp.y - next_wcp.y, saved_start_wcp.z - next_wcp.z);   */
  /* 	if (dir2.Mag()!=0 && (dir2.Angle(dir_other)/3.1415926*180. < 25) && (dir3.Angle(saved_dir)/3.1415926*180.) > 10.) { */
  /* 	  flag_continue = true; */
  /* 	  curr_wcp = next_wcp; */
  /* 	  dir_other = dir2 + dir_other * 5 * units::cm; // momentum trick ... */
  /* 	  dir_other = dir_other.Unit(); */
  /* 	  break; */
  /* 	} */
	
  /* 	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); */
            
  /* 	if (dir1.Mag()!=0 && (dir1.Angle(dir_other)/3.1415926*180. < 17.5) && (dir3.Angle(saved_dir)/3.1415926*180.) > 10.){ */
  /* 	  flag_continue = true; */
  /* 	  curr_wcp = next_wcp; */
  /* 	  dir_other = dir1 + dir_other * 5 * units::cm; // momentum trick ... */
  /* 	  dir_other = dir_other.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); */
  /* } */
  
  
  
  return curr_wcp;
}

void WCPPID::PR3DCluster::set_fit_parameters(WCPPID::ProtoVertex* vtx){
  if (vtx->get_cluster_id()==cluster_id) {
    fine_tracking_path.push_back((vtx)->get_fit_pt());
    dQ.push_back((vtx)->get_dQ());
    dx.push_back((vtx)->get_dx());
    pu.push_back((vtx)->get_pu());
    pv.push_back((vtx)->get_pv());
    pw.push_back((vtx)->get_pw());
    pt.push_back((vtx)->get_pt());
    reduced_chi2.push_back((vtx)->get_reduced_chi2());
    
    sub_cluster_rr.push_back(-1);
    
    flag_vertex.push_back(true);
    sub_cluster_id.push_back(-1);
    flag_shower.push_back(false);
  }
}

void WCPPID::PR3DCluster::set_fit_parameters(WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices){
  fine_tracking_path.clear();
  dQ.clear();
  dx.clear();
  pu.clear();
  pv.clear();
  pw.clear();
  pt.clear();
  reduced_chi2.clear();
  flag_vertex.clear();
  sub_cluster_id.clear();
  flag_shower.clear();
  sub_cluster_rr.clear();

  for (auto it = map_vertex_segments.begin(); it!=map_vertex_segments.end(); it++){
    set_fit_parameters(it->first);
  }

  //  int tmp_id = cluster_id*1000 + 1; // hack ...
  for (auto it=map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){
    WCPPID::ProtoVertex *start_v=0, *end_v=0;
    if ( (*it->second.begin())->get_wcpt().index == it->first->get_wcpt_vec().front().index){
      start_v = (*it->second.begin());
      end_v = (*it->second.rbegin());
    }else{
      end_v = (*it->second.begin());
      start_v = (*it->second.rbegin());
    }
    //    for (auto it1 = map_segment_vertices[it->first].begin(); it1!=map_segment_vertices[it->first].end(); it1++){
    //if ((*it1)->get_wcpt().index == it->first->get_wcpt_vec().front().index) start_v = *it1;
    //if ((*it1)->get_wcpt().index == it->first->get_wcpt_vec().back().index) end_v = *it1;
    //}
    int start_n = map_vertex_segments[start_v].size();
    int end_n = map_vertex_segments[end_v].size();
    
    
    set_fit_parameters(it->first, start_n, end_n);
  }
  //
}

void WCPPID::PR3DCluster::set_fit_parameters(WCPPID::ProtoSegment* seg, int start_n, int end_n){
  if ((seg)->get_cluster_id()==cluster_id){
    fine_tracking_path.insert(fine_tracking_path.end(),(seg)->get_point_vec().begin(), (seg)->get_point_vec().end());
    dQ.insert(dQ.end(),(seg)->get_dQ_vec().begin(), (seg)->get_dQ_vec().end());
    dx.insert(dx.end(),(seg)->get_dx_vec().begin(), (seg)->get_dx_vec().end());
    pu.insert(pu.end(),(seg)->get_pu_vec().begin(), (seg)->get_pu_vec().end());
    pv.insert(pv.end(),(seg)->get_pv_vec().begin(), (seg)->get_pv_vec().end());
    pw.insert(pw.end(),(seg)->get_pw_vec().begin(), (seg)->get_pw_vec().end());
    pt.insert(pt.end(),(seg)->get_pt_vec().begin(), (seg)->get_pt_vec().end());
    reduced_chi2.insert(reduced_chi2.end(),(seg)->get_reduced_chi2_vec().begin(), (seg)->get_reduced_chi2_vec().end());

    // calculate rr vector ...
    WCP::PointVector& pts = (seg)->get_point_vec();
    std::vector<double> L(pts.size(),0);
    std::vector<double> rr(pts.size(),0);
    double acc_length = 0;
    for (size_t i=0;i+1<pts.size();i++){
      acc_length += sqrt(pow(pts.at(i+1).x-pts.at(i).x,2) + pow(pts.at(i+1).y - pts.at(i).y ,2) + pow(pts.at(i+1).z - pts.at(i).z,2)); 
      L.at(i+1) = acc_length;
    }
    
   
    if ((seg)->get_flag_dir()==1){// forward direction
      for (size_t i=0;i!=pts.size();i++){
      	rr.at(pts.size()-1-i) = L.back() - L.at(pts.size()-1-i);
      }
    }else if ((seg)->get_flag_dir()==-1){ // reverse direction
      rr = L;
    }else{
      rr = L; // quick
    }
    //    std::cout << start_n << " " << end_n << std::endl;
    if (start_n>1) rr.front() = -1;
    if (end_n >1) rr.back() = -1;
    sub_cluster_rr.insert(sub_cluster_rr.end(), rr.begin(), rr.end());

    //    std::cout << seg->get_flag_dir() << " " << reduced_chi2.size() << " " << sub_cluster_rr.size() << std::endl;

    
    bool is_shower = (seg)->get_flag_shower();
    for (size_t i=0;i!=(seg)->get_point_vec().size();i++){
      flag_vertex.push_back(false);
      sub_cluster_id.push_back(cluster_id*1000 + (seg)->get_id());
      flag_shower.push_back(is_shower);
    }
  }
}


void WCPPID::PR3DCluster::set_fit_parameters(WCPPID::ProtoVertexSelection& temp_vertices, WCPPID::ProtoSegmentSelection& temp_segments, Map_Proto_Vertex_Segments& map_vertex_segments, Map_Proto_Segment_Vertices& map_segment_vertices){
  fine_tracking_path.clear();
  dQ.clear();
  dx.clear();
  pu.clear();
  pv.clear();
  pw.clear();
  pt.clear();
  reduced_chi2.clear();
  flag_vertex.clear();
  sub_cluster_id.clear();
  flag_shower.clear();

  for (auto it = temp_vertices.begin(); it!=temp_vertices.end(); it++){
    set_fit_parameters(*it);
  }
  
  for (auto it=temp_segments.begin(); it!=temp_segments.end(); it++){
     WCPPID::ProtoVertex *start_v=0, *end_v=0;
    for (auto it1 = map_segment_vertices[*it].begin(); it1!=map_segment_vertices[*it].end(); it1++){
      if ((*it1)->get_wcpt().index == (*it)->get_wcpt_vec().front().index) start_v = *it1;
      if ((*it1)->get_wcpt().index == (*it)->get_wcpt_vec().back().index) end_v = *it1;
    }
    int start_n = map_vertex_segments[start_v].size();
    int end_n = map_vertex_segments[end_v].size();
    set_fit_parameters(*it, start_n, end_n);
  }
  //
}
