#include "WCPPID/ProtoSegment.h"
#include "WCPData/TPCParams.h"
#include "WCPData/Singleton.h"
#include "WCPData/Line.h"

#include "TH1F.h"

using namespace WCP;

int WCPPID::ProtoSegment::get_particle_type(){
  if (get_flag_shower()){
    particle_type = 11; // shower are all treated as electron
  }
  return particle_type;
}

WCPPID::ProtoSegment::ProtoSegment(int id, std::list<WCP::WCPointCloud<double>::WCPoint >& path_wcps, int cluster_id )
  : id(id)
  , cluster_id(cluster_id)
  , flag_fit(false)
  , pcloud_fit(0)
  , pcloud_associated(0)
  , pcloud_associated_steiner(0)
  , flag_shower_trajectory(false)
  , flag_shower_topology(false)
  , flag_avoid_muon_check(false)
  , flag_dir(0)
  , dir_weak(false)
  , particle_type(0)
  , particle_score(100)
  , particle_mass(0)
  , kenergy_charge(0)
  , kenergy_best(0)
{
  for (int i=0;i!=4;i++){
    particle_4mom[i] = 0;
  }
  
  for (auto it = path_wcps.begin(); it!=path_wcps.end(); it++){
    wcpt_vec.push_back(*it);
    Point p;
    p.x = wcpt_vec.back().x;
    p.y = wcpt_vec.back().y;
    p.z = wcpt_vec.back().z;
    fit_pt_vec.push_back(p);
  }

  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
  
}

void WCPPID::ProtoSegment::build_pcloud_fit(){
  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
}

WCPPID::ProtoSegment::~ProtoSegment(){
  if (pcloud_fit != (ToyPointCloud*)0)
    delete pcloud_fit;
  if (pcloud_associated != (ToyPointCloud*)0)
    delete pcloud_associated;
  if (pcloud_associated_steiner != (ToyPointCloud*)0)
    delete pcloud_associated_steiner;
}


bool WCPPID::ProtoSegment::determine_shower_direction(){
  flag_dir = 0;
  // look at the points ...
  std::vector<PointVector > local_points_vec(fit_pt_vec.size());
  std::vector<std::tuple<double, double, double> > vec_rms_vals(fit_pt_vec.size(), std::make_tuple(0,0,0));
  std::vector<double> vec_dQ_dx(fit_pt_vec.size(), 0);
  std::vector<TVector3> vec_dir(fit_pt_vec.size());
  
  if (pcloud_associated == 0 ) return false;
 
  WCP::WCPointCloud<double>& cloud = pcloud_associated->get_cloud();

  for (size_t i = 0; i!= cloud.pts.size(); i++){
    Point test_p(cloud.pts.at(i).x, cloud.pts.at(i).y, cloud.pts.at(i).z);
    std::vector<std::pair<size_t,double>> results = pcloud_fit->get_closest_index(test_p,1);
    local_points_vec.at(results.front().first).push_back(test_p);
  }

  TVector3 drift_dir(1,0,0); // drift direction
  
  for (size_t i=0;i!=local_points_vec.size();i++){
    TVector3 dir_1, dir_2, dir_3;
    Point v1(0,0,0);
    for (size_t j=1;j!=3;j++){
      if (i+j<local_points_vec.size()){
	v1.x += fit_pt_vec.at(i+j).x - fit_pt_vec.at(i).x;
	v1.y += fit_pt_vec.at(i+j).y - fit_pt_vec.at(i).y;
	v1.z += fit_pt_vec.at(i+j).z - fit_pt_vec.at(i).z;
      }
      if (i>=j){
	v1.x += fit_pt_vec.at(i).x - fit_pt_vec.at(i-j).x;
	v1.y += fit_pt_vec.at(i).y - fit_pt_vec.at(i-j).y;
	v1.z += fit_pt_vec.at(i).z - fit_pt_vec.at(i-j).z;
      }
    }
    dir_1.SetXYZ(v1.x, v1.y, v1.z);
    dir_1 = dir_1.Unit();
    vec_dir.at(i) = dir_1;
    
    
    if (dir_1.Angle(drift_dir)/3.1415926*180. < 7.5){
      dir_1.SetXYZ(1,0,0);
      dir_2.SetXYZ(0,1,0);
      dir_3.SetXYZ(0,0,1);
    }else{
      dir_2 = drift_dir.Cross(dir_1);
      dir_2 = dir_2.Unit();
      dir_3 = dir_1.Cross(dir_2);
    }
    
    std::vector<std::tuple<double, double, double> > vec_projs;
    for (size_t j=0;j!=local_points_vec.at(i).size();j++){
      double proj_1 = dir_1.X() * local_points_vec.at(i).at(j).x + dir_1.Y() * local_points_vec.at(i).at(j).y + dir_1.Z() * local_points_vec.at(i).at(j).z;
      double proj_2 = dir_2.X() * local_points_vec.at(i).at(j).x + dir_2.Y() * local_points_vec.at(i).at(j).y + dir_2.Z() * local_points_vec.at(i).at(j).z;
      double proj_3 = dir_3.X() * local_points_vec.at(i).at(j).x + dir_3.Y() * local_points_vec.at(i).at(j).y + dir_3.Z() * local_points_vec.at(i).at(j).z;
      vec_projs.push_back(std::make_tuple(proj_1, proj_2, proj_3));
    }
    std::tuple<double, double, double> means = std::make_tuple(0,0,0);
    int ncount = local_points_vec.at(i).size();
    if (ncount >1){
      std::get<0>(means) = dir_1.X() * fit_pt_vec.at(i).x + dir_1.Y() * fit_pt_vec.at(i).y + dir_1.Z() * fit_pt_vec.at(i).z;
      std::get<1>(means) = dir_2.X() * fit_pt_vec.at(i).x + dir_2.Y() * fit_pt_vec.at(i).y + dir_2.Z() * fit_pt_vec.at(i).z;
      std::get<2>(means) = dir_3.X() * fit_pt_vec.at(i).x + dir_3.Y() * fit_pt_vec.at(i).y + dir_3.Z() * fit_pt_vec.at(i).z;
      
      for (size_t j=0;j!=local_points_vec.at(i).size();j++){
	std::get<0>(vec_rms_vals.at(i)) += pow(std::get<0>(vec_projs.at(j)) - std::get<0>(means),2);
	std::get<1>(vec_rms_vals.at(i)) += pow(std::get<1>(vec_projs.at(j)) - std::get<1>(means),2);
	std::get<2>(vec_rms_vals.at(i)) += pow(std::get<2>(vec_projs.at(j)) - std::get<2>(means),2);
      }

      std::get<0>(vec_rms_vals.at(i)) = sqrt(std::get<0>(vec_rms_vals.at(i))*1./(ncount));
      std::get<1>(vec_rms_vals.at(i)) = sqrt(std::get<1>(vec_rms_vals.at(i))*1./(ncount));
      std::get<2>(vec_rms_vals.at(i)) = sqrt(std::get<2>(vec_rms_vals.at(i))*1./(ncount));
    }
    vec_dQ_dx.at(i) = dQ_vec.at(i)/(dx_vec.at(i)+1e-9)/43e3*units::cm;
  }

  double max_spread = 0;
  double large_spread_length = 0;
  double total_effective_length = 0;

  double max_cont_length = 0;
  double max_cont_weighted_length = 0;
  
  double cont_length = 0;
  double cont_weighted_length = 0;

  bool flag_prev = false;
  double total_length;
  for (size_t i=0;i+1<local_points_vec.size();i++){
    double length = sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x,2) + pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2) + pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
    //std::cout << i << " " << std::get<2>(vec_rms_vals.at(i))/units::cm << std::endl;
    total_length += length;
    if (std::get<2>(vec_rms_vals.at(i))!=0){
      total_effective_length += length;
      if (std::get<2>(vec_rms_vals.at(i)) > 0.4*units::cm) {
	large_spread_length += length;

	cont_length += length;
	cont_weighted_length += length * std::get<2>(vec_rms_vals.at(i));
	flag_prev = true;
      }else{
	if (flag_prev && cont_length > max_cont_length){
	  max_cont_length = cont_length;
	  max_cont_weighted_length = cont_weighted_length;
	}
	cont_length = 0;
	cont_weighted_length = 0;
	flag_prev = false;
      }
      if (std::get<2>(vec_rms_vals.at(i)) > max_spread) max_spread = std::get<2>(vec_rms_vals.at(i));
    }    
  }
  

  if (max_spread > 0.7*units::cm && large_spread_length > 0.2 * total_effective_length && total_effective_length > 3*units::cm && total_effective_length < 15*units::cm && ( large_spread_length > 2.7*units::cm || large_spread_length > 0.35 * total_effective_length)
      || max_spread > 0.8*units::cm && large_spread_length > 0.3 * total_effective_length && total_effective_length >= 15*units::cm
      || max_spread > 1.0*units::cm && large_spread_length > 0.4 * total_effective_length) {
    TVector3 drift_dir(1,0,0);
    
    TVector3 main_dir1, main_dir2;
    bool flag_skip_angle1 = false;
    bool flag_skip_angle2 = false;
    if (fit_pt_vec.front().z < fit_pt_vec.back().z){
      main_dir1 = cal_dir_3vector(fit_pt_vec.front(), 15*units::cm);
      main_dir2 = cal_dir_3vector(fit_pt_vec.back(), 6*units::cm);
      if (fabs(main_dir1.Angle(drift_dir)/3.1415926*180.-90)<10) flag_skip_angle1 = true;
    }else{
      main_dir1 = cal_dir_3vector(fit_pt_vec.front(), 6*units::cm);
      main_dir2 = cal_dir_3vector(fit_pt_vec.back(), 15*units::cm);
      if (fabs(main_dir2.Angle(drift_dir)/3.1415926*180.-90)<10) flag_skip_angle2 = true;
    }

   
    //    std::cout << main_dir.Angle(drift_dir)/3.1415926*180. << std::endl;
    
    std::vector<std::tuple<int, int, double> > threshold_segs;
    for(size_t i=0;i!=vec_dQ_dx.size(); i++){
      //      std::cout << i << " A: " << main_dir.Angle(vec_dir.at(i))/3.1415926*180. << " " <<vec_dir.at(i).Angle(drift_dir)/3.1415926*180. << " " << vec_dQ_dx.at(i) << " " << std::get<2>(vec_rms_vals.at(i))/units::cm << std::endl;
      double angle = main_dir1.Angle(vec_dir.at(i))/3.1415926*180.;
      if (threshold_segs.size()==0 && (angle < 30 || flag_skip_angle1 && angle < 60) && (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4 || vec_dQ_dx.at(i) > 1.6)){
	threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
      }else if ((angle < 30 || flag_skip_angle1 && angle < 60) && (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4 || vec_dQ_dx.at(i) > 1.6)){
	if (i == std::get<1>(threshold_segs.back())+1 && std::get<2>(threshold_segs.back()) < std::get<2>(vec_rms_vals.at(i)) ){
	  std::get<1>(threshold_segs.back()) = i;
	  std::get<2>(threshold_segs.back()) = std::get<2>(vec_rms_vals.at(i));
	}else{
	  if (i!=std::get<1>(threshold_segs.back())+1)
	    threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
	}
      }
      // loop ...
    }
    
    double total_length1 = 0, max_length1 = 0;
    for (auto it = threshold_segs.begin(); it!= threshold_segs.end(); it++){
      int start_n = std::get<0>(*it);
      if (start_n >0) start_n --;
      int end_n = std::get<1>(*it);
      double tmp_length = 0;
      for (int i=start_n; i!=end_n;i++){
	tmp_length += sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x,2)+pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2)+pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
      }
      total_length1 += tmp_length;
      if (tmp_length > max_length1) max_length1 = tmp_length;
      //      std::cout << id << " A: " << start_n << " " << end_n << " " << tmp_length/units::cm << std::endl;
    }
    
    
    threshold_segs.clear();
    for(int i=vec_dQ_dx.size()-1;i>=0; i--){
      // std::cout << "B: " << 180-main_dir.Angle(vec_dir.at(i))/3.1415926*180. << std::endl;
      double angle = 180-main_dir2.Angle(vec_dir.at(i))/3.1415926*180.;
      if (threshold_segs.size()==0 && (angle  < 30 || flag_skip_angle2 && angle < 60) && (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4 || vec_dQ_dx.at(i) > 1.6)){
	threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
      }else if ( (angle < 30 || flag_skip_angle2 && angle < 60) && (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4 || vec_dQ_dx.at(i) > 1.6)){
	if (i == std::get<1>(threshold_segs.back())-1 && std::get<2>(threshold_segs.back()) < std::get<2>(vec_rms_vals.at(i)) ){
	  std::get<1>(threshold_segs.back()) = i;
	  std::get<2>(threshold_segs.back()) = std::get<2>(vec_rms_vals.at(i));
	}else{
	  if (i!=std::get<1>(threshold_segs.back())-1)
	    threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
	}
      }
      // loop ...
    }
    
    
    double total_length2 = 0, max_length2 = 0;
    for (auto it = threshold_segs.begin(); it!= threshold_segs.end(); it++){
      int start_n = std::get<0>(*it);
      if (start_n <fit_pt_vec.size()-1) start_n ++;
      int end_n = std::get<1>(*it);
      double tmp_length = 0;
      for (int i=start_n; i!=end_n;i--){
	tmp_length+= sqrt(pow(fit_pt_vec.at(i-1).x - fit_pt_vec.at(i).x,2)+pow(fit_pt_vec.at(i-1).y - fit_pt_vec.at(i).y,2)+pow(fit_pt_vec.at(i-1).z - fit_pt_vec.at(i).z,2));
      }
      total_length2 += tmp_length;
      if (tmp_length > max_length2) max_length2 = tmp_length;
      //       std::cout << id << " B: " << start_n << " " << end_n << " " << tmp_length/units::cm << std::endl;
    }
    
    if (total_length1 + max_length1 > 1.1*(total_length2 + max_length2)){
      flag_dir = 1;
    }else if (1.1*(total_length1 + max_length1) < total_length2 + max_length2){
      flag_dir = -1;
    }
    
    //    std::cout << cluster_id << " " << id << " " << total_length1/units::cm << " " << total_length2/units::cm << " " << max_length1/units::cm << " " << max_length2/units::cm << " " << flag_dir << std::endl;
  }else{
    if (total_length < 5*units::cm){
      if (!is_shower_trajectory())
	determine_dir_track(0, fit_pt_vec.size());
    }else{
      
      TVector3 main_dir = cal_dir_3vector(fit_pt_vec.front(),6*units::cm);
      Int_t ncount_front = 0, ncount_back = 0;
      for(size_t i=0;i!=vec_dQ_dx.size(); i++){
	if (main_dir.Angle(vec_dir.at(i))/3.1415926*180. < 30) ncount_front ++;
	  //	std::cout << i << " A: " << main_dir.Angle(vec_dir.at(i))/3.1415926*180. << std::endl;
      }
      main_dir = cal_dir_3vector(fit_pt_vec.back(),6*units::cm);
      for(int i=vec_dQ_dx.size()-1;i>=0; i--){
	if (180-main_dir.Angle(vec_dir.at(i))/3.1415926*180. < 30) ncount_back++;
	//	std::cout << "B: " << 180-main_dir.Angle(vec_dir.at(i))/3.1415926*180. << std::endl;
      }
      if (1.2 * ncount_front  < ncount_back){
	flag_dir = -1;
      }else if (ncount_front > 1.2 * ncount_back){
	flag_dir = 1;
      }
      // std::cout << id << " " << ncount_front << " " << ncount_back << std::endl;
       
    }
  }
  
  if (flag_dir !=0) return true;
  else return false;
  
  
    
   
}


