
void WCPPID::PR3DCluster::do_multi_tracking(WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, WCP::ToyCTPointCloud& ct_point_cloud, std::map<int,std::map<const WCP::GeomWire*, WCP::SMGCSelection > >& global_wc_map, double time, bool flag_dQ_dx_fit_reg, bool flag_dQ_dx_fit, bool flag_exclusion){

  /* for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){ */
  /*   std::cout << (*it)->get_wcps().front().index << " " << (*it)->get_wcps */
  /* } */
  /* for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){ */
  /*   std::cout << it->first->get_cluster_id() << " " << it->first->get_id() << " " << it->first->get_wcpt().index << " " << it->second.size() << std::endl; */
  /* } */
  {
    /* std::set<WCPPID::ProtoSegment*> removed_segments; */
    /* for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){ */
    /*   if (it->first->get_cluster_id() != cluster_id) continue; */
    /*   if ( (*it->second.rbegin())->get_wcpt().index ==  (*it->second.begin())->get_wcpt().index ){ */
    /* 	removed_segments.insert(it->first); */
    /*   } */
    /* } */

    
    /* for (auto it = removed_segments.begin(); it!= removed_segments.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = (*it); */
    /*   for (auto it1 = map_segment_vertices[sg].begin(); it1 != map_segment_vertices[sg].end(); it1++){ */
    /* 	map_vertex_segments[*it1].erase(sg); */
    /* 	if (map_vertex_segments[*it1].size()==0) map_vertex_segments.erase(*it1); */
    /*   } */
    /*   map_segment_vertices.erase(sg); */
    /* } */
    /*  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){ */
    /*   if (it->first->get_cluster_id() != cluster_id) continue; */
    /*   std::cout << it->first->get_id() << " " << it->second.size() << " " << it->first->get_wcpt_vec().size() << " " << it->first->get_wcpt_vec().front().index << " " << it->first->get_wcpt_vec().back().index << " " << (*it->second.begin()) << "  " << (*it->second.rbegin()) << std::endl; */
    /*  } */
  }
  

  bool flag_special = false;

  /* for (auto it = map_segment_vertices.begin(); it != map_segment_vertices.end(); it++){ */
  /*   WCPPID::ProtoSegment *sg = it->first; */
  /*   if (sg->get_associated_pcloud_steiner()!=0 ) flag_special = true; */
  /* } */
  for (auto it = map_vertex_segments.begin(); it != map_vertex_segments.end(); it++){
    WCPPID::ProtoVertex *vtx = it->first;
    if (vtx->get_cluster_id() !=cluster_id) continue;
    if (!vtx->get_flag_fit_fix()){
      Point p(vtx->get_wcpt().x, vtx->get_wcpt().y, vtx->get_wcpt().z);
      vtx->set_fit_pt(p);
    }
  }
  
  /* if (flag_special){ */
  /*   for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){ */
  /*     WCPPID::ProtoVertex *vtx = it->first; */
  /*     if (it->second.size()>2){ */
  /* 	std::cout << "0: "  << vtx->get_fit_pt() << " " << vtx->get_pu() << " " << vtx->get_pv() << " " << vtx->get_pw() << " " << vtx->get_pt() << " " << vtx->get_flag_fit_fix() << std::endl; */
  /* 	for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++){ */
  /* 	  WCPPID::ProtoSegment *sg = *it1; */
  /* 	  PointVector& pts = sg->get_point_vec(); */
  /* 	  std::vector<WCP::WCPointCloud<double>::WCPoint >& wcps = sg->get_wcpt_vec(); */
  /* 	  std::vector<double>& pu = sg->get_pu_vec(); */
  /* 	  std::vector<double>& pv = sg->get_pv_vec(); */
  /* 	  std::vector<double>& pw = sg->get_pw_vec(); */
  /* 	  std::vector<double>& pt = sg->get_pt_vec(); */

  /* 	  if (wcps.front().index == vtx->get_wcpt().index){ */
  /* 	    std::cout << pts.front() << " " << pu.size() << " " << pu.front() << " " << pv.front() << " " << pw.front() << " " << pt.front() << std::endl; */
  /* 	  }else if (wcps.back().index = vtx->get_wcpt().index){ */
  /* 	    std::cout << pts.back() << " " << pu.size() << " " << pu.back() << " " << pv.back() << " " << pw.back() << " " << pt.back() << std::endl; */
  /* 	  } */
	  
  /* 	} */
  /*     } */
  /*   } */
  /* } */



  // collect charge
  collect_charge_multi_trajectory(map_segment_vertices, ct_point_cloud);

  // clear things
  fine_tracking_path.clear();
  dQ.clear();
  dx.clear();
  pu.clear();
  pv.clear();
  pw.clear();
  pt.clear();
  reduced_chi2.clear();

  bool flag_1st_tracking = true;
  bool flag_2nd_tracking = true;
  bool flag_dQ_dx = flag_dQ_dx_fit;

  // prepare the data for the fit, do not contain everything ...
  // form from the cluster ...
  std::map<std::pair<int,int>,std::tuple<double,double, int> > map_2D_ut_charge;
  std::map<std::pair<int,int>,std::tuple<double,double, int> > map_2D_vt_charge;
  std::map<std::pair<int,int>,std::tuple<double,double, int> > map_2D_wt_charge;
  prepare_data(ct_point_cloud, global_wc_map, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge);


  
  
  
  // first round of organizing the path from the path_wcps (shortest path)
  double low_dis_limit = 1.2*units::cm;
  double end_point_limit = 0.6*units::cm;
  organize_segments_path(ct_point_cloud, map_vertex_segments, map_segment_vertices, low_dis_limit, end_point_limit);


  
  /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
  /*   WCPPID::ProtoSegment *sg = it->first; */
  /*   std::cout << sg->get_wcpt_vec().size() << " B " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
  /* } */
  
  // form association ...
  // map 3D index to set of 2D points
  std::map<int,std::pair<std::set<std::pair<int,int>>, float> > map_3D_2DU_set;
  std::map<int,std::pair<std::set<std::pair<int,int>>, float> > map_3D_2DV_set;
  std::map<int,std::pair<std::set<std::pair<int,int>>, float> > map_3D_2DW_set;
  std::map<int,std::tuple<WCPPID::ProtoVertex*, WCPPID::ProtoSegment*, int> > map_3D_tuple;
  // map 2D points to 3D indices
  std::map<std::pair<int,int>,std::set<int>> map_2DU_3D_set;
  std::map<std::pair<int,int>,std::set<int>> map_2DV_3D_set;
  std::map<std::pair<int,int>,std::set<int>> map_2DW_3D_set;

 
  
  
  if (flag_1st_tracking){
    form_map_multi_segments(map_vertex_segments, map_segment_vertices, ct_point_cloud,
			    map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,
			    map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_3D_tuple,
			    map_2DU_3D_set, map_2DV_3D_set, map_2DW_3D_set, flag_exclusion);

    /*  if (flag_special){ */
    /* for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){ */
    /*   WCPPID::ProtoVertex *vtx = it->first; */
    /*   if (it->second.size()>2){ */
    /* 	std::cout << "1: " << vtx->get_fit_pt() << " " << vtx->get_pu() << " " << vtx->get_pv() << " " << vtx->get_pw() << " " << vtx->get_pt() << " " << vtx->get_flag_fit_fix() << std::endl; */
    /* 	for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++){ */
    /* 	  WCPPID::ProtoSegment *sg = *it1; */
    /* 	  PointVector& pts = sg->get_point_vec(); */
    /* 	  std::vector<WCP::WCPointCloud<double>::WCPoint >& wcps = sg->get_wcpt_vec(); */
    /* 	  std::vector<double>& pu = sg->get_pu_vec(); */
    /* 	  std::vector<double>& pv = sg->get_pv_vec(); */
    /* 	  std::vector<double>& pw = sg->get_pw_vec(); */
    /* 	  std::vector<double>& pt = sg->get_pt_vec(); */

    /* 	  if (wcps.front().index == vtx->get_wcpt().index){ */
    /* 	    std::cout << pts.front() << " " << pu.size() << " " << pu.front() << " " << pv.front() << " " << pw.front() << " " << pt.front() << std::endl; */
    /* 	  }else if (wcps.back().index = vtx->get_wcpt().index){ */
    /* 	    std::cout << pts.back() << " " << pu.size() << " " << pu.back() << " " << pv.back() << " " << pw.back() << " " << pt.back() << std::endl; */
    /* 	  } */
	  
    /* 	} */
    /*   } */
    /* } */
    /*  } */
    

    
    multi_trajectory_fit(map_vertex_segments, map_segment_vertices,
			 map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_3D_tuple,
			 map_2DU_3D_set, map_2DV_3D_set, map_2DW_3D_set,
			 map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge);
    

    
    
    /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = it->first; */
    /*   PointVector& pts = sg->get_point_vec(); */
    /*   for (size_t i=0;i!=pts.size();i++){ */
    /* 	std::cout << i << " " << pts.at(i) << std::endl; */
    /*   } */
    /*   // std::cout << sg->get_wcpt_vec().size() << " C " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
    /* } */
  }

   /* if (flag_special){ */
   /*   for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){ */
   /*    WCPPID::ProtoVertex *vtx = it->first; */
   /*    if (it->second.size()>2){ */
   /* 	std::cout << "2: " << vtx->get_fit_pt() << " " << vtx->get_pu() << " " << vtx->get_pv() << " " << vtx->get_pw() << " " << vtx->get_pt() << " " << vtx->get_flag_fit_fix() << std::endl; */
   /* 	for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++){ */
   /* 	  WCPPID::ProtoSegment *sg = *it1; */
   /* 	  PointVector& pts = sg->get_point_vec(); */
   /* 	  std::vector<WCP::WCPointCloud<double>::WCPoint >& wcps = sg->get_wcpt_vec(); */
   /* 	  std::vector<double>& pu = sg->get_pu_vec(); */
   /* 	  std::vector<double>& pv = sg->get_pv_vec(); */
   /* 	  std::vector<double>& pw = sg->get_pw_vec(); */
   /* 	  std::vector<double>& pt = sg->get_pt_vec(); */

   /* 	  if (wcps.front().index == vtx->get_wcpt().index){ */
   /* 	    std::cout << pts.front() << " " << pu.size() << " " << pu.front() << " " << pv.front() << " " << pw.front() << " " << pt.front() << std::endl; */
   /* 	  }else if (wcps.back().index = vtx->get_wcpt().index){ */
   /* 	    std::cout << pts.back() << " " << pu.size() << " " << pu.back() << " " << pv.back() << " " << pw.back() << " " << pt.back() << std::endl; */
   /* 	  } */
	  
   /* 	} */
   /*    } */
   /*  } */
   /* } */

  
  if (flag_2nd_tracking){
    // second round trajectory fit ...
    low_dis_limit = 0.6*units::cm;
    end_point_limit = 0.3*units::cm;
    
    /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = it->first; */
    /*   std::cout << sg->get_wcpt_vec().size() << " A " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
    /* } */
    // organize path
    organize_segments_path_2nd(ct_point_cloud, map_vertex_segments, map_segment_vertices, low_dis_limit, end_point_limit);    
    
    //    std::cout << "haha3_1 " << std::endl;
    
    map_3D_2DU_set.clear();
    map_3D_2DV_set.clear();
    map_3D_2DW_set.clear();
    map_3D_tuple.clear();
    // map 2D points to 3D indices
    map_2DU_3D_set.clear();
    map_2DV_3D_set.clear();
    map_2DW_3D_set.clear();

    form_map_multi_segments(map_vertex_segments, map_segment_vertices, ct_point_cloud,
			    map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,
			    map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_3D_tuple,
			    map_2DU_3D_set, map_2DV_3D_set, map_2DW_3D_set, flag_exclusion);

    /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = it->first; */
    /*   std::cout << sg->get_wcpt_vec().size() << " C " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
    /* } */

    /* { */
    /*   int sum[3]={0,0,0}; */
    /*   for (auto it = map_2DU_3D_set.begin(); it!=map_2DU_3D_set.end(); it++){ */
    /* 	sum[0] += it->second.size(); */
    /*   } */
    /*   for (auto it = map_2DV_3D_set.begin(); it!=map_2DV_3D_set.end(); it++){ */
    /* 	sum[1] += it->second.size(); */
    /*   } */
    /*   for (auto it = map_2DW_3D_set.begin(); it!=map_2DW_3D_set.end(); it++){ */
    /* 	sum[2] += it->second.size(); */
    /*   } */
    /*   std::cout << map_2DU_3D_set.size() << " " << map_2DV_3D_set.size() << " " << map_2DW_3D_set.size() << " " << sum[0] << " " << sum[1] << " " << sum[2] << std::endl; */
    /* } */
    //    std::cout << "haha3_2 " << std::endl;

    multi_trajectory_fit(map_vertex_segments, map_segment_vertices,
			 map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_3D_tuple,
			 map_2DU_3D_set, map_2DV_3D_set, map_2DW_3D_set,
			 map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge);
    
    /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = it->first; */
    /*   PointVector& pts = sg->get_point_vec(); */
    /*   for (size_t i=0;i!=pts.size();i++){ */
    /*   if (i==80 || i==83)  */
    /* 	std::cout << i << " " << pts.at(i) << std::endl; */
    /*   } */
    /*   // std::cout << sg->get_wcpt_vec().size() << " C " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
    /*  } */

    /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
    /*   WCPPID::ProtoSegment *sg = it->first; */
    /*   std::cout << sg->get_wcpt_vec().size() << " D " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
    /* } */
    //    std::cout << "haha3_3 " << std::endl;
    // organize path
    low_dis_limit = 0.6*units::cm;
    organize_segments_path_3rd(ct_point_cloud, map_vertex_segments, map_segment_vertices, low_dis_limit);
  }
  
  //  std::cout << "haha4 " << std::endl;

  /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
  /*   WCPPID::ProtoSegment *sg = it->first; */
  /*   std::cout << sg->get_wcpt_vec().size() << " E " << sg->get_point_vec().size() << " " << sg->get_point_vec().front() << " " << sg->get_point_vec().back() << std::endl; */
  /*   if (sg->get_point_vec().size() > 90) std::cout << sg->get_point_vec().at(88) << std::endl; */
  /* } */
  
  /* for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end(); it++){ */
  /*  WCPPID::ProtoSegment *sg = it->first; */
  /*  PointVector& pts = sg->get_point_vec(); */
  /*  //double length = sqrt(pow(pts.front().x-pts.back().x,2) + pow(pts.front().y - pts.back().y,2) + pow(pts.front().z-pts.back().z,2)); */
  /*  //std::cout << sg->get_id() << " " << length/units::cm << std::endl; */
  /*  if (sg->get_id()==3){ */
  /* 	for (size_t i=0;i+1!=pts.size();i++){ */
  /* 	  std::cout << i << " " << sqrt(pow(pts.at(i).x - pts.at(i+1).x,2)+pow(pts.at(i).y - pts.at(i+1).y,2)+pow(pts.at(i).z - pts.at(i+1).z,2))/units::cm << std::endl; */
  /* 	} */
  /*  } */
  /* } */
  

  if (flag_dQ_dx){
    for (auto it = map_vertex_segments.begin(); it!=map_vertex_segments.end(); it++){
      if (it->first->get_cluster_id() != cluster_id) continue;
      WCPPID::ProtoVertex *vtx = it->first;
      bool flag_fix = vtx->get_flag_fit_fix();
      vtx->reset_fit_prop();
      vtx->set_flag_fit_fix(flag_fix);
    }
    for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end();it++){
      if (it->first->get_cluster_id() != cluster_id) continue;
      WCPPID::ProtoSegment *seg = it->first;
      seg->reset_fit_prop();
    }
    
    dQ_dx_multi_fit(map_vertex_segments, map_segment_vertices, global_wc_map, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge, time, end_point_limit, flag_dQ_dx_fit_reg);
  }

  /* if (flag_special){ */
  /*   for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){ */
  /*     WCPPID::ProtoVertex *vtx = it->first; */
  /*     if (it->second.size()>2){ */
  /* 	std::cout << "3: " << vtx->get_fit_pt() << " " << vtx->get_pu() << " " << vtx->get_pv() << " " << vtx->get_pw() << " " << vtx->get_pt() << " " << vtx->get_flag_fit_fix() << std::endl; */
  /* 	for (auto it1 = it->second.begin(); it1 != it->second.end(); it1++){ */
  /* 	  WCPPID::ProtoSegment *sg = *it1; */
  /* 	  PointVector& pts = sg->get_point_vec(); */
  /* 	  std::vector<WCP::WCPointCloud<double>::WCPoint >& wcps = sg->get_wcpt_vec(); */
  /* 	  std::vector<double>& pu = sg->get_pu_vec(); */
  /* 	  std::vector<double>& pv = sg->get_pv_vec(); */
  /* 	  std::vector<double>& pw = sg->get_pw_vec(); */
  /* 	  std::vector<double>& pt = sg->get_pt_vec(); */

  /* 	  if (wcps.front().index == vtx->get_wcpt().index){ */
  /* 	    std::cout << pts.front() << " " << pu.size() << " " << pu.front() << " " << pv.front() << " " << pw.front() << " " << pt.front() << std::endl; */
  /* 	  }else if (wcps.back().index = vtx->get_wcpt().index){ */
  /* 	    std::cout << pts.back() << " " << pu.size() << " " << pu.back() << " " << pv.back() << " " << pw.back() << " " << pt.back() << std::endl; */
  /* 	  } */
	  
  /* 	} */
  /*     } */
  /*   } */
  /* } */
  
}