bool WCPPID::ProtoSegment::is_shower_topology(bool tmp_val){
  flag_shower_topology = tmp_val;
  flag_dir = 0;
  
  // look at the points ...
  std::vector<PointVector > local_points_vec(fit_pt_vec.size());
  std::vector<std::tuple<double, double, double> > vec_rms_vals(fit_pt_vec.size(), std::make_tuple(0,0,0));
  std::vector<double> vec_dQ_dx(fit_pt_vec.size(), 0);

  //  std::cout << fit_pt_vec.size() << " " << pcloud_associated << " " << std::endl;
  if (pcloud_associated == 0 ) return false;
  
  WCP::WCPointCloud<double>& cloud = pcloud_associated->get_cloud();

  for (size_t i = 0; i!= cloud.pts.size(); i++){
    Point test_p(cloud.pts.at(i).x, cloud.pts.at(i).y, cloud.pts.at(i).z);
    std::vector<std::pair<size_t,double>> results = pcloud_fit->get_closest_index(test_p,1);
    local_points_vec.at(results.front().first).push_back(test_p);
  }

  TVector3 drift_dir(1,0,0); // drift direction
  
  for (size_t i=0;i!=local_points_vec.size();i++){
    TVector3 dir_1, dir_2, dir_3;
    Point v1(0,0,0);
    for (size_t j=1;j!=3;j++){
      if (i+j<local_points_vec.size()){
	v1.x += fit_pt_vec.at(i+j).x - fit_pt_vec.at(i).x;
	v1.y += fit_pt_vec.at(i+j).y - fit_pt_vec.at(i).y;
	v1.z += fit_pt_vec.at(i+j).z - fit_pt_vec.at(i).z;
      }
      if (i>=j){
	v1.x += fit_pt_vec.at(i).x - fit_pt_vec.at(i-j).x;
	v1.y += fit_pt_vec.at(i).y - fit_pt_vec.at(i-j).y;
	v1.z += fit_pt_vec.at(i).z - fit_pt_vec.at(i-j).z;
      }
    }
    dir_1.SetXYZ(v1.x, v1.y, v1.z);
    dir_1 = dir_1.Unit();

    if (dir_1.Angle(drift_dir)/3.1415926*180. < 7.5){
      dir_1.SetXYZ(1,0,0);
      dir_2.SetXYZ(0,1,0);
      dir_3.SetXYZ(0,0,1);
    }else{
      dir_2 = drift_dir.Cross(dir_1);
      dir_2 = dir_2.Unit();
      dir_3 = dir_1.Cross(dir_2);
    }
    
    std::vector<std::tuple<double, double, double> > vec_projs;
    for (size_t j=0;j!=local_points_vec.at(i).size();j++){
      double proj_1 = dir_1.X() * local_points_vec.at(i).at(j).x + dir_1.Y() * local_points_vec.at(i).at(j).y + dir_1.Z() * local_points_vec.at(i).at(j).z;
      double proj_2 = dir_2.X() * local_points_vec.at(i).at(j).x + dir_2.Y() * local_points_vec.at(i).at(j).y + dir_2.Z() * local_points_vec.at(i).at(j).z;
      double proj_3 = dir_3.X() * local_points_vec.at(i).at(j).x + dir_3.Y() * local_points_vec.at(i).at(j).y + dir_3.Z() * local_points_vec.at(i).at(j).z;
      vec_projs.push_back(std::make_tuple(proj_1, proj_2, proj_3));
    }
    std::tuple<double, double, double> means = std::make_tuple(0,0,0);
    int ncount = local_points_vec.at(i).size();
    //for (size_t j=0;j!=local_points_vec.at(i).size();j++){
    //  std::get<0>(means) += std::get<0>(vec_projs.at(j));
    //  std::get<1>(means) += std::get<1>(vec_projs.at(j));
    //  std::get<2>(means) += std::get<2>(vec_projs.at(j));
    //  ncount ++;
    //}
    if (ncount >1){
      //std::get<0>(means) /= 1.*ncount;
      //std::get<1>(means) /= 1.*ncount;
      //std::get<2>(means) /= 1.*ncount;

      std::get<0>(means) = dir_1.X() * fit_pt_vec.at(i).x + dir_1.Y() * fit_pt_vec.at(i).y + dir_1.Z() * fit_pt_vec.at(i).z;
      std::get<1>(means) = dir_2.X() * fit_pt_vec.at(i).x + dir_2.Y() * fit_pt_vec.at(i).y + dir_2.Z() * fit_pt_vec.at(i).z;
      std::get<2>(means) = dir_3.X() * fit_pt_vec.at(i).x + dir_3.Y() * fit_pt_vec.at(i).y + dir_3.Z() * fit_pt_vec.at(i).z;
      
      for (size_t j=0;j!=local_points_vec.at(i).size();j++){
	std::get<0>(vec_rms_vals.at(i)) += pow(std::get<0>(vec_projs.at(j)) - std::get<0>(means),2);
	std::get<1>(vec_rms_vals.at(i)) += pow(std::get<1>(vec_projs.at(j)) - std::get<1>(means),2);
	std::get<2>(vec_rms_vals.at(i)) += pow(std::get<2>(vec_projs.at(j)) - std::get<2>(means),2);
      }

      std::get<0>(vec_rms_vals.at(i)) = sqrt(std::get<0>(vec_rms_vals.at(i))*1./(ncount));
      std::get<1>(vec_rms_vals.at(i)) = sqrt(std::get<1>(vec_rms_vals.at(i))*1./(ncount));
      std::get<2>(vec_rms_vals.at(i)) = sqrt(std::get<2>(vec_rms_vals.at(i))*1./(ncount));
    }
    vec_dQ_dx.at(i) = dQ_vec.at(i)/(dx_vec.at(i)+1e-9)/43e3*units::cm;
    //std::cout << dir_1.Angle(drift_dir)/3.1415926*180. << " " << dir_1.Mag() << " " << v1 << std::endl;
    //    std::cout << i << " " << dQ_vec.at(i)/(dx_vec.at(i)+1e-9)/43e3*units::cm << " " << std::get<0>(vec_rms_vals.at(i))/units::cm << " " << std::get<1>(vec_rms_vals.at(i))/units::cm << " " << std::get<2>(vec_rms_vals.at(i))/units::cm << std::endl;
  }

  double max_spread = 0;
  double large_spread_length = 0;
  double total_effective_length = 0;

  double max_cont_length = 0;
  double max_cont_weighted_length = 0;
  
  double cont_length = 0;
  double cont_weighted_length = 0;

  bool flag_prev = false;
  
  for (size_t i=0;i+1<local_points_vec.size();i++){
    double length = sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x,2) + pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2) + pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
    if (std::get<2>(vec_rms_vals.at(i))!=0){
      total_effective_length += length;
      if (std::get<2>(vec_rms_vals.at(i)) > 0.4*units::cm) {
	large_spread_length += length;

	cont_length += length;
	cont_weighted_length += length * std::get<2>(vec_rms_vals.at(i));
	flag_prev = true;
      }else{
	if (flag_prev && cont_length > max_cont_length){
	  max_cont_length = cont_length;
	  max_cont_weighted_length = cont_weighted_length;
	}
	cont_length = 0;
	cont_weighted_length = 0;
	flag_prev = false;
      }
      if (std::get<2>(vec_rms_vals.at(i)) > max_spread) max_spread = std::get<2>(vec_rms_vals.at(i));
    }
  }


  
  if (max_spread > 0.7*units::cm && large_spread_length > 0.2 * total_effective_length && total_effective_length > 3*units::cm && total_effective_length < 15*units::cm && ( large_spread_length > 2.7*units::cm || large_spread_length > 0.35 * total_effective_length)
      || max_spread > 0.8*units::cm && large_spread_length > 0.3 * total_effective_length && total_effective_length >= 15*units::cm
      || max_spread > 0.8*units::cm && large_spread_length > 8*units::cm && large_spread_length > 0.18 * total_effective_length
      || max_spread > 1.0*units::cm && large_spread_length > 0.4 * total_effective_length
      || max_spread > 1.0*units::cm && large_spread_length > 5*units::cm && large_spread_length > 0.23 * total_effective_length
      ){

  
    flag_shower_topology = true;
      
  }
  //  std::cout << cluster_id << " " << id << " " << max_spread/units::cm << " " << large_spread_length/units::cm <<  " " << total_effective_length/units::cm << std::endl;
  
  
  if (flag_shower_topology){
    std::vector<std::tuple<int, int, double> > threshold_segs;
    for(size_t i=0;i!=vec_dQ_dx.size(); i++){
      if (threshold_segs.size()==0 && std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4){
	threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
      }else if (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4){
	if (i == std::get<1>(threshold_segs.back())+1 && std::get<2>(threshold_segs.back()) < std::get<2>(vec_rms_vals.at(i)) ){
	  std::get<1>(threshold_segs.back()) = i;
	  std::get<2>(threshold_segs.back()) = std::get<2>(vec_rms_vals.at(i));
	}else{
	  if (i!=std::get<1>(threshold_segs.back())+1)
	    threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
	}
      }
      // loop ...
    }

    double total_length1 = 0, max_length1 = 0;
    for (auto it = threshold_segs.begin(); it!= threshold_segs.end(); it++){
      int start_n = std::get<0>(*it);
      if (start_n >0) start_n --;
      int end_n = std::get<1>(*it);
      double tmp_length = 0;
      for (int i=start_n; i!=end_n;i++){
        tmp_length += sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x,2)+pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2)+pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
      }
      total_length1 += tmp_length;
      if (tmp_length > max_length1) max_length1 = tmp_length;
      //      std::cout << id << " A: " << start_n << " " << end_n << " " << tmp_length/units::cm << std::endl;
    }


    threshold_segs.clear();
    for(int i=vec_dQ_dx.size()-1;i>=0; i--){
      if (threshold_segs.size()==0 && std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4){
	threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
      }else if (std::get<2>(vec_rms_vals.at(i))/units::cm> 0.4){
	if (i == std::get<1>(threshold_segs.back())-1 && std::get<2>(threshold_segs.back()) < std::get<2>(vec_rms_vals.at(i)) ){
	  std::get<1>(threshold_segs.back()) = i;
	  std::get<2>(threshold_segs.back()) = std::get<2>(vec_rms_vals.at(i));
	}else{
	  if (i!=std::get<1>(threshold_segs.back())-1)
	    threshold_segs.push_back(std::make_tuple(i,i,std::get<2>(vec_rms_vals.at(i))));
	}
      }
      // loop ...
    }

    
    double total_length2 = 0, max_length2 = 0;
    for (auto it = threshold_segs.begin(); it!= threshold_segs.end(); it++){
       int start_n = std::get<0>(*it);
       if (start_n <fit_pt_vec.size()-1) start_n ++;
       int end_n = std::get<1>(*it);
       double tmp_length = 0;
       for (int i=start_n; i!=end_n;i--){
	 tmp_length+= sqrt(pow(fit_pt_vec.at(i-1).x - fit_pt_vec.at(i).x,2)+pow(fit_pt_vec.at(i-1).y - fit_pt_vec.at(i).y,2)+pow(fit_pt_vec.at(i-1).z - fit_pt_vec.at(i).z,2));
       }
       total_length2 += tmp_length;
       if (tmp_length > max_length2) max_length2 = tmp_length;
       //       std::cout << id << " B: " << start_n << " " << end_n << " " << tmp_length/units::cm << std::endl;
    }

    if (total_length1 + max_length1 > 1.1*(total_length2 + max_length2)){
      flag_dir = 1;
    }else if (1.1*(total_length1 + max_length1) < total_length2 + max_length2){
      flag_dir = -1;
    }
    double tmp_total_length = get_length();
    if (tmp_total_length > 50*units::cm && total_length1 < 0.25* tmp_total_length && total_length2 < 0.25* tmp_total_length ){
      flag_dir = 0;
      flag_shower_topology = false;
    }
	
    //    std::cout << cluster_id << " " << id << " " << total_length1/units::cm << " " << total_length2/units::cm << " " << max_length1/units::cm << " " << max_length2/units::cm << " " << flag_dir << " " << get_length()/units::cm << std::endl;
    
  }
  
  //  std::cout << cluster_id << " "  << id << " " << max_spread/units::cm << " " << large_spread_length/total_effective_length << " " << max_cont_length/units::cm << " " << total_effective_length/units::cm << " " << flag_shower_trajectory << " " << flag_shower_topology << std::endl;
  
  return flag_shower_topology;
}