WCP::Point WCPPID::PR3DCluster::get_pos_multi(std::tuple<WCPPID::ProtoVertex*, WCPPID::ProtoSegment*, int>& input){
  WCPPID::ProtoVertex* vtx = std::get<0>(input);
  WCPPID::ProtoSegment* seg = std::get<1>(input);
  int num = std::get<2>(input);
  Point p;
  if (vtx!=0){
    p = vtx->get_fit_pt();
  }else{
    p = seg->get_point_vec().at(num);
  }
  return p;
}

void WCPPID::PR3DCluster::multi_trajectory_fit(WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, std::map<int, std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DU_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DV_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DW_set, std::map<int, std::tuple<WCPPID::ProtoVertex*, WCPPID::ProtoSegment*, int> >& map_3D_tuple, std::map<std::pair<int,int>,std::set<int>>& map_2DU_3D_set, std::map<std::pair<int,int>,std::set<int>>& map_2DV_3D_set, std::map<std::pair<int,int>,std::set<int>>& map_2DW_3D_set,std::map<std::pair<int,int>, std::tuple<double, double, int > >& map_2D_ut_charge, std::map<std::pair<int,int>, std::tuple<double, double, int> >& map_2D_vt_charge, std::map<std::pair<int,int>,std::tuple<double, double, int> >& map_2D_wt_charge, int charge_div_method, double div_sigma){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  double pitch_u = mp.get_pitch_u();
  double pitch_v = mp.get_pitch_v();
  double pitch_w = mp.get_pitch_w();
  double angle_u = mp.get_angle_u();
  double angle_v = mp.get_angle_v();
  double angle_w = mp.get_angle_w();
  double time_slice_width = mp.get_ts_width();
  double first_u_dis = mp.get_first_u_dis();
  double first_v_dis = mp.get_first_v_dis();
  double first_w_dis = mp.get_first_w_dis();

  /* for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){ */
  /*   std::cout  << " " << it->mcell << std::endl; */
  /* } */
  
  double slope_x = 1./time_slice_width;
  //mcell
  double first_t_dis = point_cloud->get_cloud().pts[0].mcell->GetTimeSlice()*time_slice_width - point_cloud->get_cloud().pts[0].x;
    //  double first_t_dis = path_wcps.front().mcell->GetTimeSlice()*time_slice_width - path_wcps.front().x;
  double offset_t = first_t_dis / time_slice_width;

  //  convert Z to W ... 
  double slope_zw = 1./pitch_w * cos(angle_w);
  double slope_yw = 1./pitch_w * sin(angle_w);
  
  double slope_yu = -1./pitch_u * sin(angle_u);
  double slope_zu = 1./pitch_u * cos(angle_u);
  double slope_yv = -1./pitch_v * sin(angle_v);
  double slope_zv = 1./pitch_v * cos(angle_v);
  
  //convert Y,Z to U,V
  double offset_w = -first_w_dis/pitch_w;
  double offset_u = -first_u_dis/pitch_u;
  double offset_v = -first_v_dis/pitch_v;

  // 2D pixel and then the 3D index ...
  std::map<std::tuple<int,int, int>, double> map_Udiv_fac;
  std::map<std::tuple<int,int, int>, double> map_Vdiv_fac;
  std::map<std::tuple<int,int, int>, double> map_Wdiv_fac;

  if (charge_div_method==1){
    // equal division
    for (auto it = map_2DU_3D_set.begin(); it!=map_2DU_3D_set.end(); it++){
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1./it->second.size();
  	//std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
      }
    }
    for (auto it = map_2DV_3D_set.begin(); it!=map_2DV_3D_set.end(); it++){
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1./it->second.size();
  	//std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
      }
    }
    for (auto it = map_2DW_3D_set.begin(); it!=map_2DW_3D_set.end(); it++){
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = 1./it->second.size();
  	//	std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
      }
    }
  }else if (charge_div_method == 2){
    // use div_sigma ...
    for (auto it = map_2DU_3D_set.begin(); it!=map_2DU_3D_set.end(); it++){
      double sum = 0;
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
	Point tmp_p = get_pos_multi(map_3D_tuple[*it1]);
  	double central_t = slope_x * tmp_p.x + offset_t;
  	double central_ch = slope_yu * tmp_p.y + slope_zu * tmp_p.z + offset_u;
  	double factor = exp(-0.5 * (pow((central_t-it->first.second)*time_slice_width,2) + pow((central_ch-it->first.first)*pitch_u,2))/pow(div_sigma,2));
  	map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
  	sum += factor;
  	//std::cout << it->first.first << " " << it->first.second << " " << *it1 << std::endl;
      }
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Udiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
      }
    }

    for (auto it = map_2DV_3D_set.begin(); it!=map_2DV_3D_set.end(); it++){
      double sum = 0;
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
	Point tmp_p = get_pos_multi(map_3D_tuple[*it1]);
  	double central_t = slope_x * tmp_p.x + offset_t;
  	double central_ch = slope_yv * tmp_p.y + slope_zv * tmp_p.z + offset_v;
  	double factor = exp(-0.5 * (pow((central_t-it->first.second)*time_slice_width,2) + pow((central_ch-it->first.first)*pitch_v,2))/pow(div_sigma,2));
  	map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
  	sum += factor;
	//	if ( it->first.first == 2520 &&  it->first.second ==1)
	//  std::cout << it->first.first << " " << it->first.second << " " << central_t << " " << central_ch << " " << factor << std::endl;
      }
     
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Vdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
      }
    }

    
    for (auto it = map_2DW_3D_set.begin(); it!=map_2DW_3D_set.end(); it++){
      double sum = 0;
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
	Point tmp_p = get_pos_multi(map_3D_tuple[*it1]);
  	double central_t = slope_x * tmp_p.x + offset_t;
  	double central_ch = slope_yw * tmp_p.y + slope_zw * tmp_p.z + offset_w;
  	double factor = exp(-0.5 * (pow((central_t-it->first.second)*time_slice_width,2) + pow((central_ch-it->first.first)*pitch_w,2))/pow(div_sigma,2));
  	map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] = factor;
  	sum += factor;
  	//std::cout << it->first.first << " " << it->first.second << " " << *it1 << " " << factor << std::endl;
      }
      for (auto it1 = it->second.begin(); it1!=it->second.end(); it1++){
  	map_Wdiv_fac[std::make_tuple(it->first.first, it->first.second, *it1)] /= sum;
      }
    }
  }

  // deal vertex
  for (auto it = map_vertex_segments.begin(); it!= map_vertex_segments.end(); it++){
    if (it->first->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *vtx = it->first;
    int i = vtx->get_fit_index();
    bool flag_fit_fix = vtx->get_flag_fit_fix();
    Point init_p = vtx->get_fit_pt();
    
    if (!flag_fit_fix){ // not fix the fit ...
      init_p = fit_point(init_p, i, map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set,
			 map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,
			 map_Udiv_fac, map_Vdiv_fac, map_Wdiv_fac, offset_t, slope_x,
			 offset_u, slope_yu, slope_zu, offset_v, slope_yv, slope_zv,
			 offset_w, slope_yw, slope_zw);
    }
    //    std::cout << "V: " << init_p << std::endl;
    vtx->set_fit(init_p, 0, -1, offset_u + 0.5 + (slope_yu * init_p.y + slope_zu * init_p.z), offset_v + 0.5 + (slope_yv * init_p.y + slope_zv * init_p.z)+2400, offset_w + 0.5 + (slope_yw * init_p.y + slope_zw * init_p.z)+4800, offset_t + 0.5 + slope_x * init_p.x, -1);
  }

  // deal tracks 
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    if (it->first->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoSegment *sg = it->first;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){ */
    /*   WCPPID::ProtoVertex *vt = *it1; */
    /*   if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){ */
    /* 	start_v = vt; */
    /*   }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){ */
    /* 	end_v = vt; */
    /*   } */
    /* } */
    PointVector init_ps = sg->get_point_vec();
    std::vector<int> init_indices = sg->get_fit_index_vec();
    std::vector<bool> init_fit_skip = sg->get_fit_flag_skip();
    //    std::cout << init_ps.size() << " " << init_indices.size() << " " << init_fit_skip.size() << " " << init_fit_skip.front() << " " << init_fit_skip.back() << std::endl;
    init_ps.front() = start_v->get_fit_pt();
    init_ps.back() = end_v->get_fit_pt();
    
    PointVector final_ps;
    
    for (size_t i=0;i!=init_ps.size();i++){
      if (i==0){
	final_ps.push_back(start_v->get_fit_pt());
      }else if (i+1==init_ps.size()){
	final_ps.push_back(end_v->get_fit_pt());
      }else{
	Point temp_p = init_ps.at(i);
	if (!init_fit_skip.at(i)){
	  temp_p = fit_point(init_ps.at(i), init_indices.at(i), map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set,
			     map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,
			     map_Udiv_fac, map_Vdiv_fac, map_Wdiv_fac, offset_t, slope_x,
			     offset_u, slope_yu, slope_zu, offset_v, slope_yv, slope_zv,
			     offset_w, slope_yw, slope_zw);
	      
	  
	}
	//	std::cout << i << " " << init_ps.at(i) << " " << temp_p << std::endl;
	final_ps.push_back(temp_p);
      }
    }

    //    std::cout << final_ps.size() << " ";
    // sort out the results ...
    final_ps = examine_trajectory(final_ps, init_ps, init_indices, map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge, offset_t, slope_x,
			     offset_u, slope_yu, slope_zu, offset_v, slope_yv, slope_zv,
			     offset_w, slope_yw, slope_zw);
    // std::cout << final_ps.size() << " " << init_ps.size() << std::endl;
    
    // set results
    std::vector<double> tmp_dQ_vec(final_ps.size(),0);
    std::vector<double> tmp_dx_vec(final_ps.size(),-1);
    std::vector<double> tmp_pu_vec(final_ps.size(),0);
    std::vector<double> tmp_pv_vec(final_ps.size(),0);
    std::vector<double> tmp_pw_vec(final_ps.size(),0);
    std::vector<double> tmp_pt_vec(final_ps.size(),0);
    std::vector<double> tmp_reduced_chi2_vec(final_ps.size(),0);
    for (size_t i=0;i!=final_ps.size();i++){
      tmp_pu_vec.at(i) = offset_u + 0.5 + (slope_yu * final_ps.at(i).y + slope_zu * final_ps.at(i).z);
      tmp_pv_vec.at(i) = offset_v + 0.5 + (slope_yv * final_ps.at(i).y + slope_zv * final_ps.at(i).z)+2400;
      tmp_pw_vec.at(i) = offset_w + 0.5 + (slope_yw * final_ps.at(i).y + slope_zw * final_ps.at(i).z)+4800;
      tmp_pt_vec.at(i) = offset_t + 0.5 + slope_x * final_ps.at(i).x ;
    }
    sg->set_fit_vec(final_ps, tmp_dQ_vec, tmp_dx_vec, tmp_pu_vec, tmp_pv_vec, tmp_pw_vec, tmp_pt_vec, tmp_reduced_chi2_vec);
    
  }
  
}

WCP::PointVector WCPPID::PR3DCluster::examine_trajectory(WCP::PointVector& final_ps_vec, WCP::PointVector& init_ps_vec, std::vector<int>& init_indices, std::map<int, std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DU_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DV_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DW_set, std::map<std::pair<int,int>, std::tuple<double, double, int > >& map_2D_ut_charge, std::map<std::pair<int,int>, std::tuple<double, double, int> >& map_2D_vt_charge, std::map<std::pair<int,int>,std::tuple<double, double, int> >& map_2D_wt_charge, double offset_t, double slope_x, double offset_u, double slope_yu, double slope_zu, double offset_v, double slope_yv, double slope_zv, double offset_w, double slope_yw, double slope_zw){
  
  PointVector result_ps;
  PointVector orig_ps;
  int skip_count = 0;
  for (size_t i=0; i!=final_ps_vec.size(); i++){
    Point p = final_ps_vec.at(i);
    bool flag_skip = skip_trajectory_point(p, i, init_indices.at(i), init_ps_vec, map_3D_2DU_set, map_3D_2DV_set, map_3D_2DW_set, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge, result_ps, offset_t, slope_x,  offset_u,  slope_yu,  slope_zu,  offset_v,  slope_yv,  slope_zv,  offset_w, slope_yw,  slope_zw);
    //  std::cout << i << " " << p << " " << init_ps_vec.size() << " " << init_ps_vec.at(i) << " " << flag_skip << " " << result_ps.size() << std::endl;
    // vertex not moving ...
    if (i == 0 || i+1 == final_ps_vec.size()) flag_skip = false;
    // protection ...
    if (flag_skip){
      skip_count ++;
      if (skip_count <=3)
	continue;
      else
	skip_count = 0;
    }
    orig_ps.push_back(init_ps_vec.at(i));
    result_ps.push_back(p);
  }

  for (size_t i=0;i!=result_ps.size(); i++){
    bool flag_replace = false;
    
    if (i != 0 && i+1 != result_ps.size()) {
      // -1 vs. +1
      double a = sqrt(pow(result_ps.at(i-1).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i-1).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i-1).z - result_ps.at(i).z,2));
      double b = sqrt(pow(result_ps.at(i+1).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i+1).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i+1).z - result_ps.at(i).z,2));
      double c = sqrt(pow(result_ps.at(i-1).x - result_ps.at(i+1).x,2)
		      +pow(result_ps.at(i-1).y - result_ps.at(i+1).y,2)
		      +pow(result_ps.at(i-1).z - result_ps.at(i+1).z,2));
      double s = (a+b+c)/2.;
      double area1 = sqrt(s*(s-a)*(s-b)*(s-c));

      a = sqrt(pow(result_ps.at(i-1).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i-1).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i-1).z - orig_ps.at(i).z,2));
      b = sqrt(pow(result_ps.at(i+1).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i+1).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i+1).z - orig_ps.at(i).z,2));
      s = (a+b+c)/2.;
      double area2 = sqrt(s*(s-a)*(s-b)*(s-c));

      //      std::cout << i << " A " << area1/c << " " << area2/c  << std::endl;

      if (area1 > 1.8*units::mm * c && area1 > 1.7 * area2)
	flag_replace = true;
    }
      
    // -2, +1
    if ((!flag_replace) && i>=2 && i+1 != result_ps.size()){
      double a = sqrt(pow(result_ps.at(i-2).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i-2).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i-2).z - result_ps.at(i).z,2));
      double b = sqrt(pow(result_ps.at(i+1).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i+1).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i+1).z - result_ps.at(i).z,2));
      double c = sqrt(pow(result_ps.at(i-2).x - result_ps.at(i+1).x,2)
		      +pow(result_ps.at(i-2).y - result_ps.at(i+1).y,2)
		      +pow(result_ps.at(i-2).z - result_ps.at(i+1).z,2));
      double s = (a+b+c)/2.;
      double area1 = sqrt(s*(s-a)*(s-b)*(s-c));

      a = sqrt(pow(result_ps.at(i-2).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i-2).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i-2).z - orig_ps.at(i).z,2));
      b = sqrt(pow(result_ps.at(i+1).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i+1).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i+1).z - orig_ps.at(i).z,2));
      s = (a+b+c)/2.;
      double area2 = sqrt(s*(s-a)*(s-b)*(s-c));
      //std::cout << i << " B " << area1/c << " " << area2/c  << std::endl;
      if (area1 > 1.8*units::mm * c && area1 > 1.7 * area2)
	flag_replace = true;	
     
      
    }
    
    
    // -1, +2
    if ((!flag_replace) && i>0 && i+2<result_ps.size()){
      double a = sqrt(pow(result_ps.at(i-1).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i-1).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i-1).z - result_ps.at(i).z,2));
      double b = sqrt(pow(result_ps.at(i+2).x - result_ps.at(i).x,2)
		      +pow(result_ps.at(i+2).y - result_ps.at(i).y,2)
		      +pow(result_ps.at(i+2).z - result_ps.at(i).z,2));
      double c = sqrt(pow(result_ps.at(i-1).x - result_ps.at(i+2).x,2)
		      +pow(result_ps.at(i-1).y - result_ps.at(i+2).y,2)
		      +pow(result_ps.at(i-1).z - result_ps.at(i+2).z,2));
      double s = (a+b+c)/2.;
      double area1 = sqrt(s*(s-a)*(s-b)*(s-c));

      a = sqrt(pow(result_ps.at(i-1).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i-1).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i-1).z - orig_ps.at(i).z,2));
      b = sqrt(pow(result_ps.at(i+2).x - orig_ps.at(i).x,2)
	       +pow(result_ps.at(i+2).y - orig_ps.at(i).y,2)
	       +pow(result_ps.at(i+2).z - orig_ps.at(i).z,2));
      s = (a+b+c)/2.;
      double area2 = sqrt(s*(s-a)*(s-b)*(s-c));

      //      std::cout << i << " C " << area1/c << " " << area2/c  << std::endl;
      
      if (area1 > 1.8*units::mm * c && area1 > 1.7 * area2)
	flag_replace = true;
    }

    
    if (flag_replace){
      result_ps.at(i) = orig_ps.at(i);
      //      std::cout << i << std::endl;
    }
  }
  
  
  return result_ps;
}