bool WCPPID::ProtoSegment::is_shower_trajectory(double step_size){
  flag_shower_trajectory = false;
  double length = get_length();
  // too long
  if (length > 50*units::cm) return flag_shower_trajectory;
  
  int ncount = std::round(length/step_size);
  if (ncount ==0 ) ncount = 1;

  std::vector<std::pair<int,int> > sections(ncount);
  for (size_t i=0;i!=ncount;i++){
    sections.at(i)=std::make_pair(std::round(fit_pt_vec.size()/ncount*i),std::round(fit_pt_vec.size()/ncount*(i+1)));
  }
  sections.back().second = fit_pt_vec.size()-1;

  int n_shower_like = 0;

  TVector3 drift_dir(1,0,0);
  
  // 
  for (size_t j=0;j!=ncount;j++){
    TVector3 dir_1(fit_pt_vec.at(sections.at(j).first).x - fit_pt_vec.at(sections.at(j).second).x, fit_pt_vec.at(sections.at(j).first).y - fit_pt_vec.at(sections.at(j).second).y, fit_pt_vec.at(sections.at(j).first).z - fit_pt_vec.at(sections.at(j).second).z);
    dir_1 = dir_1.Unit();
    double tmp_dQ_dx = get_medium_dQ_dx(sections.at(j).first, sections.at(j).second)/(50000/units::cm);

    //    std::cout << fabs(drift_dir.Angle(dir_1)/3.1415926*180.-90.) << std::endl;
    double angle_diff = fabs(drift_dir.Angle(dir_1)/3.1415926*180.-90.);
    if (angle_diff>10 ){ // not parallel case ...
      double direct_length = get_direct_length(sections.at(j).first, sections.at(j).second);
      double integrated_length = get_length(sections.at(j).first, sections.at(j).second);
      double max_dev = get_max_deviation(sections.at(j).first, sections.at(j).second);

      //      std::cout << max_dev/units::cm << " " << fabs(drift_dir.Angle(dir_1)/3.1415926*180.-90.) << std::endl;
      
      double length_ratio;
      if (direct_length == 0 ) length_ratio = 1;
      else length_ratio = direct_length / integrated_length;
    
      if (tmp_dQ_dx*0.11 + 2*length_ratio < 2.03 && tmp_dQ_dx < 2 && length_ratio < 0.95 && (angle_diff < 60 || integrated_length < 10*units::cm || integrated_length >= 10*units::cm && max_dev > 0.75*units::cm)
	  ) n_shower_like ++;
      //      std::cout << "Xin: " << id << " " << j << " " << sections.at(j).first << " " << sections.at(j).second <<  " " << length_ratio << " " << tmp_dQ_dx << " " << direct_length << " " << drift_dir.Angle(dir_1)/3.1415926*180. << std::endl;
    }else{
      TVector3 dir_2 = drift_dir.Cross(dir_1);
      dir_2 = dir_2.Unit();
      TVector3 dir_3 = dir_1.Cross(dir_2);
      double direct_length = get_direct_length(sections.at(j).first, sections.at(j).second, dir_2);
      double integrated_length = get_length(sections.at(j).first, sections.at(j).second, dir_2);
      double max_dev = get_max_deviation(sections.at(j).first, sections.at(j).second);
      double length_ratio;
      if (direct_length == 0 ) length_ratio = 1;
      else length_ratio = direct_length / integrated_length;
      if (tmp_dQ_dx*0.11 + 2*length_ratio < 2.06 && tmp_dQ_dx < 2 && length_ratio < 0.97 && (integrated_length < 10*units::cm || integrated_length >= 10*units::cm && max_dev > 0.75*units::cm) ) n_shower_like ++;
      // std::cout << "Xin: " << j << " " << sections.at(j).first << " " << sections.at(j).second <<  " " << length_ratio << " " << tmp_dQ_dx << " " << direct_length << " " << drift_dir.Angle(dir_1)/3.1415926*180. << " " << tmp_dQ_dx*0.11 + 2*length_ratio - 2 << std::endl;  
    }
  }

  
  
  //std::cout << "BB " << id << " " << sections.size() << " " << get_length()/units::cm << " " << n_shower_like << std::endl;
  //std::cout << id << " " << get_medium_dQ_dx()/(43e3/units::cm) << std::endl;

  //  double medium_dQ_dx = get_medium_dQ_dx()/(43e3/units::cm);
  // not a good idea to require dQ/dx ...
  if (n_shower_like >=0.5*sections.size()
      //&& medium_dQ_dx < 1.1
      ) flag_shower_trajectory = true;
  
  // calculate direct length, accumulated length, medium dQ/dx in each section ...
  //  std::cout << length/units::cm << " " << ncount << std::endl;
  return flag_shower_trajectory;
}

double WCPPID::ProtoSegment::get_max_deviation(int n1, int n2){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;
  
  double max_dev = 0;
  if (n1 != n2){
    WCP::Line l(fit_pt_vec.at(n1), fit_pt_vec.at(n2));
    for (size_t i=n1; i<=n2;i++){
      double dis = l.closest_dis(fit_pt_vec.at(i));
      if (dis > max_dev) max_dev = dis;
    }
  }
  
  return max_dev;
}

double WCPPID::ProtoSegment::get_direct_length(int n1, int n2){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;

  double length = sqrt(pow(fit_pt_vec.at(n1).x - fit_pt_vec.at(n2).x,2)+pow(fit_pt_vec.at(n1).y - fit_pt_vec.at(n2).y,2)+pow(fit_pt_vec.at(n1).z - fit_pt_vec.at(n2).z,2));
  return length;
}

double WCPPID::ProtoSegment::get_length(int n1, int n2){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;
  double length = 0;
  for (int i=n1;i+1<=n2;i++){
    length += sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x,2)+pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2)+pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
  }
  return length;
}

double WCPPID::ProtoSegment::get_direct_length(int n1, int n2, TVector3 dir_perp){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;

  TVector3 temp_dir(fit_pt_vec.at(n1).x - fit_pt_vec.at(n2).x, fit_pt_vec.at(n1).y - fit_pt_vec.at(n2).y, fit_pt_vec.at(n1).z - fit_pt_vec.at(n2).z);
  double length = sqrt(pow(temp_dir.Mag(),2) - pow(temp_dir.Dot(dir_perp.Unit()),2));
  return length;
}

double WCPPID::ProtoSegment::get_length(int n1, int n2, TVector3 dir_perp){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;
  
  double length = 0;
  for (int i=n1;i+1<=n2;i++){
    TVector3 temp_dir(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x, fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y, fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z);
    length += sqrt(pow(temp_dir.Mag(),2)-pow(temp_dir.Dot(dir_perp.Unit()),2));
  }
  return length;
}

double WCPPID::ProtoSegment::get_rms_dQ_dx(){
  std::vector<double> vec_dQ_dx;
  double sum[3]={0,0,0};
  if (dQ_vec.size()<=1) return 0;
  
  for (int i = 0 ; i< dQ_vec.size(); i++){
    vec_dQ_dx.push_back(dQ_vec.at(i)/(dx_vec.at(i)+1e-9));
    sum[0] += pow(vec_dQ_dx.back(),2);
    sum[1] += vec_dQ_dx.back();
    sum[2] += 1;
  }

  return sqrt((sum[0] - pow(sum[1],2)/sum[2])/(sum[2]-1.));
}

double WCPPID::ProtoSegment::get_medium_dQ_dx(){
  return get_medium_dQ_dx(0, fit_pt_vec.size());
}

double WCPPID::ProtoSegment::get_medium_dQ_dx(int n1, int n2){
  if (n1 < 0) n1 = 0;  if (n1+1 > fit_pt_vec.size()) n1 = int(fit_pt_vec.size())-1;
  if (n2 < 0) n2 = 0;  if (n2+1 > fit_pt_vec.size()) n2 = int(fit_pt_vec.size())-1;
  std::vector<double> vec_dQ_dx;
  for (int i = n1 ; i<=n2; i++){
    vec_dQ_dx.push_back(dQ_vec.at(i)/(dx_vec.at(i)+1e-9));
  }
  std::nth_element(vec_dQ_dx.begin(), vec_dQ_dx.begin() + vec_dQ_dx.size()/2, vec_dQ_dx.end());
  return *std::next(vec_dQ_dx.begin(), vec_dQ_dx.size()/2);
}

double WCPPID::ProtoSegment::get_direct_length(){
  double length =sqrt(pow(fit_pt_vec.front().x - fit_pt_vec.back().x ,2) + pow(fit_pt_vec.front().y - fit_pt_vec.back().y,2) + pow(fit_pt_vec.front().z - fit_pt_vec.back().z,2)); 
  
  return length;
}

double WCPPID::ProtoSegment::get_length(){
  double length = 0;
  for (size_t i=0;i+1<fit_pt_vec.size();i++){
    length += sqrt(pow(fit_pt_vec.at(i+1).x - fit_pt_vec.at(i).x ,2) + pow(fit_pt_vec.at(i+1).y - fit_pt_vec.at(i).y,2) + pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
  }
  return length;
}

std::tuple<WCP::Point, TVector3, TVector3, bool> WCPPID::ProtoSegment::search_kink(Point& start_p){
  // find the first point ...
  //  Point start_p = fit_pt_vec.front();
  std::pair<double, WCP::Point> tmp_results = get_closest_point(start_p);
  Point test_p = tmp_results.second;

  TVector3 drift_dir(1,0,0);

  std::vector<double> refl_angles(fit_pt_vec.size(),0);
  std::vector<double> para_angles(fit_pt_vec.size(),0);
  
  // start the angle search
 
  for (int i=0;i!=fit_pt_vec.size(); i++){
    double angle1 = 0;
    double angle2 = 0;
    
    for (int j=0;j!=6;j++){    
      TVector3 v10(0,0,0);
      TVector3 v20(0,0,0);

      //      std::cout << i << " " << j << std::endl;
      if (i >= (j+1)*2){
	v10.SetXYZ(fit_pt_vec.at(i).x - fit_pt_vec.at(i-(j+1)*2).x,
		   fit_pt_vec.at(i).y - fit_pt_vec.at(i-(j+1)*2).y,
		   fit_pt_vec.at(i).z - fit_pt_vec.at(i-(j+1)*2).z);
      }else{
	v10.SetXYZ(fit_pt_vec.at(i).x - fit_pt_vec.front().x,
		   fit_pt_vec.at(i).y - fit_pt_vec.front().y,
		   fit_pt_vec.at(i).z - fit_pt_vec.front().z);
      }
      
      if (i+(j+1)*2<fit_pt_vec.size()){
	v20.SetXYZ(fit_pt_vec.at(i+(j+1)*2).x - fit_pt_vec.at(i).x,
		   fit_pt_vec.at(i+(j+1)*2).y - fit_pt_vec.at(i).y,
		   fit_pt_vec.at(i+(j+1)*2).z - fit_pt_vec.at(i).z);
      }else{
	v20.SetXYZ(fit_pt_vec.back().x - fit_pt_vec.at(i).x,
		   fit_pt_vec.back().y - fit_pt_vec.at(i).y,
		   fit_pt_vec.back().z - fit_pt_vec.at(i).z);
      }
      
      if (j==0){
	angle1 = v10.Angle(v20)/3.1415926*180.;
	angle2 = std::max(fabs(v10.Angle(drift_dir)/3.1415926*180.-90.),
			  fabs(v20.Angle(drift_dir)/3.1415926*180.-90.));
      }else{
	if (v10.Mag()!=0 && v20.Mag()!=0){

	  // std::cout << i << " " << j << " " << angle1 << " " << angle2 << std::endl;
	  angle1 = std::min(v10.Angle(v20)/3.1415926*180., angle1);
	  angle2 = std::min(std::max(fabs(v10.Angle(drift_dir)/3.1415926*180.-90.),
				     fabs(v20.Angle(drift_dir)/3.1415926*180.-90.)),angle2);
	}
      }
    }
    
    refl_angles.at(i) = angle1;
    para_angles.at(i) = angle2;
  }

  bool flag_check = false;
  int save_i = -1;
  bool flag_switch = false;
  bool flag_search = false;
    
  for (int i=0;i!=fit_pt_vec.size();i++){
    if (sqrt(pow(test_p.x - fit_pt_vec.at(i).x,2) +
	     pow(test_p.y - fit_pt_vec.at(i).y,2) +
	     pow(test_p.z - fit_pt_vec.at(i).z,2) ) < 0.1*units::cm) flag_check = true;

    // not too close and not too far to the begin and end ...
    if (sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.front().x,2) +
	     pow(fit_pt_vec.at(i).y - fit_pt_vec.front().y,2) +
	     pow(fit_pt_vec.at(i).z - fit_pt_vec.front().z,2) ) < 1*units::cm ||
	sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.back().x,2) +
	     pow(fit_pt_vec.at(i).y - fit_pt_vec.back().y,2) +
	     pow(fit_pt_vec.at(i).z - fit_pt_vec.back().z,2) ) < 1*units::cm ||
	sqrt(pow(fit_pt_vec.at(i).x - start_p.x,2) +
	     pow(fit_pt_vec.at(i).y - start_p.y,2) +
	     pow(fit_pt_vec.at(i).z - start_p.z,2) ) < 1*units::cm
	) continue;

    // std::cout << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.front().x,2) +
    // 	     pow(fit_pt_vec.at(i).y - fit_pt_vec.front().y,2) +
    // 		      pow(fit_pt_vec.at(i).z - fit_pt_vec.front().z,2) )/units::cm << std::endl;
    
    if (flag_check){
      double ave_dQ_dx = 0; int ave_count = 0;
      double max_dQ_dx = dQ_vec.at(i)/dx_vec.at(i);
      for (size_t j = -2;j!=3;j++){
	if (i+j<fit_pt_vec.size() && i+j>=0){
	  ave_dQ_dx += dQ_vec.at(i+j)/dx_vec.at(i+j);
	  ave_count ++;
	  if (dQ_vec.at(i+j)/dx_vec.at(i+j) > max_dQ_dx) max_dQ_dx = dQ_vec.at(i+j)/dx_vec.at(i+j);
	}
      }
      if (ave_count!=0) ave_dQ_dx /= ave_count; 
      
      double sum_angles = 0;
      double nsum = 0;

      double sum_angles1 = 0;
      double nsum1 = 0;
      
      for (int j = -2; j!=3;j++){
	if (i+j>=0 && i+j<fit_pt_vec.size()){
	  if (para_angles.at(i+j)>10){
	    sum_angles += pow(refl_angles.at(i+j),2);
	    nsum ++;
	  }
	  if (para_angles.at(i+j)>7.5){
	    sum_angles1 += pow(refl_angles.at(i+j),2);
	    nsum1 ++;
	  }
	}
      }
      if (nsum!=0) sum_angles=sqrt(sum_angles/nsum);
      if (nsum1!=0) sum_angles1 = sqrt(sum_angles1/nsum1);
      //if (wcpt_vec.front().index >940 && wcpt_vec.front().index < 1200)

      // if (fabs(fit_pt_vec.at(i).x-2121.19)<30 && fabs(fit_pt_vec.at(i).y-218.775) < 30 && fabs(fit_pt_vec.at(i).z-715.347)<30)
      // std::cout << i << " " << max_dQ_dx << " " << para_angles.at(i) << " " << refl_angles.at(i) << " " << sum_angles << " " << sum_angles1<< " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.front().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.front().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.front().z,2) ) /units::cm << " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.back().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.back().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.back().z,2) )/units::cm << " " << fit_pt_vec.at(i) << std::endl;
      
      if (para_angles.at(i) > 10 && refl_angles.at(i) > 30 && sum_angles > 15 ){
	save_i = i;
	break;
      }else if (para_angles.at(i) > 7.5 && refl_angles.at(i) > 45 && sum_angles1 > 25){
	save_i = i;
	break;
      }else if (para_angles.at(i) > 15 && refl_angles.at(i) > 27 && sum_angles > 12.5){
	//std::cout << i << " " << ave_dQ_dx << " " <<max_dQ_dx << " " << para_angles.at(i) << " " << refl_angles.at(i) << " " << sum_angles << " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.front().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.front().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.front().z,2) ) /units::cm << " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.back().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.back().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.back().z,2) )/units::cm << " " << fit_pt_vec.at(i) << std::endl;
	save_i = i;
	break;
      }else if(para_angles.at(i) > 15 && refl_angles.at(i) > 22 && sum_angles > 19 && max_dQ_dx > 43e3/units::cm*1.5 && ave_dQ_dx > 43e3/units::cm){
	//	std::cout << i << " " << ave_dQ_dx << " " << max_dQ_dx << " " << para_angles.at(i) << " " << refl_angles.at(i) << " " << sum_angles << " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.front().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.front().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.front().z,2) ) /units::cm << " " << sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.back().x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.back().y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.back().z,2) )/units::cm << " " << fit_pt_vec.at(i) << std::endl;
	save_i = i;
	flag_search = true;
	break;
      }
    }
  }

  // return the results ...
  if (save_i>0 && save_i+1 < fit_pt_vec.size() ){
    Point p = fit_pt_vec.at(save_i);
    
    Point prev_p(0,0,0);
    Point next_p(0,0,0);
    int num_p = 0;
    int num_p1 = 0;

    double length1 = 0;
    double length2 = 0;
    Point last_p1, last_p2;
    for (size_t i=1;i!=10;i++){
      if (save_i>=i){
	length1 += sqrt(pow(fit_pt_vec.at(save_i-i).x -fit_pt_vec.at(save_i-i+1).x,2) + pow(fit_pt_vec.at(save_i-i).y -fit_pt_vec.at(save_i-i+1).y,2) + pow(fit_pt_vec.at(save_i-i).z -fit_pt_vec.at(save_i-i+1).z,2));
	  
	prev_p.x += fit_pt_vec.at(save_i-i).x;
	prev_p.y += fit_pt_vec.at(save_i-i).y;
	prev_p.z += fit_pt_vec.at(save_i-i).z;

	last_p1 = fit_pt_vec.at(save_i-i);
	
	num_p ++;
      }
      if (save_i+i<fit_pt_vec.size()){
	length2 += sqrt(pow(fit_pt_vec.at(save_i+i).x - fit_pt_vec.at(save_i+i-1).x,2)+pow(fit_pt_vec.at(save_i+i).y - fit_pt_vec.at(save_i+i-1).y,2)+pow(fit_pt_vec.at(save_i+i).z - fit_pt_vec.at(save_i+i-1).z,2));
	next_p.x += fit_pt_vec.at(save_i+i).x;
	next_p.y += fit_pt_vec.at(save_i+i).y;
	next_p.z += fit_pt_vec.at(save_i+i).z;
	
	last_p2 = fit_pt_vec.at(save_i+i);
	num_p1 ++;
      }
    }
    double length1_1 = sqrt(pow(last_p1.x - fit_pt_vec.at(save_i).x,2) + pow(last_p1.y - fit_pt_vec.at(save_i).y,2) + pow(last_p1.z - fit_pt_vec.at(save_i).z,2));
    double length2_1 = sqrt(pow(last_p2.x - fit_pt_vec.at(save_i).x,2) + pow(last_p2.y - fit_pt_vec.at(save_i).y,2) + pow(last_p2.z - fit_pt_vec.at(save_i).z,2));

    
    if (fabs(length2 - length2_1)< 0.03 * length2_1 && length1 * length2_1 > 1.06 * length2 * length1_1){
      flag_switch = true;
      flag_search = true;
    }else if (fabs(length1 - length1_1)< 0.03 * length1_1 && length2 * length1_1 > 1.06 * length1 * length2_1){
      flag_search = true;
    }
    
    
    //    std::cout << length1/length1_1 << " " << length2/length2_1 << " " << flag_switch << std::endl;
    
    prev_p.x /= num_p;
    prev_p.y /= num_p;
    prev_p.z /= num_p;

    next_p.x /= num_p1;
    next_p.y /= num_p1;
    next_p.z /= num_p1;
    
    TVector3 dir(p.x - prev_p.x, p.y - prev_p.y, p.z - prev_p.z);
    dir = dir.Unit();
    TVector3 dir1(p.x - next_p.x, p.y - next_p.y, p.z - next_p.z);
    dir1 = dir1.Unit();
    
    double sum_dQ = 0, sum_dx = 0;
    for (int i=-2;i!=3;i++){
      if (save_i+i>=0&&save_i+i<fit_pt_vec.size()){
	sum_dQ += dQ_vec.at(save_i+i);
	sum_dx += dx_vec.at(save_i+i);
      }
    }

    //    std::cout << sqrt(pow(p.x-start_p.x,2) + pow(p.y-start_p.y,2) + pow(p.z-start_p.z,2))/units::cm << std::endl;
    //    std::cout << sum_dQ/(sum_dx) << " " << flag_search << std::endl;
    
    if (flag_search){
      if (flag_switch){
	std::cout << "Cluster: " << cluster_id << " Continue Search True Kink in Backward" << std::endl;
	return std::make_tuple(p, dir1, dir, true);
      }else{
	std::cout << "Cluster: " << cluster_id<< " Continue Search True Kink in Forward" << std::endl;
	return std::make_tuple(p, dir, dir1, true);
      }
    }else if (sum_dQ/(sum_dx+1e-9) > 2500 ){
      if (flag_switch){
	return std::make_tuple(p, dir1, dir, false);
      }else{
	return std::make_tuple(p, dir, dir1, false);
      }
    }else{
      if (flag_switch){
	std::cout << "Cluster: " << cluster_id<< " Continue Search True Kink in Backward" << std::endl;
	return std::make_tuple(p, dir1, dir, true);
      }else{
	std::cout << "Cluster: " << cluster_id << " Continue Search True Kink in Forward" << std::endl;
	return std::make_tuple(p, dir, dir1, true);
      }
    }

  }else{

    Point p1 = fit_pt_vec.back();
    TVector3 dir(0,0,0);
    return std::make_tuple(p1,dir, dir,false);

  }
  //  std::cout << save_i << std::endl;
  
}