Point WCPPID::PR3DCluster::fit_point(Point& init_p, int i, std::map<int, std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DU_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DV_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DW_set, std::map<std::pair<int,int>, std::tuple<double, double, int > >& map_2D_ut_charge, std::map<std::pair<int,int>, std::tuple<double, double, int> >& map_2D_vt_charge, std::map<std::pair<int,int>,std::tuple<double, double, int> >& map_2D_wt_charge, std::map<std::tuple<int,int, int>, double>& map_Udiv_fac, std::map<std::tuple<int,int, int>, double>& map_Vdiv_fac, std::map<std::tuple<int,int, int>, double>& map_Wdiv_fac, double offset_t, double slope_x, double offset_u, double slope_yu, double slope_zu, double offset_v, double slope_yv, double slope_zv, double offset_w, double slope_yw, double slope_zw){
  int n_2D_u = 2* map_3D_2DU_set[i].first.size();
  int n_2D_v = 2* map_3D_2DV_set[i].first.size();
  int n_2D_w = 2* map_3D_2DW_set[i].first.size();
  Eigen::VectorXd temp_pos_3D(3),data_u_2D(n_2D_u), data_v_2D(n_2D_v), data_w_2D(n_2D_w);
  Eigen::VectorXd temp_pos_3D_init(3);
  Eigen::SparseMatrix<double> RU(n_2D_u, 3) ;
  Eigen::SparseMatrix<double> RV(n_2D_v, 3) ;
  Eigen::SparseMatrix<double> RW(n_2D_w, 3) ;
  // initialization
  temp_pos_3D_init(0) = init_p.x;
  temp_pos_3D_init(1) = init_p.y;
  temp_pos_3D_init(2) = init_p.z;

  for (size_t i=0;i!=n_2D_u;i++){
    data_u_2D(i) = 0;
  }
  for (size_t i=0;i!=n_2D_v;i++){
    data_v_2D(i) = 0;
  }
  for (size_t i=0;i!=n_2D_w;i++){
    data_w_2D(i) = 0;
  }

  //  std::cout << map_3D_2DU_set[i].first.size() << " " << map_3D_2DV_set[i].first.size() << " " << map_3D_2DW_set[i].first.size() << std::endl;
  
  int index = 0; 
  for (auto it = map_3D_2DU_set[i].first.begin(); it!=map_3D_2DU_set[i].first.end(); it++){
    double charge, charge_err;
    if (map_2D_ut_charge.find(*it)!=map_2D_ut_charge.end()){
      charge = std::get<0>(map_2D_ut_charge[*it]);
      charge_err = std::get<1>(map_2D_ut_charge[*it]);
    }else{
      charge = 100; // serve as regularization ...
      charge_err = 1000;
    }
    if (charge<100){
      charge = 100;
      charge_err = 1000;
    }
    // induction plane error is large, so they have less weights in the fit to decide X position
    double scaling = charge/charge_err * map_Udiv_fac[std::make_tuple(it->first, it->second, i)];
    
    if (map_3D_2DU_set[i].second < 0.5){
      if (map_3D_2DU_set[i].second!=0)
	scaling *= pow(map_3D_2DU_set[i].second/0.5,1);
      else
	scaling *= 0.05;
    }

    //    std::cout << "U: " << scaling << std::endl;
    
    if (scaling!=0){
      data_u_2D(2*index) =  scaling * (it->first - offset_u);
      data_u_2D(2*index+1) = scaling * (it->second - offset_t);
      
      RU.insert(2*index, 1) = scaling * slope_yu; // Y--> U
      RU.insert(2*index, 2) = scaling * slope_zu; // Z--> U
      RU.insert(2*index+1,0) = scaling * slope_x; // X --> T
    }
    index ++;
  }

  index = 0;
  for (auto it = map_3D_2DV_set[i].first.begin(); it!=map_3D_2DV_set[i].first.end(); it++){
    double charge, charge_err;
    if (map_2D_vt_charge.find(*it)!=map_2D_vt_charge.end()){
      charge = std::get<0>(map_2D_vt_charge[*it]);
      charge_err = std::get<1>(map_2D_vt_charge[*it]);
    }else{
      charge = 100; // serve as regularization ...
      charge_err = 1000;
    }
    if (charge < 100){
      charge = 100;
      charge_err = 1000;
    }
    // induction plane error is large, so they have less weights in the fit to decide X position
    double scaling = charge/charge_err * map_Vdiv_fac[std::make_tuple(it->first, it->second, i)];
    if (map_3D_2DV_set[i].second < 0.5){
      if (map_3D_2DV_set[i].second!=0)
	scaling *= pow(map_3D_2DV_set[i].second/0.5,1);
      else
	scaling *= 0.05;
    }

    //    std::cout << "V: " << scaling << std::endl;
    if (scaling!=0){
      data_v_2D(2*index) =  scaling * (it->first - offset_v);
      data_v_2D(2*index+1) = scaling * (it->second - offset_t);
      
      RV.insert(2*index, 1) = scaling * slope_yv; // Y--> V
      RV.insert(2*index, 2) = scaling * slope_zv; // Z--> V
      RV.insert(2*index+1,0) = scaling * slope_x; // X --> T
    }
    index ++;
  }

  index = 0;
  for (auto it = map_3D_2DW_set[i].first.begin(); it!=map_3D_2DW_set[i].first.end(); it++){
    double charge, charge_err;
    if (map_2D_wt_charge.find(*it)!=map_2D_wt_charge.end()){
      charge = std::get<0>(map_2D_wt_charge[*it]);
      charge_err = std::get<1>(map_2D_wt_charge[*it]);
    }else{
      charge = 100; // serve as regularization ...
      charge_err = 1000;
    }
    if (charge<100){
      charge = 100;
      charge_err = 1000;
    }
    // induction plane error is large, so they have less weights in the fit to decide X position
    double scaling = charge/charge_err * map_Wdiv_fac[std::make_tuple(it->first, it->second, i)];
    
    if (map_3D_2DW_set[i].second < 0.5){
      if (map_3D_2DW_set[i].second!=0)
	scaling *= pow(map_3D_2DW_set[i].second/0.5,1);
      else
	scaling *= 0.05;
    }
    //std::cout << "W: " << scaling << std::endl;
    if (scaling!=0){
      data_w_2D(2*index) =  scaling * (it->first - offset_w);
      data_w_2D(2*index+1) = scaling * (it->second - offset_t);
      
      RW.insert(2*index, 2) = scaling * slope_zw; // Z--> W
      RW.insert(2*index+1,0) = scaling * slope_x; // X --> T
    }
    index ++;
  }
  

  Point final_p;
  
  Eigen::SparseMatrix<double> RUT = Eigen::SparseMatrix<double>(RU.transpose());
  Eigen::SparseMatrix<double> RVT = Eigen::SparseMatrix<double>(RV.transpose());
  Eigen::SparseMatrix<double> RWT = Eigen::SparseMatrix<double>(RW.transpose());
  
  Eigen::BiCGSTAB<Eigen::SparseMatrix<double>> solver;
  Eigen::VectorXd b = RUT * data_u_2D + RVT * data_v_2D + RWT * data_w_2D;// + PMatrixT * pos_3D_init ;
  
  Eigen::SparseMatrix<double> A =   RUT * RU + RVT * RV + RWT * RW ;// + PMatrixT*PMatrix;
  solver.compute(A);
  
  temp_pos_3D = solver.solveWithGuess(b,temp_pos_3D_init);
  
  
  if (std::isnan(solver.error())){
    final_p.x = temp_pos_3D_init(0);
    final_p.y = temp_pos_3D_init(1);
    final_p.z = temp_pos_3D_init(2);
  }else{
    final_p.x = temp_pos_3D(0);
    final_p.y = temp_pos_3D(1);
    final_p.z = temp_pos_3D(2);
    }
  
    
  //  std::cout << init_p << " " << final_p << " " << std::isnan(solver.error()) << std::endl;
  
  return final_p;
}


void WCPPID::PR3DCluster::form_map_multi_segments(WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, WCP::ToyCTPointCloud& ct_point_cloud,
						  std::map<std::pair<int,int>,std::tuple<double,double, int> >& map_2D_ut_charge, std::map<std::pair<int,int>,std::tuple<double,double, int> >& map_2D_vt_charge, std::map<std::pair<int,int>,std::tuple<double,double, int> >& map_2D_wt_charge,
						  std::map<int, std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DU_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DV_set, std::map<int,std::pair<std::set<std::pair<int,int>>, float> >& map_3D_2DW_set, std::map<int, std::tuple<WCPPID::ProtoVertex*, WCPPID::ProtoSegment*, int> >& map_3D_tuple,
						  std::map<std::pair<int,int>,std::set<int>>& map_2DU_3D_set, std::map<std::pair<int,int>,std::set<int>>& map_2DV_3D_set, std::map<std::pair<int,int>,std::set<int>>& map_2DW_3D_set, bool flag_exclusion,
						  double end_point_factor, double mid_point_factor, int nlevel, double time_cut, double charge_cut){
  map_3D_2DU_set.clear();
  map_3D_2DV_set.clear();
  map_3D_2DW_set.clear();
  map_3D_tuple.clear();

  map_2DU_3D_set.clear();
  map_2DV_3D_set.clear();
  map_2DW_3D_set.clear();

  for (auto it = map_vertex_segments.begin(); it!=map_vertex_segments.end(); it++){
    if (it->first->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *vtx = it->first;
    bool flag_fix = vtx->get_flag_fit_fix();
    vtx->reset_fit_prop();
    vtx->set_flag_fit_fix(flag_fix);
  }
  //  std::vector<ToyPointCloud* > associated_pclouds_steiner;
  ProtoSegmentSelection segments;
  
  for (auto it = map_segment_vertices.begin(); it!=map_segment_vertices.end();it++){
    if (it->first->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoSegment *seg = it->first;
    seg->reset_fit_prop();
    //    WCP::ToyPointCloud *pcloud_associated_steiner =  seg->get_associated_pcloud_steiner();
    //if (pcloud_associated_steiner != 0) associated_pclouds_steiner.push_back(pcloud_associated_steiner);
    segments.push_back(seg);
  }


  
  int count = 0;
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){ */
    /*   WCPPID::ProtoVertex *vt = *it1; */
    /*   if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){ */
    /* 	start_v = vt; */
    /*   }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){ */
    /* 	end_v = vt; */
    /*   } */
    /* } */
    std::vector<WCP::Point >& pts = sg->get_point_vec();
    PointVector saved_pts;
    std::vector<int> saved_index;
    std::vector<bool> saved_skip;
    
    
    std::vector<double> distances;
    for (size_t i=0;i+1!=pts.size();i++){
      distances.push_back(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)));
    }
    
    for (size_t i=0;i!=pts.size();i++){
      double dis_cut;
      if (i==0){
	dis_cut = std::min(distances.at(i) * end_point_factor,4/3.*end_point_factor*units::cm);
	if (start_v->get_fit_range()<0){
	  start_v->set_fit_range(dis_cut);
	}else{
	  if (dis_cut < start_v->get_fit_range()) start_v->set_fit_range(dis_cut);
	}
      }else if (i+1==pts.size()){
	dis_cut = std::min(distances.back() * end_point_factor,4/3.*end_point_factor*units::cm);
	if (end_v->get_fit_range()<0){
	  end_v->set_fit_range(dis_cut);
	}else{
	  if (dis_cut < end_v->get_fit_range()) end_v->set_fit_range(dis_cut);
	}
      }else{
	dis_cut = std::min(std::max(distances.at(i-1)*mid_point_factor,distances.at(i)*mid_point_factor),4/3.*mid_point_factor*units::cm);
      }

      
      
      // not the first and last point
      if (i!=0 && i+1!=pts.size()){
	std::set<std::pair<int,int> > temp_2dut, temp_2dvt, temp_2dwt;
	form_point_association(pts.at(i), temp_2dut, temp_2dvt, temp_2dwt, ct_point_cloud, dis_cut, nlevel, time_cut);

	// examine things ...
	//if (sg->get_associated_pcloud_steiner()!=0 )
	//std::cout << pts.at(i) << " " << sqrt(pow(pts.front().x-pts.back().x,2) + pow(pts.front().y-pts.back().y,2) + pow(pts.front().z-pts.back().z,2))/units::cm << std::endl;

	if (flag_exclusion)
	  update_association(temp_2dut, temp_2dvt, temp_2dwt, sg, segments);
	

	// examine ...
	std::vector<int> temp_results = ct_point_cloud.convert_3Dpoint_time_ch(pts.at(i));
	temp_results.at(2)-=2400;
	temp_results.at(3)-=4800;
	std::vector<float> temp_flag;
	if (i==0 || i==1 || i+1 ==pts.size() || i+2 == pts.size()){
	  temp_flag = examine_point_association(temp_results, temp_2dut, temp_2dvt, temp_2dwt, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,true,charge_cut);
	}else{
	  temp_flag = examine_point_association(temp_results, temp_2dut, temp_2dvt, temp_2dwt, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,false,charge_cut);
	}
	if (temp_flag.at(0) + temp_flag.at(1) + temp_flag.at(2) > 0){
	  map_3D_2DU_set[count] = std::make_pair(temp_2dut,temp_flag.at(0));
	  map_3D_2DV_set[count] = std::make_pair(temp_2dvt,temp_flag.at(1));
	  map_3D_2DW_set[count] = std::make_pair(temp_2dwt,temp_flag.at(2));
	  map_3D_tuple[count] = std::make_tuple((WCPPID::ProtoVertex*)0,sg,i);
	  
	  for (auto it = temp_2dut.begin(); it!=temp_2dut.end();it++){
	    if (map_2DU_3D_set.find(*it)==map_2DU_3D_set.end()){
	      std::set<int>  temp_set;
	      temp_set.insert(count);
	      map_2DU_3D_set[*it] = temp_set;
	    }else{
	      map_2DU_3D_set[*it].insert(count);
	    }
	  }
	  for (auto it = temp_2dvt.begin(); it!=temp_2dvt.end();it++){
	    if (map_2DV_3D_set.find(*it)==map_2DV_3D_set.end()){
	      std::set<int>  temp_set;
	      temp_set.insert(count);
	      map_2DV_3D_set[*it] = temp_set;
	    }else{
	      map_2DV_3D_set[*it].insert(count);
	    }
	  }
	  for (auto it = temp_2dwt.begin(); it!=temp_2dwt.end();it++){
	    if (map_2DW_3D_set.find(*it)==map_2DW_3D_set.end()){
	      std::set<int>  temp_set;
	      temp_set.insert(count);
	      map_2DW_3D_set[*it] = temp_set;
	    }else{
	      map_2DW_3D_set[*it].insert(count);
	    }
	  }
	  
	  saved_pts.push_back(pts.at(i));
	  saved_index.push_back(count);
	  saved_skip.push_back(false);
	  count ++;
	}
      }else if (i==0){ // first point
	saved_pts.push_back(pts.at(i));
	saved_index.push_back(count);
	saved_skip.push_back(true);
	if (start_v->get_fit_index()==-1){
	  start_v->set_fit_index(count);
	  count ++;
	}
      }else if (i+1==pts.size()){ // last point
	saved_pts.push_back(pts.at(i));
	saved_index.push_back(count);
	saved_skip.push_back(true);
	if (end_v->get_fit_index()==-1){
	  end_v->set_fit_index(count);
	  count ++;
	}
      } 
    } // loop over points ...
    // fill the segments ...
    sg->set_fit_associate_vec(saved_pts, saved_index, saved_skip);
    //    std::cout << saved_pts.size() << " " << saved_index.size() << " " << saved_skip.size() << std::endl;
  } // loop over segment

  // deal with all the vertex again ...
  for (auto it = map_vertex_segments.begin(); it!=map_vertex_segments.end(); it++){
    if (it->first->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *vtx = it->first;
    double dis_cut = vtx->get_fit_range();
    count = vtx->get_fit_index();
    Point pt = vtx->get_fit_pt();

    //std::cout << dis_cut/units::cm << std::endl;
    
    std::set<std::pair<int,int> > temp_2dut, temp_2dvt, temp_2dwt; 
    form_point_association(pt, temp_2dut, temp_2dvt, temp_2dwt, ct_point_cloud, dis_cut, nlevel, time_cut); 
    // examine ... 
    std::vector<int> temp_results = ct_point_cloud.convert_3Dpoint_time_ch(pt); 
    temp_results.at(2)-=2400; 
    temp_results.at(3)-=4800; 
    std::vector<float> temp_flag;

    //    std::cout << temp_2dut.size() << " " << temp_2dvt.size() << " " << temp_2dwt.size() << std::endl;
    
    temp_flag = examine_point_association(temp_results, temp_2dut, temp_2dvt, temp_2dwt, map_2D_ut_charge, map_2D_vt_charge, map_2D_wt_charge,true,charge_cut); 

    //    std::cout << temp_2dut.size() << " " << temp_2dvt.size() << " " << temp_2dwt.size() << std::endl;
    
    map_3D_2DU_set[count] = std::make_pair(temp_2dut,temp_flag.at(0)); 
    map_3D_2DV_set[count] = std::make_pair(temp_2dvt,temp_flag.at(1)); 
    map_3D_2DW_set[count] = std::make_pair(temp_2dwt,temp_flag.at(2)); 
    map_3D_tuple[count] = std::make_tuple(vtx,(WCPPID::ProtoSegment*)0,0); 
	  
    for (auto it = temp_2dut.begin(); it!=temp_2dut.end();it++){
      if (map_2DU_3D_set.find(*it)==map_2DU_3D_set.end()){
	std::set<int>  temp_set;
	temp_set.insert(count);
	map_2DU_3D_set[*it] = temp_set;
      }else{
	map_2DU_3D_set[*it].insert(count);
      }
    }
    for (auto it = temp_2dvt.begin(); it!=temp_2dvt.end();it++){
      if (map_2DV_3D_set.find(*it)==map_2DV_3D_set.end()){
	std::set<int>  temp_set;
	temp_set.insert(count);
	map_2DV_3D_set[*it] = temp_set;
      }else{
	map_2DV_3D_set[*it].insert(count);
      }
    }
    for (auto it = temp_2dwt.begin(); it!=temp_2dwt.end();it++){
      if (map_2DW_3D_set.find(*it)==map_2DW_3D_set.end()){
	std::set<int>  temp_set;
	temp_set.insert(count);
	map_2DW_3D_set[*it] = temp_set;
      }else{
	map_2DW_3D_set[*it].insert(count);
      }
    }
   
  }


}

void WCPPID::PR3DCluster::update_association(std::set<std::pair<int,int> >& temp_2dut, std::set<std::pair<int,int> >& temp_2dvt, std::set<std::pair<int,int> >& temp_2dwt, WCPPID::ProtoSegment* sg, WCPPID::ProtoSegmentSelection& segments){
  
  // get global information
  TPCParams& mp = Singleton<TPCParams>::Instance();
  double pitch_u = mp.get_pitch_u();
  double pitch_v = mp.get_pitch_v();
  double pitch_w = mp.get_pitch_w();
  double angle_u = mp.get_angle_u();
  double angle_v = mp.get_angle_v();
  double angle_w = mp.get_angle_w();
  double time_slice_width = mp.get_ts_width();
  double first_u_dis = mp.get_first_u_dis();
  double first_v_dis = mp.get_first_v_dis();
  double first_w_dis = mp.get_first_w_dis();

  /* for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){ */
  /*   std::cout  << " " << it->mcell << std::endl; */
  /* } */
  
  double slope_x = 1./time_slice_width;
  //mcell
  double first_t_dis = point_cloud->get_cloud().pts[0].mcell->GetTimeSlice()*time_slice_width - point_cloud->get_cloud().pts[0].x;
    //  double first_t_dis = path_wcps.front().mcell->GetTimeSlice()*time_slice_width - path_wcps.front().x;
  double offset_t = first_t_dis / time_slice_width;

  //  convert Z to W ... 
  double slope_zw = 1./pitch_w * cos(angle_w);
  double slope_yw = 1./pitch_w * sin(angle_w);
  
  double slope_yu = -1./pitch_u * sin(angle_u);
  double slope_zu = 1./pitch_u * cos(angle_u);
  double slope_yv = -1./pitch_v * sin(angle_v);
  double slope_zv = 1./pitch_v * cos(angle_v);
  
  //convert Y,Z to U,V
  double offset_w = -first_w_dis/pitch_w;
  double offset_u = -first_u_dis/pitch_u;
  double offset_v = -first_v_dis/pitch_v;

  std::set<std::pair<int,int> > save_2dut, save_2dvt, save_2dwt;
  
  for (auto it = temp_2dut.begin(); it!=temp_2dut.end(); it++){
    double x = (it->second - offset_t)/slope_x;
    double y = (it->first - offset_u)*pitch_u;
    
    //    double min_dis = sg->get_associated_pcloud_steiner()->get_closest_2d_dis(x,y,0);
    double min_dis_track = sg->get_closest_2d_dis(x,y,0);

    //    double min_dis1 = 1e9;
    double min_dis1_track = 1e9;
    
    for (size_t i=0;i!=segments.size();i++){ 
      if (segments.at(i)==sg) continue;
      // if (segments.at(i)->get_associated_pcloud_steiner()!=0){
      //	double temp_dis = segments.at(i)->get_associated_pcloud_steiner()->get_closest_2d_dis(x,y,0);
      //	if (temp_dis < min_dis1) min_dis1 = temp_dis;
      //}
      double temp_dis = segments.at(i)->get_closest_2d_dis(x,y,0);
      if (temp_dis < min_dis1_track)
	min_dis1_track = temp_dis;
    } 
    bool flag_save = false;
    
    if (min_dis_track < min_dis1_track || // closer to the main track ...
	min_dis_track < 0.3*units::cm 
	//	|| 	min_dis_track < 0.6*units::cm && min_dis1_track > 0.45*units::cm
	){
      flag_save = true;
      save_2dut.insert(*it);
      
      //  if (sg->get_associated_pcloud_steiner()!=0)
      //std::cout << it->first << " " << it->second << " " << flag_save << " " << min_dis_track/units::cm <<  " " << min_dis1_track/units::cm << std::endl;
      
    }
  }

  for (auto it = temp_2dvt.begin(); it!=temp_2dvt.end(); it++){
    double x = (it->second - offset_t)/slope_x;
    double y = (it->first - offset_v)*pitch_v;
    
    double min_dis_track = sg->get_closest_2d_dis(x,y,1);
    double min_dis1_track = 1e9;
    
    for (size_t i=0;i!=segments.size();i++){ 
      if (segments.at(i)==sg) continue;
      double temp_dis = segments.at(i)->get_closest_2d_dis(x,y,1);
      if (temp_dis < min_dis1_track)
	min_dis1_track = temp_dis;
    } 
    
    if (min_dis_track < min_dis1_track || // closer to the main track ...
	min_dis_track < 0.3*units::cm 
	//|| 	min_dis_track < 0.6*units::cm && min_dis1_track > 0.45*units::cm
	){
      save_2dvt.insert(*it);
    }
  }
  
  for (auto it = temp_2dwt.begin(); it!=temp_2dwt.end(); it++){
    double x = (it->second - offset_t)/slope_x;
    double y = (it->first - offset_w)*pitch_w;
    
    double min_dis_track = sg->get_closest_2d_dis(x,y,2);
    double min_dis1_track = 1e9;
    
    for (size_t i=0;i!=segments.size();i++){ 
      if (segments.at(i)==sg) continue;
      double temp_dis = segments.at(i)->get_closest_2d_dis(x,y,2);
      if (temp_dis < min_dis1_track)
	min_dis1_track = temp_dis;
    } 
    
    if (min_dis_track < min_dis1_track  // closer to the main track ...
	|| min_dis_track < 0.3*units::cm 
	// || min_dis_track < 0.6*units::cm && min_dis1_track > 0.45*units::cm
	){
      save_2dwt.insert(*it);
    }
  }

  //  std::cout << temp_2dut.size() << " " << save_2dut.size() << " " << temp_2dvt.size() << " " << save_2dvt.size() << " " << temp_2dwt.size() << " " << save_2dwt.size() << std::endl;
  
  temp_2dut = save_2dut; 
  temp_2dvt = save_2dvt; 
  temp_2dwt = save_2dwt; 
  
}