void WCPPID::ProtoSegment::set_point_vec(std::vector<WCP::Point >& tmp_pt_vec){
  fit_pt_vec = tmp_pt_vec;
  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
}


void WCPPID::ProtoSegment::set_fit_vec(std::vector<WCP::Point >& tmp_fit_pt_vec, std::vector<double>& tmp_dQ_vec, std::vector<double>& tmp_dx_vec, std::vector<double>& tmp_pu_vec, std::vector<double>& tmp_pv_vec, std::vector<double>& tmp_pw_vec, std::vector<double>& tmp_pt_vec, std::vector<double>& tmp_reduced_chi2_vec){
  flag_fit = true;
  
  fit_pt_vec = tmp_fit_pt_vec;
  dQ_vec = tmp_dQ_vec;
  dx_vec = tmp_dx_vec;
  dQ_dx_vec.resize(dQ_vec.size(),0);
  for (size_t i=0;i!=dQ_vec.size();i++){
    dQ_dx_vec.at(i) = dQ_vec.at(i)/(dx_vec.at(i)+1e-9);
  }

  pu_vec = tmp_pu_vec;
  pv_vec = tmp_pv_vec;
  pw_vec = tmp_pw_vec;
  pt_vec = tmp_pt_vec;
  reduced_chi2_vec = tmp_reduced_chi2_vec;

  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
}

void WCPPID::ProtoSegment::set_fit_associate_vec(std::vector<WCP::Point >& tmp_fit_pt_vec, std::vector<int>& tmp_fit_index, std::vector<bool>& tmp_fit_skip){
  fit_pt_vec = tmp_fit_pt_vec;
  fit_index_vec = tmp_fit_index;
  fit_flag_skip = tmp_fit_skip;
  
  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
}

void WCPPID::ProtoSegment::reset_fit_prop(){
  fit_index_vec.resize(fit_pt_vec.size(),-1);
  fit_flag_skip.resize(fit_pt_vec.size(),false);
}

void WCPPID::ProtoSegment::clear_fit(){
  flag_fit = false;

  fit_pt_vec.clear();
  fit_pt_vec.resize(wcpt_vec.size());
  for (size_t i=0;i!=wcpt_vec.size();i++){
    fit_pt_vec.at(i).x = wcpt_vec.at(i).x;
    fit_pt_vec.at(i).y = wcpt_vec.at(i).y;
    fit_pt_vec.at(i).z = wcpt_vec.at(i).z;
  }
  if (pcloud_fit != (ToyPointCloud*)0) delete pcloud_fit;
  pcloud_fit = new ToyPointCloud();
  pcloud_fit->AddPoints(fit_pt_vec);
  pcloud_fit->build_kdtree_index();
  
  
  
  dQ_vec.clear();
  dx_vec.clear();
  dQ_dx_vec.clear();
  pu_vec.clear();
  pv_vec.clear();
  pw_vec.clear();
  pt_vec.clear();
  reduced_chi2_vec.clear();

  fit_index_vec.clear();
  fit_flag_skip.clear();
}

void WCPPID::ProtoSegment::reset_associate_points(){
  if (pcloud_associated != (ToyPointCloud*)0){
    delete pcloud_associated;
    //    flag_good_associated.clear();
  }
  pcloud_associated = 0;
  if (pcloud_associated_steiner != (ToyPointCloud*)0){
    delete pcloud_associated_steiner;
    //    flag_good_associated_steiner.clear();
  }
  pcloud_associated_steiner = 0;
}

void WCPPID::ProtoSegment::add_associate_point_steiner(WCP::WCPointCloud<double>::WCPoint& wcp){
  if (pcloud_associated_steiner == (ToyPointCloud*)0)
    pcloud_associated_steiner = new ToyPointCloud();
  pcloud_associated_steiner->AddPoint(wcp);
  //  flag_good_associated_steiner.push_back(true);
}

void WCPPID::ProtoSegment::add_associate_point(WCPointCloud<double>::WCPoint& wcp, WC2DPointCloud<double>::WC2DPoint& wcp_u, WC2DPointCloud<double>::WC2DPoint& wcp_v, WC2DPointCloud<double>::WC2DPoint& wcp_w){
  //  std::cout << pcloud_associated << std::endl;
  
  if (pcloud_associated == (ToyPointCloud*)0)
    pcloud_associated = new ToyPointCloud();
  pcloud_associated->AddPoint(wcp, wcp_u, wcp_v, wcp_w);
  // flag_good_associated.push_back(true);
  
}

void WCPPID::ProtoSegment::print_dis(){
  for (size_t i=0;i!=wcpt_vec.size(); i++){
    Point p(wcpt_vec.at(i).x, wcpt_vec.at(i).y, wcpt_vec.at(i).z);
    std::pair<double, WCP::Point> results = get_closest_point(p);
    if (results.first > 0.6*units::cm)
      std::cout << i << " " << results.first/units::cm << std::endl;
  }
}

std::pair<double, WCP::Point> WCPPID::ProtoSegment::get_closest_point(WCP::Point &p){
  if (pcloud_fit != (ToyPointCloud*)0) 
    return pcloud_fit->get_closest_point(p);
  else{
    WCP::Point p1(0,0,0);
    return std::make_pair(-1,p1);
  }
}

double WCPPID::ProtoSegment::get_closest_2d_dis(double x, double y, int plane){
  return pcloud_fit->get_closest_2d_dis(x, y, plane);
}

std::tuple<double, double, double> WCPPID::ProtoSegment::get_closest_2d_dis(WCP::Point &p){
  if (pcloud_fit != (ToyPointCloud*)0) {
    std::pair<int, double> results_u = pcloud_fit->get_closest_2d_dis(p, 0);
    std::pair<int, double> results_v = pcloud_fit->get_closest_2d_dis(p, 1);
    std::pair<int, double> results_w = pcloud_fit->get_closest_2d_dis(p, 2);
    
    return std::make_tuple(results_u.second, results_v.second, results_w.second);
  }else{
    return std::make_tuple(-1,-1,-1);
  }
}


WCP::WCPointCloud<double>::WCPoint WCPPID::ProtoSegment::get_closest_wcpt(WCP::Point& test_p){
  double min_dis = 1e9;
  WCP::WCPointCloud<double>::WCPoint min_wcpt = wcpt_vec.front();
  for (auto it = wcpt_vec.begin(); it!=wcpt_vec.end(); it++){
    double dis = sqrt(pow(test_p.x - (*it).x,2) + pow(test_p.y - (*it).y,2) + pow(test_p.z - (*it).z,2));
    if (dis < min_dis){
      min_dis = dis;
      min_wcpt =  *it;
    }
  }
  return min_wcpt;
}

std::vector<double> WCPPID::ProtoSegment::do_track_comp(std::vector<double>& L , std::vector<double>& dQ_dx, double compare_range, double offset_length){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  TGraph *g_muon = mp.get_muon_dq_dx();
  TGraph *g_proton = mp.get_proton_dq_dx();
  TGraph *g_electron = mp.get_electron_dq_dx();
  
  double end_L = L.back() + 0.15*units::cm-offset_length;
  
  
  int ncount = 0;
  std::vector<double> vec_x;
  std::vector<double> vec_y;
    
  for (size_t i=0;i!=L.size(); i++){
    if (end_L - L.at(i) < compare_range && end_L - L.at(i) > 0){ // check up to compared range ...
      vec_x.push_back(end_L-L.at(i));
      vec_y.push_back(dQ_dx.at(i));
      ncount ++;
    }
  }
    
  TH1F *h1 = new TH1F("h1","h1",ncount,0,ncount);
  TH1F *h2 = new TH1F("h2","h2",ncount,0,ncount);
  TH1F *h3 = new TH1F("h3","h3",ncount,0,ncount);
  TH1F *h4 = new TH1F("h4","h4",ncount,0,ncount);
  TH1F *h5 = new TH1F("h5","h5",ncount,0,ncount);
    
    
  for (size_t i=0;i!=ncount;i++){
    // std::cout << i << " " << vec_y.at(i) << std::endl;
    h1->SetBinContent(i+1,vec_y.at(i));
    h2->SetBinContent(i+1,g_muon->Eval((vec_x.at(i))/units::cm)); //muon
    h3->SetBinContent(i+1,50e3); //MIP like ...
    h4->SetBinContent(i+1,g_proton->Eval(vec_x.at(i)/units::cm)); //proton
    h5->SetBinContent(i+1,g_electron->Eval(vec_x.at(i)/units::cm));    // electron
  }
  double ks1 = h2->KolmogorovTest(h1,"M");
  double ratio1 = h2->GetSum()/(h1->GetSum()+1e-9);
  double ks2 = h3->KolmogorovTest(h1,"M");
  double ratio2 = h3->GetSum()/(h1->GetSum()+1e-9);
  double ks3 = h4->KolmogorovTest(h1,"M");
  double ratio3 = h4->GetSum()/(h1->GetSum()+1e-9);
  double ks4 = h5->KolmogorovTest(h1,"M");
  double ratio4 = h5->GetSum()/(h1->GetSum()+1e-9);
  
  delete h1;
  delete h2;
  delete h3;
  delete h4;
  delete h5;
  
  //  std::cout << id << " " << get_length()/units::cm << " " << ks1 << " " << ratio1 << " " << ks2 << " " << ratio2 << " " << ks3 << " " << ratio3 << " " << ks4 << " " << ratio4 << " " << ks1-ks2 + (fabs(ratio1-1)-fabs(ratio2-1))/1.5*0.3 << std::endl;

  std::vector<double> results;
  results.push_back(eval_ks_ratio(ks1, ks2, ratio1, ratio2)); // direction metric
  
  results.push_back(sqrt(pow(ks1,2) + pow(ratio1-1,2))); // muon information
  //  results.push_back(ratio1);

  results.push_back(sqrt(pow(ks3,2) + pow(ratio3-1,2))); // proton information
  //results.push_back(ratio3);

  results.push_back(sqrt(pow(ks4,2) + pow(ratio4-1,2))); // electron information
  //  results.push_back(ratio4);
  
  return results;
  
}

bool WCPPID::ProtoSegment::eval_ks_ratio(double ks1, double ks2, double ratio1, double ratio2){
  //  std::cout << ks1 << " " << ks2 << " " << ratio1 << " " << ratio2 << " " << sqrt(pow(ks2/0.06,2)+pow((ratio2-1)/0.06,2)) << " " << ks1-ks2 + (fabs(ratio1-1)-fabs(ratio2-1))/1.5*0.3 << " " <<  ks1-ks2 + (fabs(ratio1-1)-fabs(ratio2-1))/1.5*0.3 << " " << std::endl;
  if (ks1-ks2 >= 0.0) return false;
  if (sqrt(pow(ks2/0.06,2)+pow((ratio2-1)/0.06,2))< 1.4 && ks1-ks2 + (fabs(ratio1-1)-fabs(ratio2-1))/1.5*0.3 > -0.02) return false;

  if (ks1 - ks2 < -0.02 && (ks2 > 0.09 && fabs(ratio2-1) >0.1 || ratio2 > 1.5 || ks2 > 0.2)) return true;
  if ( ks1-ks2 + (fabs(ratio1-1)-fabs(ratio2-1))/1.5*0.3 < 0) return true;

  return false;
 }


bool WCPPID::ProtoSegment::do_track_pid(std::vector<double>& L , std::vector<double>& dQ_dx, double compare_range , double offset_length, bool flag_force ){
  std::vector<double> rL(L.size(),0);
  std::vector<double> rdQ_dx(L.size(),0);
  // get reverse  vectors ...
  for (size_t i=0;i!=L.size();i++){
    rL.at(i) = L.back() - L.at(L.size()-1-i);
    rdQ_dx.at(i) = dQ_dx.at(L.size()-1-i);
  }

  std::vector<double> result_forward = do_track_comp(L, dQ_dx, compare_range, offset_length);
  std::vector<double> result_backward = do_track_comp(rL, rdQ_dx, compare_range, offset_length);

  // direction determination 
  bool flag_forward = std::round(result_forward.at(0));
  bool flag_backward = std::round(result_backward.at(0));

  double length = get_length();
  double length1 = sqrt(pow(fit_pt_vec.front().x - fit_pt_vec.back().x,2) + pow(fit_pt_vec.front().y - fit_pt_vec.back().y,2) + pow(fit_pt_vec.front().z - fit_pt_vec.back().z,2));
  
  int forward_particle_type = 13; // default muon
  double min_forward_val = result_forward.at(1) ;
  if (result_forward.at(2) < min_forward_val){
    min_forward_val = result_forward.at(2);
    forward_particle_type = 2212;
  }
  
  if (result_forward.at(3) < min_forward_val && length < 20*units::cm){
    min_forward_val = result_forward.at(3);
    forward_particle_type = 11;
  }

  //  std::cout << length/units::cm << " " << length1/units::cm << std::endl;
  
  int backward_particle_type  =13; // default muon
  double min_backward_val = result_backward.at(1);
  if (result_backward.at(2) < min_backward_val){
    min_backward_val = result_backward.at(2);
    backward_particle_type = 2212;
  }
  if (result_backward.at(3) < min_backward_val && length < 20*units::cm){
    min_backward_val = result_backward.at(3);
    backward_particle_type = 11;
  }

  // std::cout << id << " " << get_length()/units::cm << " " << flag_forward << " " << result_forward.at(0) << " m: " << result_forward.at(1)  << " p: " << result_forward.at(2) << " e: " << result_forward.at(3)  << std::endl;
  // std::cout << id << " " << get_length()/units::cm << " " << flag_backward << " " << result_backward.at(0) << " m: " << result_backward.at(1)  << " p: " << result_backward.at(2)  << " e: " << result_backward.at(3) <<  std::endl;

  
  if (flag_forward == 1 && flag_backward == 0){
    flag_dir = 1;
    particle_type = forward_particle_type;
    particle_score = min_forward_val;
    return true;
  }else if (flag_forward == 0 && flag_backward == 1){
    flag_dir = -1;
    particle_type = backward_particle_type;
    particle_score = min_backward_val;
    return true;
  }else if (flag_forward == 1 && flag_backward == 1){
    if (min_forward_val < min_backward_val){
      flag_dir = 1;
      particle_type = forward_particle_type;
      particle_score = min_forward_val;
    }else{
      flag_dir = -1;
      particle_type = backward_particle_type;
      particle_score = min_backward_val;
    }
    return true;
  }else if (flag_forward ==0 && flag_backward == 0 && flag_force){
    if (min_forward_val < min_backward_val){
      particle_score = min_forward_val;
      particle_type = forward_particle_type;
      flag_dir = 1;
    }else{
      particle_score = min_backward_val;
      particle_type = backward_particle_type;
      flag_dir = -1;
    }
    return true;
  }

  // reset before return ...
  flag_dir = 0;
  particle_type  = 0;
  
  return false;
}

bool WCPPID::ProtoSegment::is_dir_weak(){
  double length = get_length();
  
  
  if (fabs(particle_type)==13 && particle_score > 0.07 && length >= 5*units::cm
      || fabs(particle_type)==13 &&  particle_score > 0.15 && length < 5*units::cm
      || fabs(particle_type)==2212 && particle_score > 0.13 && length >=5*units::cm
      || fabs(particle_type)==2212 && particle_score > 0.27 && length < 5*units::cm
      || dir_weak ) return true;

  
  return false;
}

bool WCPPID::ProtoSegment::get_flag_shower(){
  return flag_shower_trajectory || flag_shower_topology || get_flag_shower_dQdx();
}

bool WCPPID::ProtoSegment::get_flag_shower_dQdx(){
  if (fabs(particle_type)==11) return true;
  return false;
}



double WCPPID::ProtoSegment::cal_kine_dQdx(std::vector<double>& vec_dQ, std::vector<double>& vec_dx){
  double kine_energy =0;
  Double_t alpha = 1.0;
  Double_t beta = 0.255;
  
  for (size_t i=0;i!=vec_dQ.size();i++){
    Double_t dQdx = vec_dQ.at(i)/(vec_dx.at(i) + 1e-9) * units::cm;
    if (dQdx/43e3 > 1000) dQdx = 0;

    
    //std::cout << id << " " << i << " " << dQdx/43e3 << std::endl;
    
    Double_t dEdx = (exp(dQdx * 23.6e-6*beta/1.38/0.273) - alpha)/(beta/1.38/0.273) * units::MeV/units::cm;

    if (dEdx < 0) dEdx = 0;
    if (dEdx > 50*units::MeV/units::cm ) dEdx = 50*units::MeV/units::cm;
    
    kine_energy += dEdx * vec_dx.at(i);
  }
  
  return kine_energy;
}

double WCPPID::ProtoSegment::cal_kine_dQdx(){
  double kine_energy =0;
  Double_t alpha = 1.0;
  Double_t beta = 0.255;
  for (size_t i=0;i!=fit_pt_vec.size();i++){
    Double_t dQdx = dQ_vec.at(i)/(dx_vec.at(i) + 1e-9) * units::cm;
    if (dQdx/43e3 > 1000) dQdx = 0;


    
    Double_t dEdx = (exp(dQdx * 23.6e-6*beta/1.38/0.273) - alpha)/(beta/1.38/0.273) * units::MeV/units::cm;

    if (dEdx < 0) dEdx = 0;
    if (dEdx > 50*units::MeV/units::cm ) dEdx = 50*units::MeV/units::cm;
    
    //  std::cout << id << " " << i << " " << dQdx/(43e3) << " " << dEdx/(units::MeV/units::cm) << " " << dx_vec.at(i)/units::cm << " " << kine_energy << std::endl;
    
    if (i==0){
      double dis = sqrt(pow(fit_pt_vec.at(1).x - fit_pt_vec.at(0).x,2) + pow(fit_pt_vec.at(1).y - fit_pt_vec.at(0).y,2) + pow(fit_pt_vec.at(1).z - fit_pt_vec.at(0).z,2));
      if (dx_vec.at(i) > dis*1.5){
	kine_energy += dEdx * dis ;
      }else{
	kine_energy += dEdx * dx_vec.at(i);
      }
    }else if (i+1 == fit_pt_vec.size()){
      double dis = sqrt(pow(fit_pt_vec.at(i).x - fit_pt_vec.at(i-1).x,2) + pow(fit_pt_vec.at(i).y - fit_pt_vec.at(i-1).y,2) + pow(fit_pt_vec.at(i).z - fit_pt_vec.at(i-1).z,2));
      if (dx_vec.at(i) > dis*1.5){
	kine_energy += dEdx * dis ;
      }else{
	kine_energy += dEdx * dx_vec.at(i);
      }
    }else{
      kine_energy += dEdx * dx_vec.at(i);
    }
  }
  
  return kine_energy;
}

double WCPPID::ProtoSegment::cal_kine_range(double L){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  TGraph *g_range = 0;
  if (fabs(particle_type)==11)    g_range = mp.get_electron_r2ke();
  else if (fabs(particle_type)==13) g_range = mp.get_muon_r2ke();
  else if (fabs(particle_type)==211) g_range = mp.get_pion_r2ke();
  else if (fabs(particle_type)==321) g_range = mp.get_kaon_r2ke();
  else if (fabs(particle_type)==2212) g_range = mp.get_proton_r2ke();

  double kine_energy = g_range->Eval(L/units::cm) * units::MeV;
  return kine_energy;
}

double WCPPID::ProtoSegment::cal_kine_range(){
  TPCParams& mp = Singleton<TPCParams>::Instance();
  TGraph *g_range = mp.get_muon_r2ke();
  if (fabs(particle_type)==11)    g_range = mp.get_electron_r2ke();
  else if (fabs(particle_type)==13) g_range = mp.get_muon_r2ke();
  else if (fabs(particle_type)==211) g_range = mp.get_pion_r2ke();
  else if (fabs(particle_type)==321) g_range = mp.get_kaon_r2ke();
  else if (fabs(particle_type)==2212) g_range = mp.get_proton_r2ke();

  //  std::cout << g_range << std::endl;
  
  double kine_energy = g_range->Eval(get_length()/units::cm) * units::MeV;
  return kine_energy;
}

void WCPPID::ProtoSegment::cal_4mom(){
 
  double length = get_length();
  double kine_energy = 0;

  if (length < 4*units::cm){
    kine_energy = cal_kine_dQdx(); // short track 
  }else if (flag_shower_trajectory){
    kine_energy = cal_kine_dQdx();
  }else{
    kine_energy = cal_kine_range();
  }

  kenergy_best = kine_energy;

  //  std::cout << id << " " << cal_kine_dQdx() << " " << cal_kine_range() << " " << particle_mass << " " << particle_type << std::endl;
  
  //std::cout << kine_energy << std::endl;
  particle_4mom[3]= kine_energy + particle_mass;
  double mom = sqrt(pow(particle_4mom[3],2) - pow(particle_mass,2));
  //  direction vector ...

  TVector3 v1 = cal_dir_3vector();
  particle_4mom[0] = mom * v1.X();
  particle_4mom[1] = mom * v1.Y();
  particle_4mom[2] = mom * v1.Z();
}

TVector3 WCPPID::ProtoSegment::cal_dir_3vector(WCP::Point& p, double dis_cut){
  WCP::Point p1(0,0,0);
  Int_t ncount = 0;
  for (size_t i=0;i!=fit_pt_vec.size();i++){
    double dis = sqrt(pow(fit_pt_vec.at(i).x - p.x,2)+pow(fit_pt_vec.at(i).y - p.y,2)+pow(fit_pt_vec.at(i).z - p.z,2));
    //    std::cout << dis/units::cm << std::endl;
    if (dis < dis_cut){
      p1.x += fit_pt_vec.at(i).x;
      p1.y += fit_pt_vec.at(i).y;
      p1.z += fit_pt_vec.at(i).z;
      ncount ++;
    }
  }
  
  TVector3 v1(p1.x/ncount - p.x, p1.y/ncount - p.y, p1.z/ncount - p.z);
  v1 = v1.Unit();
  return v1;
}