void WCPPID::PR3DCluster::organize_segments_path_3rd(WCP::ToyCTPointCloud& ct_point_cloud, WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, double step_size){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  double pitch_u = mp.get_pitch_u();
  double pitch_v = mp.get_pitch_v();
  double pitch_w = mp.get_pitch_w();
  double angle_u = mp.get_angle_u();
  double angle_v = mp.get_angle_v();
  double angle_w = mp.get_angle_w();
  double time_slice_width = mp.get_ts_width();
  double first_u_dis = mp.get_first_u_dis();
  double first_v_dis = mp.get_first_v_dis();
  double first_w_dis = mp.get_first_w_dis();

  /* for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){ */
  /*   std::cout  << " " << it->mcell << std::endl; */
  /* } */
  
  double slope_x = 1./time_slice_width;
  //mcell
  double first_t_dis = point_cloud->get_cloud().pts[0].mcell->GetTimeSlice()*time_slice_width - point_cloud->get_cloud().pts[0].x;
    //  double first_t_dis = path_wcps.front().mcell->GetTimeSlice()*time_slice_width - path_wcps.front().x;
  double offset_t = first_t_dis / time_slice_width;

  //  convert Z to W ... 
  double slope_zw = 1./pitch_w * cos(angle_w);
  double slope_yw = 1./pitch_w * sin(angle_w);
  
  double slope_yu = -1./pitch_u * sin(angle_u);
  double slope_zu = 1./pitch_u * cos(angle_u);
  double slope_yv = -1./pitch_v * sin(angle_v);
  double slope_zv = 1./pitch_v * cos(angle_v);
  
  //convert Y,Z to U,V
  double offset_w = -first_w_dis/pitch_w;
  double offset_u = -first_u_dis/pitch_u;
  double offset_v = -first_v_dis/pitch_v;

   // examine things
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){
    // WCPPID::ProtoVertex *vt = *it1;
    //  if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){
    //	start_v = vt;
    // }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){
    //	end_v = vt;
    // }
    //}
    // check the distance between the two vertices ...
    if (sqrt(pow(start_v->get_fit_pt().x - end_v->get_fit_pt().x,2) + pow(start_v->get_fit_pt().y - end_v->get_fit_pt().y,2) + pow(start_v->get_fit_pt().z - end_v->get_fit_pt().z,2)) < 0.01*units::cm){
      //      std::cout << "too close" << std::endl;
      if (map_vertex_segments[start_v].size()==1){
	Point tmp_p(start_v->get_wcpt().x, start_v->get_wcpt().y, start_v->get_wcpt().z);
	start_v->set_fit_pt(tmp_p);
      }
      if (map_vertex_segments[end_v].size()==1){
	Point tmp_p(end_v->get_wcpt().x, end_v->get_wcpt().y, end_v->get_wcpt().z);
	end_v->set_fit_pt(tmp_p);
      }
    }
  }


  
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){
    // WCPPID::ProtoVertex *vt = *it1;
    // if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){
    //	start_v = vt;
    // }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){
    //	end_v = vt;
    // }
    //}
    bool flag_startv_end = true;
    bool flag_endv_end = true;
    if (map_vertex_segments[start_v].size()>1) flag_startv_end = false;
    if (map_vertex_segments[end_v].size()>1) flag_endv_end = false;
    
    PointVector pts;
    PointVector curr_pts = examine_end_ps_vec(ct_point_cloud, sg->get_point_vec(), flag_startv_end, flag_endv_end); //= sg->get_point_vec();
    Point start_p, end_p;
    
    start_p = curr_pts.front();
    end_p = curr_pts.back();

    //    std::cout << start_p << " " << end_p << std::endl;
    
    // middle points
    pts.push_back(start_p);
    double dis_end;
    double dis_prev;

    // extra distance ...
    double extra_dis = 0;
    for (size_t i=0;i!=curr_pts.size(); i++){
      Point p1 = curr_pts.at(i);
      //      std::cout << i << " " << p1 << std::endl;

      dis_end = sqrt(pow(p1.x-end_p.x,2) + pow(p1.y-end_p.y,2) + pow(p1.z-end_p.z,2)); 
      if (dis_end < step_size) continue;

      dis_prev = sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2));
      if (dis_prev + extra_dis > step_size){
	extra_dis += dis_prev;
	while (extra_dis > step_size){
	  Point tmp_p;
	  tmp_p.x = pts.back().x + (p1.x - pts.back().x)/dis_prev * step_size;
	  tmp_p.y = pts.back().y + (p1.y - pts.back().y)/dis_prev * step_size;
	  tmp_p.z = pts.back().z + (p1.z - pts.back().z)/dis_prev * step_size;
	  pts.push_back(tmp_p);
	  dis_prev = sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2));	  
	  extra_dis -= step_size;
	}
	
      }else if (dis_prev + extra_dis < step_size){
	extra_dis += dis_prev;
	continue;
      }else{
	pts.push_back(p1);
      }
    }

    // proper deal with the end_point ...
    {
      double dis1 = sqrt(pow(pts.back().x-end_p.x,2) + pow(pts.back().y-end_p.y,2) + pow(pts.back().z-end_p.z,2));

      /* if (sg->get_id()==3){ */
      /* 	std::cout << "kkk: " << dis1/units::cm << " " << std::endl; */
      /* 	for (size_t i=0;i+1!=curr_pts.size();i++){ */
      /* 	  std::cout << "kk: " << i << " " << sqrt(pow(curr_pts.at(i).x - curr_pts.at(i+1).x,2) + pow(curr_pts.at(i).y - curr_pts.at(i+1).y,2) + pow(curr_pts.at(i).z - curr_pts.at(i+1).z,2))/units::cm << std::endl; */
      /* 	} */
      /* } */
      
      if (dis1 < step_size * 0.6) {
	if (pts.size()<=1){
	}else{
	  double dis2 = sqrt(pow(pts.back().x - pts.at(pts.size()-2).x,2) + pow(pts.back().y - pts.at(pts.size()-2).y,2) + pow(pts.back().z - pts.at(pts.size()-2).z,2));
	  double dis3 = (dis1+dis2)/2.;

	  Point tmp_p;
	  tmp_p.x =  pts.at(pts.size()-2).x + (pts.back().x - pts.at(pts.size()-2).x)/dis2*dis3;
	  tmp_p.y =  pts.at(pts.size()-2).y + (pts.back().y - pts.at(pts.size()-2).y)/dis2*dis3;
	  tmp_p.z =  pts.at(pts.size()-2).z + (pts.back().z - pts.at(pts.size()-2).z)/dis2*dis3;
	  pts.pop_back();
	  pts.push_back(tmp_p);
	}
      }else if (dis1 > step_size * 1.6){

	int npoints = std::round(dis1/step_size);
	Point p_save = pts.back();
	for (int j=0;j+1<npoints;j++){
	  Point p(p_save.x + (end_p.x-p_save.x) / npoints * (j+1),
		  p_save.y + (end_p.y-p_save.y) / npoints * (j+1),
		  p_save.z + (end_p.z-p_save.z) / npoints * (j+1));
	  pts.push_back(p);
	}
      }
      
      pts.push_back(end_p);
    }


    // ensure there is no single point ...
    if (pts.size()==1){
      //if (sqrt(pow(end_p.x - pts.back().x,2) + pow(end_p.y - pts.back().y,2) + pow(end_p.z - pts.back().z,2) ) >0)
      pts.push_back(end_p);
      //else{
      //	pts.clear();
      //	Point tmp1(start_v->get_wcpt().x, start_v->get_wcpt().y, start_v->get_wcpt().z);
      //	pts.push_back(tmp1);
      //Point tmp2(end_v->get_wcpt().x, end_v->get_wcpt().y, end_v->get_wcpt().z);
      //pts.push_back(tmp2);
      //}
    }
    
    //    for (size_t i=1;i<pts.size();i++){
    //  std::cout << i << " " << sqrt(pow(pts.at(i).x-pts.at(i-1).x,2) + pow(pts.at(i).y-pts.at(i-1).y,2) + pow(pts.at(i).z-pts.at(i-1).z,2))/units::cm << std::endl;
    //}
    
    
    std::vector<double> tmp_dQ_vec(pts.size(),0);
    std::vector<double> tmp_dx_vec(pts.size(),-1);
    std::vector<double> tmp_pu_vec(pts.size(),0);
    std::vector<double> tmp_pv_vec(pts.size(),0);
    std::vector<double> tmp_pw_vec(pts.size(),0);
    std::vector<double> tmp_pt_vec(pts.size(),0);
    std::vector<double> tmp_reduced_chi2_vec(pts.size(),0);
    for (size_t i=0;i!=pts.size();i++){
      tmp_pu_vec.at(i) = offset_u + 0.5 + (slope_yu * pts.at(i).y + slope_zu * pts.at(i).z);
      tmp_pv_vec.at(i) = offset_v + 0.5 + (slope_yv * pts.at(i).y + slope_zv * pts.at(i).z)+2400;
      tmp_pw_vec.at(i) = offset_w + 0.5 + (slope_yw * pts.at(i).y + slope_zw * pts.at(i).z)+4800;
      tmp_pt_vec.at(i) = offset_t + 0.5 + slope_x * pts.at(i).x ;
    }
    sg->set_fit_vec(pts, tmp_dQ_vec, tmp_dx_vec, tmp_pu_vec, tmp_pv_vec, tmp_pw_vec, tmp_pt_vec, tmp_reduced_chi2_vec);

    
    //    std::cout << sg << " " << pts.front() << " " << pts.back() << " " << start_v << " " << end_v << std::endl;
    //    std::cout << sg << flag_startv_end << " " << flag_endv_end << std::endl;
    //    std::cout << sg << " " << start_v << " " << end_v << std::endl;
    
  }

  
}