TVector3 WCPPID::ProtoSegment::cal_dir_3vector(){
  WCP::Point p(0,0,0);  
  if (flag_dir == 1){
    for (size_t i=1;i<5;i++){
      if (i>= fit_pt_vec.size()) break;
      p.x += fit_pt_vec.at(i).x - fit_pt_vec.at(0).x;
      p.y += fit_pt_vec.at(i).y - fit_pt_vec.at(0).y;
      p.z += fit_pt_vec.at(i).z - fit_pt_vec.at(0).z;
    }
  }else if (flag_dir == -1){
    for (size_t i=1;i<5;i++){
      if (i+1 > fit_pt_vec.size()) break;
      p.x += fit_pt_vec.at(fit_pt_vec.size()-i-1).x - fit_pt_vec.back().x;
      p.y += fit_pt_vec.at(fit_pt_vec.size()-i-1).y - fit_pt_vec.back().y;
      p.z += fit_pt_vec.at(fit_pt_vec.size()-i-1).z - fit_pt_vec.back().z;
    }
  }
  TVector3 v1(p.x, p.y, p.z);
  v1 = v1.Unit();
  return v1;
}


void WCPPID::ProtoSegment::determine_dir_track(int start_n, int end_n, bool flag_print){
  flag_dir = 0;
  int npoints = fit_pt_vec.size();
  int start_n1 = 0, end_n1 = npoints - 1;
  // if more than one point, exclude the vertex ...
  if (end_n!=1) {
    end_n1 = npoints - 2;
    npoints -= 1;
  }
  if (start_n!=1) {
    npoints -=1;
    start_n1 = 1;
  }
  // if (start_n == 1 && end_n == 1){
  //   end_n1 = npoints - 2;
  //   npoints -= 1;
    
  //   npoints -=1;
  //   start_n1 = 1;
  // }
  
  // if (id == 1) std::cout << id << " " << npoints << " " << end_n1 << " " << start_n1 << std::endl;
  
  if (npoints ==0 || end_n1 < start_n1) return;
  std::vector<double> L(npoints,0);
  std::vector<double> dQ_dx(npoints,0);
  
  double dis = 0;
  //  std::cout << start_n1 << " " << end_n1 << " " << dQ_vec.size() << " " << fit_pt_vec.size() << std::endl;
  for (size_t i = start_n1; i <= end_n1; i++){
    L.at(i-start_n1) = dis;
    dQ_dx.at(i-start_n1) = dQ_vec.at(i)/(dx_vec.at(i)/units::cm+1e-9);
    if (i+1 < fit_pt_vec.size())
      dis += sqrt(pow(fit_pt_vec.at(i+1).x-fit_pt_vec.at(i).x,2) + pow(fit_pt_vec.at(i+1).y-fit_pt_vec.at(i).y,2) + pow(fit_pt_vec.at(i+1).z - fit_pt_vec.at(i).z,2));
  }
    
  if (npoints >=2){ // reasonably long ...
     if (start_n == 1 && end_n == 1 &&  npoints >=15){
      // can use the dQ/dx to do PID and direction ...
       bool tmp_flag_pid = do_track_pid(L, dQ_dx, 35*units::cm, 1*units::cm, true);
       if (!tmp_flag_pid) tmp_flag_pid =do_track_pid(L, dQ_dx, 15*units::cm, 1*units::cm, true);
     }else{
       // can use the dQ/dx to do PID and direction ...
       bool tmp_flag_pid = do_track_pid(L, dQ_dx);
       if (!tmp_flag_pid) tmp_flag_pid =do_track_pid(L, dQ_dx, 15*units::cm);
       // if (!tmp_flag_pid) tmp_flag_pid = do_track_pid(L, dQ_dx, 35*units::cm, 3*units::cm);
       // if (!tmp_flag_pid) tmp_flag_pid =do_track_pid(L, dQ_dx, 15*units::cm, 3*units::cm);
     }
  }

 
  double length = get_length();

  // not a good idea to constraing dQ/dx for electron
  
  // short track what to do???
  if (particle_type == 0){
    // calculate medium dQ/dx
    std::vector<double> vec_dQ_dx = dQ_dx;
    std::nth_element(vec_dQ_dx.begin(), vec_dQ_dx.begin() + vec_dQ_dx.size()/2, vec_dQ_dx.end());
    double medium_dQ_dx = *std::next(vec_dQ_dx.begin(), vec_dQ_dx.size()/2);
    if (medium_dQ_dx > 43e3 * 1.75) particle_type = 2212; // proton
    else if (medium_dQ_dx < 43e3 * 1.2) particle_type = 13; //muon type
    else if (medium_dQ_dx < 43e3 * 1.5 && length < 4*units::cm) particle_type = 13;
    //    std::cout << medium_dQ_dx/(43e3) << std::endl;
  }

  // electron and both end contain stuff
  if (abs(particle_type)==11 && (start_n>1 && end_n >1)){
    dir_weak = true;
    flag_dir = 0;
    if (particle_score < 0.15) particle_type = 13;
  }else if (abs(particle_type)==11 && (start_n >1 && flag_dir == -1 || end_n >1 && flag_dir == 1) ){
    dir_weak = true;
    flag_dir = 0;
    if (particle_score < 0.15) particle_type = 13;
  }else if (length < 1.5*units::cm){
    dir_weak = true;
  }

  
  
  // vertex activities ...
  if (length < 1.5*units::cm && (start_n == 1 || end_n == 1)){
    if (start_n ==1 && end_n > 2){
      flag_dir = -1;
      std::vector<double> vec_dQ_dx = dQ_dx;
      std::nth_element(vec_dQ_dx.begin(), vec_dQ_dx.begin() + vec_dQ_dx.size()/2, vec_dQ_dx.end());
      double medium_dQ_dx = *std::next(vec_dQ_dx.begin(), vec_dQ_dx.size()/2);
      if (medium_dQ_dx > 43e3 * 1.75) particle_type = 2212;
      else if (medium_dQ_dx < 43e3*1.2) particle_type = 211;
    }else if (end_n==1 && start_n>2){
      flag_dir = 1;
      std::vector<double> vec_dQ_dx = dQ_dx;
      std::nth_element(vec_dQ_dx.begin(), vec_dQ_dx.begin() + vec_dQ_dx.size()/2, vec_dQ_dx.end());
      double medium_dQ_dx = *std::next(vec_dQ_dx.begin(), vec_dQ_dx.size()/2);
      if (medium_dQ_dx > 43e3 * 1.75) particle_type = 2212;
      else if (medium_dQ_dx < 43e3*1.2) particle_type = 211;
    }
  }


  // if the particle score is really bad, make it a shower ...
  if (length > 10*units::cm && particle_score > 1.0 && particle_score < 100){
    particle_type = 11;
    particle_score = 200;
    flag_dir = 0;
  }
 
  
  if (particle_type!=0){
    TPCParams& mp = Singleton<TPCParams>::Instance();
    if (fabs(particle_type)==11)    particle_mass = mp.get_mass_electron();
    else if (fabs(particle_type)==13) particle_mass = mp.get_mass_muon();
    else if (fabs(particle_type)==211) particle_mass = mp.get_mass_pion();
    else if (fabs(particle_type)==321) particle_mass = mp.get_mass_kaon();
    else if (fabs(particle_type)==2212) particle_mass = mp.get_mass_proton();
  }

  

  if ((flag_dir==1 && end_n == 1 || flag_dir==-1 && start_n ==1) && particle_type!=0){
    cal_4mom();
  }

  if (flag_print)
    std::cout << id << " " << length/units::cm << " Track " << flag_dir << " " << is_dir_weak() <<  " " << particle_type << " " << particle_mass/units::MeV << " " << (particle_4mom[3]-particle_mass)/units::MeV << " " << particle_score << std::endl;
  
  
}

void WCPPID::ProtoSegment::determine_dir_shower_trajectory(int start_n, int end_n, bool flag_print){
  flag_dir = 0;
  double length = get_length();
 
  // hack for now ...
  particle_type = 11;
  TPCParams& mp = Singleton<TPCParams>::Instance();

  
  if (start_n==1 && end_n >1){
    flag_dir = -1;
  }else if (start_n > 1 && end_n == 1){
    flag_dir = 1;
  }else{
    determine_dir_track(start_n, end_n, false);
    if (particle_type!=11){
      particle_type = 11;
      flag_dir = 0;
    }
  }
  
  particle_mass = mp.get_mass_electron();
  
  cal_4mom();

  if (flag_print)
    std::cout << id << " " << length/units::cm << " S_traj " << flag_dir << " " << is_dir_weak() <<  " " << particle_type << " " << particle_mass/units::MeV << " " << (particle_4mom[3]-particle_mass)/units::MeV << " " << particle_score << std::endl;

}

void WCPPID::ProtoSegment::determine_dir_shower_topology(int start_n, int end_n, bool flag_print){
  
  double length = get_length();
  // hack for now
  particle_type = 11;
  TPCParams& mp = Singleton<TPCParams>::Instance();
  particle_mass = mp.get_mass_electron();
  
  //if (start_n==1 && end_n >1){
  //  flag_dir = -1;
  //}else if (start_n > 1 && end_n == 1){
  //  flag_dir = 1;
  //}
  // WCP::Point center(0,0,0);
  // WCP::WCPointCloud<double>& cloud = pcloud_associated->get_cloud();
  // for (size_t i=0;i!=cloud.pts.size();i++){
  //   center.x += cloud.pts.at(i).x;
  //   center.y += cloud.pts.at(i).y;
  //   center.z += cloud.pts.at(i).z;
  // }
  // center.x /= cloud.pts.size();
  // center.y /= cloud.pts.size();
  // center.z /= cloud.pts.size();

  // double dis1 = sqrt(pow(center.x - fit_pt_vec.front().x ,2) + pow(center.y - fit_pt_vec.front().y,2) + pow(center.z - fit_pt_vec.front().z,2));
  // double dis2 = sqrt(pow(center.x - fit_pt_vec.back().x ,2) + pow(center.y - fit_pt_vec.back().y,2) + pow(center.z - fit_pt_vec.back().z,2));

  // std::cout << dis1/units::cm << " " << dis2/units::cm << std::endl;

  if (flag_print)
    std::cout << id << " " << length/units::cm << " S_topo " << flag_dir << " " << is_dir_weak() <<  " " << particle_type << " " << particle_mass/units::MeV << " " << (particle_4mom[3]-particle_mass)/units::MeV << " " << particle_score << std::endl;
}


std::tuple<WCPPID::ProtoSegment*, WCPPID::ProtoVertex*, WCPPID::ProtoSegment*> WCPPID::ProtoSegment::break_segment_at_point(WCP::Point& p, int& acc_segment_id, int& acc_vertex_id){

  int nbreak = -1;
  double min_dis = 1e9;
  for (size_t i=0;i!=wcpt_vec.size(); i++){
    double dis = sqrt(pow(wcpt_vec.at(i).x-p.x,2) + pow(wcpt_vec.at(i).y-p.y,2) + pow(wcpt_vec.at(i).z-p.z,2));
    if (dis < min_dis){
      min_dis = dis;
      nbreak = i;
    }
  }
  
  int nbreak_fit = -1;
  min_dis = 1e9;
  for (size_t i=0; i!=fit_pt_vec.size(); i++){
    double dis = sqrt(pow(fit_pt_vec.at(i).x-p.x,2) + pow(fit_pt_vec.at(i).y-p.y,2) + pow(fit_pt_vec.at(i).z-p.z,2));
    if (dis < min_dis){
      min_dis = dis;
      nbreak_fit = i;
    }
  }

 
  if (nbreak_fit == 0 || nbreak_fit+1 == fit_pt_vec.size()){
    return std::make_tuple(this, (WCPPID::ProtoVertex*)0, (WCPPID::ProtoSegment*)0);
  }else{
    if (nbreak ==0 ) nbreak ++;
    if (nbreak+1==wcpt_vec.size()) nbreak --;
  }


 
  
  // Now start to construct new segment ...
  std::list<WCP::WCPointCloud<double>::WCPoint > path_wcps1;
  std::list<WCP::WCPointCloud<double>::WCPoint > path_wcps2;
  WCP::WCPointCloud<double>::WCPoint vertex_wcp;
  for (size_t i=0; i!=wcpt_vec.size(); i++){
    if (i==nbreak){
      path_wcps1.push_back(wcpt_vec.at(i));
      path_wcps2.push_back(wcpt_vec.at(i));
      vertex_wcp = wcpt_vec.at(i);
    }else if ( i< nbreak){
      path_wcps1.push_back(wcpt_vec.at(i));
    }else{
      path_wcps2.push_back(wcpt_vec.at(i));
    }
  }
  // create new segment vertex ...
  WCPPID::ProtoSegment *sg1 = new WCPPID::ProtoSegment(acc_segment_id, path_wcps1, cluster_id); acc_segment_id ++;
  WCPPID::ProtoVertex *vtx = new WCPPID::ProtoVertex(acc_vertex_id, vertex_wcp, cluster_id); acc_vertex_id ++;
  WCPPID::ProtoSegment *sg2 = new WCPPID::ProtoSegment(acc_segment_id, path_wcps2, cluster_id); acc_segment_id ++;

  //std::cout << cluster_id << " " << nbreak_fit << " " << fit_pt_vec.size() << " " << dQ_vec.size() << " " << fit_index_vec.size() << " " << pu_vec.size() << " " << reduced_chi2_vec.size() << " " << nbreak << " " << wcpt_vec.size() << std::endl;
  //  if (fit_index_vec.size()==0)
  //  fit_index_vec.resize(dQ_vec.size(),0);

  
  // fill in the vertex
  vtx->set_neutrino_vertex(false);
  vtx->set_fit_pt(fit_pt_vec.at(nbreak_fit) );
  vtx->set_fit_index(fit_index_vec.at(nbreak_fit) );
  vtx->set_flag_fit_fix(false);
  vtx->set_fit_range(1*units::cm);
  vtx->set_dx(dx_vec.at(nbreak_fit) );
  vtx->set_fit_flag(true);
  vtx->set_dQ(dQ_vec.at(nbreak_fit));
  vtx->set_pu(pu_vec.at(nbreak_fit));
  vtx->set_pv(pv_vec.at(nbreak_fit));
  vtx->set_pw(pw_vec.at(nbreak_fit));
  vtx->set_pt(pt_vec.at(nbreak_fit));
  vtx->set_reduced_chi2(reduced_chi2_vec.at(nbreak_fit));

  // std::cout << cluster_id << " " << nbreak_fit << " " << fit_pt_vec.size() << " " << dQ_vec.size() << " " << fit_index_vec.size() << " " << pu_vec.size() << " " << reduced_chi2_vec.size() << " " << nbreak << " " << wcpt_vec.size() << std::endl;
  
  // fill in first and segments
  sg1->set_dir_weak(dir_weak); sg2->set_dir_weak(dir_weak);
  sg1->set_fit_flag(flag_fit); sg2->set_fit_flag(flag_fit);
  sg1->set_flag_dir(flag_dir); sg2->set_flag_dir(flag_dir);
  sg1->set_particle_type(particle_type); sg2->set_particle_type(particle_type);
  sg1->set_particle_mass(particle_mass); sg2->set_particle_mass(particle_mass);
  sg1->set_flag_shower_trajectory(flag_shower_trajectory); sg2->set_flag_shower_trajectory(flag_shower_trajectory);
  sg1->set_flag_shower_topology(flag_shower_topology); sg2->set_flag_shower_topology(flag_shower_topology);
  sg1->set_particle_score(particle_score); sg2->set_particle_score(particle_score);
  
  
  std::vector<WCP::Point >& fit_pt_vec1 = sg1->get_point_vec();
  std::vector<WCP::Point >& fit_pt_vec2 = sg2->get_point_vec();
  std::vector<double>& dQ_vec1 = sg1->get_dQ_vec();
  std::vector<double>& dQ_vec2 = sg2->get_dQ_vec();
  std::vector<double>& dx_vec1 = sg1->get_dx_vec();
  std::vector<double>& dx_vec2 = sg2->get_dx_vec();
  std::vector<double>& dQ_dx_vec1 = sg1->get_dQ_dx_vec();
  std::vector<double>& dQ_dx_vec2 = sg2->get_dQ_dx_vec();
  std::vector<double>& pu_vec1 = sg1->get_pu_vec();
  std::vector<double>& pu_vec2 = sg2->get_pu_vec();
  std::vector<double>& pv_vec1 = sg1->get_pv_vec();
  std::vector<double>& pv_vec2 = sg2->get_pv_vec();
  std::vector<double>& pw_vec1 = sg1->get_pw_vec();
  std::vector<double>& pw_vec2 = sg2->get_pw_vec();
  std::vector<double>& pt_vec1 = sg1->get_pt_vec();
  std::vector<double>& pt_vec2 = sg2->get_pt_vec();
  std::vector<double>& reduced_chi2_vec1 = sg1->get_reduced_chi2_vec();
  std::vector<double>& reduced_chi2_vec2 = sg2->get_reduced_chi2_vec();
  std::vector<int>& fit_index_vec1 = sg1->get_fit_index_vec();
  std::vector<int>& fit_index_vec2 = sg2->get_fit_index_vec();
  std::vector<bool>& fit_flag_skip1 = sg1->get_fit_flag_skip(); // vertex???
  std::vector<bool>& fit_flag_skip2 = sg2->get_fit_flag_skip(); // vertex???

  fit_pt_vec1.clear(); fit_pt_vec2.clear();
  dQ_vec1.clear(); dQ_vec2.clear();
  pu_vec1.clear(); pu_vec2.clear();
  pv_vec1.clear(); pv_vec2.clear();
  pw_vec1.clear(); pw_vec2.clear();
  pt_vec1.clear(); pt_vec2.clear();
  reduced_chi2_vec1.clear(); reduced_chi2_vec2.clear();
  fit_index_vec1.clear(); fit_index_vec2.clear();
  fit_flag_skip1.clear(); fit_flag_skip2.clear();

  //  std::cout << fit_pt_vec.size() << " " << dQ_vec.size() << " " << dx_vec.size() << " " << pu_vec.size() << " " << pv_vec.size() << " " << pw_vec.size() << " " << pt_vec.size() << " " << reduced_chi2_vec.size() << " " << fit_index_vec.size() << " " << fit_flag_skip.size() << std::endl;
  
  for (size_t i=0; i!=fit_pt_vec.size(); i++){
    if (i==nbreak_fit){
      fit_pt_vec1.push_back(fit_pt_vec.at(i)); fit_pt_vec2.push_back(fit_pt_vec.at(i));
      dQ_vec1.push_back(dQ_vec.at(i)); dQ_vec2.push_back(dQ_vec.at(i));
      dx_vec1.push_back(dx_vec.at(i)); dx_vec2.push_back(dx_vec.at(i));
      dQ_dx_vec1.push_back(dQ_dx_vec.at(i));  dQ_dx_vec2.push_back(dQ_dx_vec.at(i));
      pu_vec1.push_back(pu_vec.at(i)); pu_vec2.push_back(pu_vec.at(i));
      pv_vec1.push_back(pv_vec.at(i)); pv_vec2.push_back(pv_vec.at(i));
      pw_vec1.push_back(pw_vec.at(i)); pw_vec2.push_back(pw_vec.at(i));
      pt_vec1.push_back(pt_vec.at(i)); pt_vec2.push_back(pt_vec.at(i));
      reduced_chi2_vec1.push_back(reduced_chi2_vec.at(i)); reduced_chi2_vec2.push_back(reduced_chi2_vec.at(i));
      fit_index_vec1.push_back(fit_index_vec.at(i)); fit_index_vec2.push_back(fit_index_vec.at(i));
      fit_flag_skip1.push_back(fit_flag_skip.at(i)); fit_flag_skip2.push_back(fit_flag_skip.at(i));
    }else if (i< nbreak_fit){
      fit_pt_vec1.push_back(fit_pt_vec.at(i));
      dQ_vec1.push_back(dQ_vec.at(i));
      dx_vec1.push_back(dx_vec.at(i));
      dQ_dx_vec1.push_back(dQ_dx_vec.at(i));
      pu_vec1.push_back(pu_vec.at(i));
      pv_vec1.push_back(pv_vec.at(i)); 
      pw_vec1.push_back(pw_vec.at(i)); 
      pt_vec1.push_back(pt_vec.at(i));
      reduced_chi2_vec1.push_back(reduced_chi2_vec.at(i)); 
      fit_index_vec1.push_back(fit_index_vec.at(i)); 
      fit_flag_skip1.push_back(fit_flag_skip.at(i)); 
    }else{
      fit_pt_vec2.push_back(fit_pt_vec.at(i));
      dQ_vec2.push_back(dQ_vec.at(i));
      dx_vec2.push_back(dx_vec.at(i));
      dQ_dx_vec2.push_back(dQ_dx_vec.at(i));
      pu_vec2.push_back(pu_vec.at(i));
      pv_vec2.push_back(pv_vec.at(i));
      pw_vec2.push_back(pw_vec.at(i));
      pt_vec2.push_back(pt_vec.at(i));
      reduced_chi2_vec2.push_back(reduced_chi2_vec.at(i));
      fit_index_vec2.push_back(fit_index_vec.at(i));
      fit_flag_skip2.push_back(fit_flag_skip.at(i));
    }
  }

  // std::cout << nbreak_fit << " " << fit_pt_vec.size() << std::endl;
  //  std::cout << fit_pt_vec2.size() << " " << dQ_vec2.size() << " " << dx_vec2.size() << " " << pu_vec2.size() << " " << pv_vec2.size() << " " << pw_vec2.size() << " " << pt_vec2.size() << " " << reduced_chi2_vec2.size() << " " << fit_index_vec2.size() << " " << fit_flag_skip2.size() << std::endl;

  
  
  sg1->build_pcloud_fit();
  sg2->build_pcloud_fit();

  //  std::cout << fit_pt_vec2.size() << " " << dQ_vec2.size() << " " << dx_vec2.size() << " " << pu_vec2.size() << " " << pv_vec2.size() << " " << pw_vec2.size() << " " << pt_vec2.size() << " " << reduced_chi2_vec2.size() << " " << fit_index_vec2.size() << " " << fit_flag_skip2.size() << std::endl;

  sg1->cal_4mom();
  sg2->cal_4mom();

  WCP::WCPointCloud<double>& cloud = pcloud_associated->get_cloud();
  WCP::WC2DPointCloud<double>& cloud_u = pcloud_associated->get_cloud_u();
  WCP::WC2DPointCloud<double>& cloud_v = pcloud_associated->get_cloud_v();
  WCP::WC2DPointCloud<double>& cloud_w = pcloud_associated->get_cloud_w();
  Int_t ncount[2]={0,0};
  for (size_t i=0;i!=cloud.pts.size();i++){
    Point p(cloud.pts.at(i).x, cloud.pts.at(i).y, cloud.pts.at(i).z);
    if (sg1->get_closest_point(p).first < sg2->get_closest_point(p).first){
      sg1->add_associate_point(cloud.pts.at(i), cloud_u.pts.at(i), cloud_v.pts.at(i), cloud_w.pts.at(i));
      ncount[0]++;
    }else{
      sg2->add_associate_point(cloud.pts.at(i), cloud_u.pts.at(i), cloud_v.pts.at(i), cloud_w.pts.at(i));
      ncount[1]++;
    }
  }
  //  std::cout << ncount[0] << " " << ncount[1] << std::endl;
  if (ncount[0] !=0)
    sg1->get_associated_pcloud()->build_kdtree_index();
  if (ncount[1] !=0)
    sg2->get_associated_pcloud()->build_kdtree_index();

  
  //  std::cout << nbreak << " " << wcpt_vec.size() << " " << nbreak_fit << " " << fit_pt_vec.size() << std::endl;

  return std::make_tuple(sg1, vtx, sg2);
  
}