void WCPPID::PR3DCluster::organize_segments_path_2nd(WCP::ToyCTPointCloud& ct_point_cloud, WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices,double low_dis_limit, double end_point_limit){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  double pitch_u = mp.get_pitch_u();
  double pitch_v = mp.get_pitch_v();
  double pitch_w = mp.get_pitch_w();
  double angle_u = mp.get_angle_u();
  double angle_v = mp.get_angle_v();
  double angle_w = mp.get_angle_w();
  double time_slice_width = mp.get_ts_width();
  double first_u_dis = mp.get_first_u_dis();
  double first_v_dis = mp.get_first_v_dis();
  double first_w_dis = mp.get_first_w_dis();

  /* for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){ */
  /*   std::cout  << " " << it->mcell << std::endl; */
  /* } */
  
  double slope_x = 1./time_slice_width;
  //mcell
  double first_t_dis = point_cloud->get_cloud().pts[0].mcell->GetTimeSlice()*time_slice_width - point_cloud->get_cloud().pts[0].x;
    //  double first_t_dis = path_wcps.front().mcell->GetTimeSlice()*time_slice_width - path_wcps.front().x;
  double offset_t = first_t_dis / time_slice_width;

  //  convert Z to W ... 
  double slope_zw = 1./pitch_w * cos(angle_w);
  double slope_yw = 1./pitch_w * sin(angle_w);
  
  double slope_yu = -1./pitch_u * sin(angle_u);
  double slope_zu = 1./pitch_u * cos(angle_u);
  double slope_yv = -1./pitch_v * sin(angle_v);
  double slope_zv = 1./pitch_v * cos(angle_v);
  
  //convert Y,Z to U,V
  double offset_w = -first_w_dis/pitch_w;
  double offset_u = -first_u_dis/pitch_u;
  double offset_v = -first_v_dis/pitch_v;


  // examine things
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){
    // WCPPID::ProtoVertex *vt = *it1;
    // if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){
    //	start_v = vt;
    // }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){
    //	end_v = vt;
    // }
    //}
    // check the distance between the two vertices ...
    if (sqrt(pow(start_v->get_fit_pt().x - end_v->get_fit_pt().x,2) + pow(start_v->get_fit_pt().y - end_v->get_fit_pt().y,2) + pow(start_v->get_fit_pt().z - end_v->get_fit_pt().z,2)) < 0.01*units::cm){
      //      std::cout << "too close" << std::endl;
      if (map_vertex_segments[start_v].size()==1){
	Point tmp_p(start_v->get_wcpt().x, start_v->get_wcpt().y, start_v->get_wcpt().z);
	start_v->set_fit_pt(tmp_p);
      }
      if (map_vertex_segments[end_v].size()==1){
	Point tmp_p(end_v->get_wcpt().x, end_v->get_wcpt().y, end_v->get_wcpt().z);
	end_v->set_fit_pt(tmp_p);
      }
    }
  }
  
  // start organization 
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    WCPPID::ProtoVertex *start_v = 0, *end_v = 0;
    if ( (*it->second.begin())->get_wcpt().index == sg->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=it->second.begin(); it1!=it->second.end(); it1++){
    // WCPPID::ProtoVertex *vt = *it1;
    // if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){
    //	start_v = vt;
    //}else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){
    //	end_v = vt;
    //}
    //}
    bool flag_startv_end = true;
    bool flag_endv_end = true;
    if (map_vertex_segments[start_v].size()>1) flag_startv_end = false;
    if (map_vertex_segments[end_v].size()>1) flag_endv_end = false;

    PointVector pts;
    PointVector curr_pts = examine_end_ps_vec(ct_point_cloud, sg->get_point_vec(), flag_startv_end, flag_endv_end);
    Point start_p, end_p;

    if (!start_v->get_flag_fit_fix()){
      // start_ point
      start_p = curr_pts.front();
      
      if (flag_startv_end){
	Point p2 = curr_pts.front();
	double dis1 = 0;
	for (auto it = curr_pts.begin(); it!= curr_pts.end(); it++){
	  p2 = (*it);
	  dis1 = sqrt(pow(start_p.x-p2.x,2)+pow(start_p.y-p2.y,2)+pow(start_p.z-p2.z,2));
	  if (dis1 > low_dis_limit) break;
	}
	if (dis1!=0){
	  start_p.x += (start_p.x-p2.x)/dis1*end_point_limit;
	  start_p.y += (start_p.y-p2.y)/dis1*end_point_limit;
	  start_p.z += (start_p.z-p2.z)/dis1*end_point_limit;
	}
      }
      start_v->set_fit_pt(start_p);
    }else{
      start_p = start_v->get_fit_pt();
    }

    if (!end_v->get_flag_fit_fix()){
      // end_point
      end_p = curr_pts.back();
      
      if (flag_endv_end){
	Point p2 = curr_pts.back();
	double dis1 = 0;
	for (auto it = curr_pts.rbegin(); it!=curr_pts.rend(); it++){
	  p2 = (*it);
	  dis1 = sqrt(pow(end_p.x-p2.x,2)+pow(end_p.y-p2.y,2)+pow(end_p.z-p2.z,2));
	  if (dis1 > low_dis_limit) break;
	}
	if (dis1!=0){
	  end_p.x += (end_p.x - p2.x)/dis1 * end_point_limit;
	  end_p.y += (end_p.y - p2.y)/dis1 * end_point_limit;
	  end_p.z += (end_p.z - p2.z)/dis1 * end_point_limit;
	}
      }
      
      end_v->set_fit_pt(end_p);
    }else{
      end_p = end_v->get_fit_pt();
    }

    
    //    std::cout << start_p << " " << end_p << std::endl;
    

    
    // middle points
    pts.push_back(start_p);
    for (size_t i=0;i!=curr_pts.size(); i++){
      Point p1 = curr_pts.at(i);
      double dis = low_dis_limit;
      double dis1 = sqrt(pow(p1.x-end_p.x,2) + pow(p1.y-end_p.y,2) + pow(p1.z-end_p.z,2));
      if (pts.size()>0)
	dis = sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2));

      if (dis1 < low_dis_limit * 0.8){
	continue;
      }else if (dis < low_dis_limit * 0.8 ){
	continue;
      }else if (dis < low_dis_limit * 1.6){
	pts.push_back(p1);
	//std::cout << p1 << " " << sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2))/units::cm << std::endl;
      }else{
	int npoints = std::round(dis/low_dis_limit);
	
	Point p_save = pts.back();
	for (int j=0;j!=npoints;j++){
	  Point p(p_save.x + (p1.x-p_save.x) / npoints * (j+1),
		  p_save.y + (p1.y-p_save.y) / npoints * (j+1),
		  p_save.z + (p1.z-p_save.z) / npoints * (j+1));
	  pts.push_back(p);
	  //	std::cout << p << " " << sqrt(pow(p.x-pts.back().x,2)+pow(p.y-pts.back().y,2)+pow(p.z-pts.back().z,2))/units::cm << std::endl;
	}
	
      }
    }

    {
      double dis1 = sqrt(pow(pts.back().x-end_p.x,2) + pow(pts.back().y-end_p.y,2) + pow(pts.back().z-end_p.z,2));
      if (dis1 < low_dis_limit * 0.2) {
	if (pts.size()>1) pts.pop_back();
      }else if (dis1 > low_dis_limit * 1.6){

	int npoints = std::round(dis1/low_dis_limit);
	Point p_save = pts.back();
	for (int j=0;j+1<npoints;j++){
	  Point p(p_save.x + (end_p.x-p_save.x) / npoints * (j+1),
		  p_save.y + (end_p.y-p_save.y) / npoints * (j+1),
		  p_save.z + (end_p.z-p_save.z) / npoints * (j+1));
	  pts.push_back(p);
	}
	
	//Point tmp_p;
	//tmp_p.x =  pts.back().x + (end_p.x - pts.back().x )/2.;
	//tmp_p.y =  pts.back().y + (end_p.y - pts.back().y )/2.;
	//tmp_p.z =  pts.back().z + (end_p.z - pts.back().z )/2.;
	//pts.push_back(tmp_p);
      }
      pts.push_back(end_p);
    }
    
    if (pts.size()==1){
      //      if (sqrt(pow(end_p.x - pts.back().x,2) + pow(end_p.y - pts.back().y,2) + pow(end_p.z - pts.back().z,2) ) >0)
      pts.push_back(end_p);
      /* else{ */
      /* 	pts.clear(); */
      /* 	Point tmp1(start_v->get_wcpt().x, start_v->get_wcpt().y, start_v->get_wcpt().z); */
      /* 	pts.push_back(tmp1); */
      /* 	Point tmp2(end_v->get_wcpt().x, end_v->get_wcpt().y, end_v->get_wcpt().z); */
      /* 	pts.push_back(tmp2); */
      /* } */
    }
    
    
    // sg->set_point_vec(pts);
    std::vector<double> tmp_dQ_vec(pts.size(),0);
    std::vector<double> tmp_dx_vec(pts.size(),-1);
    std::vector<double> tmp_pu_vec(pts.size(),0);
    std::vector<double> tmp_pv_vec(pts.size(),0);
    std::vector<double> tmp_pw_vec(pts.size(),0);
    std::vector<double> tmp_pt_vec(pts.size(),0);
    std::vector<double> tmp_reduced_chi2_vec(pts.size(),0);
    for (size_t i=0;i!=pts.size();i++){
      tmp_pu_vec.at(i) = offset_u + 0.5 + (slope_yu * pts.at(i).y + slope_zu * pts.at(i).z);
      tmp_pv_vec.at(i) = offset_v + 0.5 + (slope_yv * pts.at(i).y + slope_zv * pts.at(i).z)+2400;
      tmp_pw_vec.at(i) = offset_w + 0.5 + (slope_yw * pts.at(i).y + slope_zw * pts.at(i).z)+4800;
      tmp_pt_vec.at(i) = offset_t + 0.5 + slope_x * pts.at(i).x ;
    }
    sg->set_fit_vec(pts, tmp_dQ_vec, tmp_dx_vec, tmp_pu_vec, tmp_pv_vec, tmp_pw_vec, tmp_pt_vec, tmp_reduced_chi2_vec);
    
    
    //    std::cout << sg << " " << pts.front() << " " << pts.back() << " " << start_v << " " << end_v << std::endl;
    //    std::cout << sg << flag_startv_end << " " << flag_endv_end << std::endl;
    //    std::cout << sg << " " << start_v << " " << end_v << std::endl;
  }
}

void WCPPID::PR3DCluster::organize_segments_path(WCP::ToyCTPointCloud& ct_point_cloud, WCPPID::Map_Proto_Vertex_Segments& map_vertex_segments, WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, double low_dis_limit, double end_point_limit){
  
  for (auto it = map_segment_vertices.begin(); it!= map_segment_vertices.end(); it++){
    WCPPID::ProtoSegment *sg = it->first;
    if (sg->get_cluster_id() != cluster_id) continue;
    
     WCPPID::ProtoVertex *start_v = 0, *end_v = 0;     
    /* for (auto it1=it->second.begin(); it1!=it->second.end(); it1++){ */
    /*   WCPPID::ProtoVertex *vt = *it1; */
    /*   if ( vt->get_wcpt().index == sg->get_wcpt_vec().front().index){ */
    /* 	start_v = vt; */
    /*   }else if ( vt->get_wcpt().index == sg->get_wcpt_vec().back().index){ */
    /* 	end_v = vt; */
    /*   } */
    /* } */
     if ( (*it->second.begin())->get_wcpt().index == sg->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());
     }
     
    
    bool flag_startv_end = true;
    bool flag_endv_end = true;
    if (map_vertex_segments[start_v].size()>1) flag_startv_end = false;
    if (map_vertex_segments[end_v].size()>1) flag_endv_end = false;

    PointVector pts;
    std::vector<WCPointCloud<double>::WCPoint> temp_wcps_vec = sg->get_wcpt_vec();
    Point start_p, end_p;

    if (!start_v->get_flag_fit_fix()){
      // start_point
      start_p.x = temp_wcps_vec.front().x;
      start_p.y = temp_wcps_vec.front().y;
      start_p.z = temp_wcps_vec.front().z;
      
      if (flag_startv_end){
	Point p2(temp_wcps_vec.front().x, temp_wcps_vec.front().y, temp_wcps_vec.front().z);
	double dis1 = 0;
	for (auto it = temp_wcps_vec.begin(); it!=temp_wcps_vec.end(); it++){
	  p2.x = (*it).x;
	  p2.y = (*it).y;
	  p2.z = (*it).z;
	  dis1 = sqrt(pow(start_p.x-p2.x,2)+pow(start_p.y-p2.y,2)+pow(start_p.z-p2.z,2));
	  if (dis1 > low_dis_limit) break;
	}
	if (dis1!=0){
	  start_p.x += (start_p.x-p2.x)/dis1*end_point_limit;
	  start_p.y += (start_p.y-p2.y)/dis1*end_point_limit;
	  start_p.z += (start_p.z-p2.z)/dis1*end_point_limit;
	}
      }
      start_v->set_fit_pt(start_p);
    }else{
      start_p = start_v->get_fit_pt();
    }

    if (!end_v->get_flag_fit_fix()){
      // end_point
      end_p.x = temp_wcps_vec.back().x;
      end_p.y = temp_wcps_vec.back().y;
      end_p.z = temp_wcps_vec.back().z;
      if (flag_endv_end){
	Point p2(temp_wcps_vec.back().x, temp_wcps_vec.back().y, temp_wcps_vec.back().z);
	double dis1 = 0;
	for (auto it = temp_wcps_vec.rbegin(); it!=temp_wcps_vec.rend(); it++){
	  p2.x = (*it).x;
	  p2.y = (*it).y;
	  p2.z = (*it).z;
	  dis1 = sqrt(pow(end_p.x-p2.x,2)+pow(end_p.y-p2.y,2)+pow(end_p.z-p2.z,2));
	  if (dis1 > low_dis_limit) break;
	}
	if (dis1!=0){
	  end_p.x += (end_p.x - p2.x)/dis1 * end_point_limit;
	  end_p.y += (end_p.y - p2.y)/dis1 * end_point_limit;
	  end_p.z += (end_p.z - p2.z)/dis1 * end_point_limit;
	}
      }
      end_v->set_fit_pt(end_p);
    }else{
      end_p = end_v->get_fit_pt();
    }

    //    std::cout << start_p << " " << end_p << std::endl;
    
    // middle points
    pts.push_back(start_p);
    for (size_t i=0;i!=temp_wcps_vec.size(); i++){
      Point p1(temp_wcps_vec.at(i).x, temp_wcps_vec.at(i).y, temp_wcps_vec.at(i).z);
      double dis = low_dis_limit;
      double dis1 = sqrt(pow(p1.x-end_p.x,2) + pow(p1.y-end_p.y,2) + pow(p1.z-end_p.z,2));
      if (pts.size()>0)
	dis = sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2));

      if (dis1 < low_dis_limit * 0.8){
	continue;
      }else if (dis < low_dis_limit * 0.8 ){
	continue;
      }else if (dis < low_dis_limit * 1.6){
	pts.push_back(p1);
	//std::cout << p1 << " " << sqrt(pow(p1.x-pts.back().x,2)+pow(p1.y-pts.back().y,2)+pow(p1.z-pts.back().z,2))/units::cm << std::endl;
      }else{
	int npoints = std::round(dis/low_dis_limit);
	
	Point p_save = pts.back();
	for (int j=0;j!=npoints;j++){
	  Point p(p_save.x + (p1.x-p_save.x) / npoints * (j+1),
		  p_save.y + (p1.y-p_save.y) / npoints * (j+1),
		  p_save.z + (p1.z-p_save.z) / npoints * (j+1));
	  pts.push_back(p);
	  //	std::cout << p << " " << sqrt(pow(p.x-pts.back().x,2)+pow(p.y-pts.back().y,2)+pow(p.z-pts.back().z,2))/units::cm << std::endl;
	}
	
      }
    }

    {
      double dis1 = sqrt(pow(pts.back().x-end_p.x,2) + pow(pts.back().y-end_p.y,2) + pow(pts.back().z-end_p.z,2));
      if (dis1 < low_dis_limit * 0.2) {
	if (pts.size()>1) pts.pop_back();
      }else if (dis1 > low_dis_limit * 1.6){
	
	int npoints = std::round(dis1/low_dis_limit);
	Point p_save = pts.back();
	for (int j=0;j+1<npoints;j++){
	  Point p(p_save.x + (end_p.x-p_save.x) / npoints * (j+1),
		  p_save.y + (end_p.y-p_save.y) / npoints * (j+1),
		  p_save.z + (end_p.z-p_save.z) / npoints * (j+1));
	  pts.push_back(p);
	}
	
	//Point tmp_p;
	//tmp_p.x =  pts.back().x + (end_p.x - pts.back().x )/2.;
	//tmp_p.y =  pts.back().y + (end_p.y - pts.back().y )/2.;
	//tmp_p.z =  pts.back().z + (end_p.z - pts.back().z )/2.;
	//pts.push_back(tmp_p);
      }
      pts.push_back(end_p);
    }

    sg->set_point_vec(pts);

    //    std::cout << sg->get_id() << " " << sg->get_point_vec().size() << " " << pts.front() << " " << pts.back() << std::endl;
    //    std::cout << sg << flag_startv_end << " " << flag_endv_end << std::endl;
    //    std::cout << sg << " " << start_v << " " << end_v << std::endl;
  }
}
    


void WCPPID::PR3DCluster::collect_charge_multi_trajectory(WCPPID::Map_Proto_Segment_Vertices& map_segment_vertices, WCP::ToyCTPointCloud& ct_point_cloud, double dis_cut, double range_cut){
  //clear up ...
  collected_charge_map.clear();
  
  std::set<std::pair<int,int>> existing_tcs;

  // form a set cotaining everything inside the cluster
  for (auto it = mcells.begin(); it!=mcells.end(); it++){
    SlimMergeGeomCell *mcell = (*it);
    int time_slice = mcell->GetTimeSlice();
    GeomWireSelection& uwires = mcell->get_uwires();
    GeomWireSelection& vwires = mcell->get_vwires();
    GeomWireSelection& wwires = mcell->get_wwires();
    for (auto it1 = uwires.begin(); it1!=uwires.end(); it1++){
      const GeomWire *wire = (*it1);
      existing_tcs.insert(std::make_pair(time_slice,wire->channel()));
    }
    for (auto it1 = vwires.begin(); it1!=vwires.end(); it1++){
      const GeomWire *wire = (*it1);
      existing_tcs.insert(std::make_pair(time_slice,wire->channel()));
    }
    for (auto it1 = wwires.begin(); it1!=wwires.end(); it1++){
      const GeomWire *wire = (*it1);
      existing_tcs.insert(std::make_pair(time_slice,wire->channel()));
    }
  }


  for (auto it2 = map_segment_vertices.begin(); it2 != map_segment_vertices.end(); it2++){
    WCPPID::ProtoSegment* sg1 = it2->first;
    if (sg1->get_cluster_id() != cluster_id) continue;
    // form a trajectory according to dis and fine tracking?
    PointVector traj_pts;
    //  PointVector& pts = get_fine_tracking_path();
    
    // not using fine tracking results ... 
    PointVector pts;
    std::vector<WCPointCloud<double>::WCPoint>& path_wcps = sg1->get_wcpt_vec();
    for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
      Point p((*it).x, (*it).y, (*it).z);
      pts.push_back(p);
    }

     //std::cout << "trajectory points " << pts.size() << std::endl;

    // if (cluster_id==13){
    //   for (int i=0; i!=pts.size(); i++){
    //     std::cout << i << " " << pts.at(i).x/units::cm << " " << pts.at(i).y/units::cm << " "<< pts.at(i).z/units::cm << std::endl;
    //   }
    // }

    for (int i=0; i!=pts.size(); i++){
      if (pts.at(i).y <-120*units::cm || pts.at(i).y > 120*units::cm ||
	  pts.at(i).z < -5*units::cm || pts.at(i).z > 1070*units::cm) continue;
      
      if (traj_pts.size()==0){
	traj_pts.push_back(pts.at(i));
      }else{
	double dis = sqrt(pow(pts.at(i).x-pts.at(i-1).x,2) +
			  pow(pts.at(i).y-pts.at(i-1).y,2) +
			  pow(pts.at(i).z-pts.at(i-1).z,2));
	if (dis <= dis_cut){
	  traj_pts.push_back(pts.at(i));
	}else{
	  int nseg = dis / dis_cut + 1;
	  //	std::cout << i << " " << pts.at(i).x/units::cm << " " << pts.at(i).y/units::cm << " "<< pts.at(i).z/units::cm << " " << dis << " " << dis_cut << " " << nseg << std::endl;
	  for (int j=0; j!=nseg;j++){
	    Point temp_pt;
	    temp_pt.x = pts.at(i-1).x + (pts.at(i).x-pts.at(i-1).x) *(j+1.)/nseg;
	    temp_pt.y = pts.at(i-1).y + (pts.at(i).y-pts.at(i-1).y) *(j+1.)/nseg;
	    temp_pt.z = pts.at(i-1).z + (pts.at(i).z-pts.at(i-1).z) *(j+1.)/nseg;
	    traj_pts.push_back(temp_pt);
	  }
	}
      }
    }

    // collect the nearby points, and compare with existing maps
    for (size_t i=0;i!=traj_pts.size();i++){
      //std::cout << i << " " << traj_pts.at(i).x/units::cm << " " << traj_pts.at(i).y/units::cm << " " << traj_pts.at(i).z/units::cm << " " << range_cut << std::endl;
      WCP::CTPointCloud<double> nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i),range_cut,0);
      
      //    std::cout << "0 " << nearby_points.pts.size() << std::endl;
      
      for (size_t j=0;j!=nearby_points.pts.size();j++){
	if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)) == existing_tcs.end()){
	  collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge,nearby_points.pts.at(j).charge_err);
	}
      }
      nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i),range_cut,1);
      //std::cout << "1 " << nearby_points.pts.size() << std::endl;
      
      for (size_t j=0;j!=nearby_points.pts.size();j++){
	if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)) == existing_tcs.end()){
	  collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge,nearby_points.pts.at(j).charge_err);
	}
      }
      nearby_points = ct_point_cloud.get_closest_points(traj_pts.at(i),range_cut,2);
      
      //    std::cout << "2 " << nearby_points.pts.size() << std::endl;
      
      for (size_t j=0;j!=nearby_points.pts.size();j++){
	if (existing_tcs.find(std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)) == existing_tcs.end()){
	  collected_charge_map[std::make_pair(nearby_points.pts.at(j).time_slice,nearby_points.pts.at(j).channel)] = std::make_pair(nearby_points.pts.at(j).charge,nearby_points.pts.at(j).charge_err);
	}
      }
    }
    
    //    std::cout << collected_charge_map.size() << std::endl;
    
  }

  
}



