#include "WCP2dToy/WCCosmic.h"
#include "WCPData/Line.h"
#include "TVector3.h"

using namespace WCP;

bool WCP2dToy::WCCosmic::IsNearBy(ToyTracking *toytracking){
  //collect all the mcells inside the good tracks.
  MergeSpaceCellSelection mcells;
  for (int i=0;i!=toytracking->get_good_tracks().size();i++){
    for (int j=0;j!=toytracking->get_good_tracks().at(i)->get_centerVP_cells().size();j++){
      auto it = find(mcells.begin(),mcells.end(),toytracking->get_good_tracks().at(i)->get_centerVP_cells().at(j));
      if (it == mcells.end())
	mcells.push_back(toytracking->get_good_tracks().at(i)->get_centerVP_cells().at(j));
    }
  }
  
  int nsum = 0;
  int nsum_cut = 0;
  float min_dis = 1e9;
  for (int i=0;i!=mcells.size();i++){
    nsum += mcells.at(i)->Get_all_spacecell().size();
    for (int j=0;j!=points.size();j++){
      float dis = sqrt(pow(mcells.at(i)->Get_Center().x - points.at(j).x,2)
		       +pow(mcells.at(i)->Get_Center().y - points.at(j).y,2)
		       +pow(mcells.at(i)->Get_Center().z - points.at(j).z,2));
      if (min_dis > dis ) min_dis = dis;
      if (dis < 25*units::cm){
	nsum_cut += mcells.at(i)->Get_all_spacecell().size();
	break;
      }
    }
  }
  // if (min_dis < 50 * units::cm)
  //   std::cout << min_dis / units::cm << " " << nsum_cut << " " << nsum << std::endl;
  if (nsum_cut > nsum * 0.8) return true;

  return false;
}

WCP2dToy::WCCosmic::WCCosmic(WCP2dToy::ToyTrackingSelection& toytrackings)
  : toytrackings(toytrackings)
{
  center.x = 0;
  center.y = 0;
  center.z = 0;

  cosmic_flag = false;
  
  int sum = 0;
  for (int i=0;i!=toytrackings.size();i++){
    ToyTracking *tracking = toytrackings.at(i);
    for (int j=0;j!=tracking->get_good_tracks().size();j++){
      WCTrack *track = tracking->get_good_tracks().at(j);
      for (int k=0;k!=track->get_centerVP_cells().size();k++){
	MergeSpaceCell *mcell = track->get_centerVP_cells().at(k);
	auto it = find(mcells.begin(),mcells.end(),mcell);
	if (it == mcells.end()){
	  mcells.push_back(mcell);
	  center.x += mcell->Get_all_spacecell().size() * mcell->Get_Center().x;
	  center.y += mcell->Get_all_spacecell().size() * mcell->Get_Center().y;
	  center.z += mcell->Get_all_spacecell().size() * mcell->Get_Center().z;
	  sum += mcell->Get_all_spacecell().size();
	}
      }
    }
  }

  ct = 0;

  //std::cout << mcells.size() << std::endl;
  if (sum >1){
    //calculate the center
    center.x = center.x/sum;
    center.y = center.y/sum;
    center.z = center.z/sum;
    
    //std::cout << "Hough " << std::endl;
    //calculate the angle
    ct = new ClusterTrack(mcells.at(0));
    for (int i=1;i< mcells.size();i++){
      ct->AddMSCell_anyway(mcells.at(i));
    }
    ct->SC_Hough(center);
    theta = ct->Get_Theta();
    phi = ct->Get_Phi();
    
    //std::cout << "Sort " << std::endl; 
    Sort();
    //std::cout << "Fill Points" << std::endl;
    fill_points();
    //std::cout << mcells.size() << " " << cal_pos(mcells.front()) << " " << cal_pos(mcells.back()) << std::endl;
    judge_cosmic();
  }
}

void WCP2dToy::WCCosmic::judge_cosmic(){
  cosmic_flag = false;
  
  // if anythng is outside x
  for (int i=0;i!=points.size();i++){
    if (points.at(i).x < 1*units::cm || points.at(i).x > (256 - 1) * units::cm){
      // std::cout << points.at(i) << std::endl;
      //std::cout << "qx1: " <<  points.at(i).x/units::cm << std::endl;
      cosmic_flag = true;
      break;
    }
  }

  if (!cosmic_flag){
    bool front_boundary = false;
    bool back_boundary = false;

    if (fabs(points.front().y) > 233 * units::cm/2. - 10*units::cm) front_boundary = true;
    if (fabs(points.back().y) > 233 * units::cm/2. - 10*units::cm) back_boundary = true;
    if (points.front().z < 10 * units::cm || points.front().z > 10.3692 *100 * units::cm - 10 * units::cm) front_boundary = true;
    if (points.back().z < 10 * units::cm || points.back().z > 10.3692 *100 * units::cm - 10 * units::cm) back_boundary = true;

    //std::cout << "qx2: " << front_boundary << " " << back_boundary << std::endl;

    if (front_boundary && back_boundary) cosmic_flag = true;

    if ((front_boundary || back_boundary) && (!cosmic_flag)){
      // look at the angle
      TVector3 dir1(0,1,0);
      TVector3 dir3(0,0,1);
      TVector3 dir2(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
      //     std::cout << dir1.Dot(dir2)/dir2.Mag() << std::endl;
      if (fabs(dir1.Dot(dir2)/dir2.Mag()) > 0.9) cosmic_flag = true;
      TVector3 dir4;
      Point vertex_p;
      if (front_boundary){
	dir4.SetXYZ(points.front().x - points.back().x, points.front().y - points.back().y, points.front().z - points.back().z);

	vertex_p.x = points.back().x;
	vertex_p.y = points.back().y;
	vertex_p.z = points.back().z;

	if (dir4.Dot(dir2) < 0 ) 
	  dir2.SetXYZ(-dir2.x(), -dir2.y(), -dir2.z());
      }else{
	dir4.SetXYZ(points.back().x - points.front().x, points.back().y - points.front().y, points.back().z - points.front().z);

	vertex_p.x = points.front().x;
	vertex_p.y = points.front().y;
	vertex_p.z = points.front().z;

	if (dir4.Dot(dir2) < 0)
	  dir2.SetXYZ(-dir2.x(), -dir2.y(), -dir2.z());
      }
      
      if (dir3.Dot(dir2)/dir2.Mag() < -0.5) cosmic_flag = true;
      
      //std::cout << "qx3: " << fabs(dir1.Dot(dir2)/dir2.Mag()) << " " << dir3.Dot(dir2)/dir2.Mag() << " " << front_boundary << " " << back_boundary << std::endl;
      
      if (!cosmic_flag){
	// count how many good tracks were in ... 
	int ntrack = 0;
	for (int i=0;i!=toytrackings.size();i++){
	  WCP2dToy::ToyTracking *toytracking = toytrackings.at(i);
	  for (int j=0;j!=toytracking->get_vertices().size();j++){
	    WCVertex *vertex = toytracking->get_vertices().at(j);
	    if (vertex->get_ntracks() >=2){
	      for (int k=0;k!=vertex->get_ntracks();k++){
		WCTrack *track = vertex->get_tracks().at(k);
		std::vector<float> pp = track->get_direction();
		TVector3 dir5(pp.at(0),pp.at(1),pp.at(2));
		float dis = sqrt(pow(vertex_p.x - vertex->Center().x,2) + pow(vertex_p.y - vertex->Center().y,2) + pow(vertex_p.z - vertex->Center().z,2));

		if (dir3.Dot(dir2)/dir2.Mag() < 0.5 && fabs(dir1.Dot(dir2)/dir2.Mag()) > 0.5){ // angle to the beam,   angle to vertical 
		  if (dis < 5*units::cm){
		    if (fabs(dir5.Dot(dir2)/dir5.Mag()/dir2.Mag()) < 0.9)
		      ntrack ++;
		  }else{
		    if (track->get_range()/units::cm > 10 && fabs(dir5.Dot(dir2)/dir5.Mag()/dir2.Mag()) < 0.9)
		      ntrack++;
		  }
		}else{
		  if (dis < 5*units::cm){
		    if (fabs(dir5.Dot(dir2)/dir5.Mag()/dir2.Mag()) < 0.9)
		      ntrack ++;
		  }else{
		    if (track->get_range()/units::cm > 5 && fabs(dir5.Dot(dir2)/dir5.Mag()/dir2.Mag()) < 0.9)
		      ntrack++;
		  }
		}
		
		//std::cout << "qx4: " << i << " " << j << " " << fabs(dir1.Dot(dir2)/dir2.Mag()) << " " <<  dir3.Dot(dir2)/dir2.Mag() << " " << dir5.Dot(dir2)/dir5.Mag()/dir2.Mag() << " " << track->get_range()/units::cm<< " " << ntrack << " " << dis/units::cm << " " << vertex->Center().x/units::cm << " " << vertex->Center().y/units::cm << " " << vertex->Center().z/units::cm << std::endl;
	      }
	    }else if (vertex->get_ntracks() ==1){
	      WCTrack *track = vertex->get_tracks().at(0);
	      float dis = sqrt(pow(vertex_p.x - vertex->Center().x,2) + pow(vertex_p.y - vertex->Center().y,2) + pow(vertex_p.z - vertex->Center().z,2));
	      
	      if (dis > 10 *units::cm && track->get_range()/units::cm > 10){
		if (dir3.Dot(dir2)/dir2.Mag() > 0.5 && fabs(dir1.Dot(dir2)/dir2.Mag()) < 0.5)
		  ntrack ++;
	      }
	    }
	    // std::cout << vertex->get_ntracks() << std::endl;
	    // ntrack += toytrackings.at(i)->get_good_tracks().size();
	    //	for (int j=0;j!=toytrackings.at(i)->get_good_tracks().size()
	  }
	}

	if (ntrack == 0) cosmic_flag = true;
      }
      
      // if (!cosmic_flag){
      // 	//figure out the direction, and then compare with dir2 ...
      // 	WCTrackSelection tracks;
      // 	for (int i=0;i!=toytrackings.size();i++){
      // 	  for (int j=0;j!=toytrackings.at(i)->get_good_tracks().size();j++){
      // 	    tracks.push_back(toytrackings.at(i)->get_good_tracks().at(j));
      // 	  }
      // 	}
      // 	double sum = 0;
      // 	// go through all the tracks 
      // 	for (int i=0;i!=tracks.size();i++){
      // 	  WCTrack *track = tracks.at(i);
      // 	  // find out the cloest point and furthest point
      // 	  Point end_p1 = track->get_centerVP().front();
      // 	  Point end_p2 = track->get_centerVP().back();

      // 	  //	  if one of the end point is too close to the end
      // 	  float temp_dis1 = sqrt(pow(vertex_p.x - end_p1.x,2) + pow(vertex_p.y - end_p1.y,2) + pow(vertex_p.z - end_p1.z,2));
      // 	  if (temp_dis1 < 10*units::cm) continue;
      // 	  float temp_dis2 = sqrt(pow(vertex_p.x - end_p2.x,2) + pow(vertex_p.y - end_p2.y,2) + pow(vertex_p.z - end_p2.z,2));
      // 	  if (temp_dis2 < 10*units::cm) continue;

      // 	  float min_dis1 = 1e9;
      // 	  float min_dis2 = 1e9;
      // 	  float dis1, dis2, dis1_save, dis2_save;
      // 	  for (int j=0;j!=points.size();j++){
      // 	    dis1 = sqrt(pow(end_p1.x-points.at(j).x,2)+pow(end_p1.y-points.at(j).y,2)+pow(end_p1.z-points.at(j).z,2));
      // 	    if (dis1 < min_dis1) {
      // 	      min_dis1 = dis1;
      // 	      Point temp_p(points.at(j).x + dir2.x(),
      // 			   points.at(j).y + dir2.y(),
      // 			   points.at(j).z + dir2.z());
      // 	      Line l1(points.at(j), temp_p);
      // 	      dis1_save = l1.closest_dis(end_p1);
      // 	    }
      // 	    dis2 = sqrt(pow(end_p2.x-points.at(j).x,2)+pow(end_p2.y-points.at(j).y,2)+pow(end_p2.z-points.at(j).z,2));
      // 	    if (dis2 < min_dis2) {
      // 	      min_dis2 = dis2;
      // 	      Point temp_p(points.at(j).x + dir2.x(),
      // 			   points.at(j).y + dir2.y(),
      // 			   points.at(j).z + dir2.z());
      // 	      Line l1(points.at(j), temp_p);
      // 	      dis2_save = l1.closest_dis(end_p2);
      // 	    }
      // 	  }
      // 	  float max_dis;
      // 	  // define direction as cloest --> furthest
      // 	  TVector3 track_dir;
      // 	  if(dis2_save > dis1_save){
      // 	    max_dis = dis2_save;
      // 	    track_dir.SetXYZ(end_p2.x-end_p1.x,end_p2.y-end_p1.y,end_p2.z-end_p1.z);
      // 	  }else{
      // 	    max_dis = dis1_save;
      // 	    track_dir.SetXYZ(end_p1.x-end_p2.x,end_p1.y-end_p2.y,end_p1.z-end_p2.z);
      // 	  }
	  
	  
	  
      // 	  // sign is the determined by dot
      // 	  // magnitude is determined by the furthest distance
      // 	  if (dir2.Dot(track_dir)>0){
      // 	    sum += fabs(max_dis);
      // 	  }else{
      // 	    sum -= fabs(max_dis);
      // 	  }
	  
	  
      // 	}
      // 	// calculate sum , if sum is too negative, 
      // 	//std::cout << "dis: " << sum/units::cm << std::endl;
      // 	if (sum <=-3*units::cm) cosmic_flag = true;
      // }
      
      //std::cout << theta << " " << phi << " " << ntrack << std::endl;
    }

    // if (!cosmic_flag && !front_boundary && !back_boundary){
    //   TVector3 dir1(0,1,0);
    //   TVector3 dir2(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
    //   if (fabs(dir1.Dot(dir2)/dir2.Mag()) > 0.9) cosmic_flag = true;
    // }
    
  }
  
}


void WCP2dToy::WCCosmic::Add(WCCosmic *cosmic){
  toytrackings.insert(toytrackings.end(), cosmic->get_trackings().begin(),
		      cosmic->get_trackings().end());
  mcells.insert(mcells.end(),cosmic->get_mcells().begin(),cosmic->get_mcells().end());
  center.x = 0;
  center.y = 0;
  center.z = 0;
  
  int sum = 0;
  for (int i=0;i!=mcells.size();i++){
    MergeSpaceCell *mcell = mcells.at(i);
    center.x += mcell->Get_all_spacecell().size() * mcell->Get_Center().x;
    center.y += mcell->Get_all_spacecell().size() * mcell->Get_Center().y;
    center.z += mcell->Get_all_spacecell().size() * mcell->Get_Center().z;
    sum += mcell->Get_all_spacecell().size();
  }
  
  if (sum >1){
    //calculate the center
    center.x = center.x/sum;
    center.y = center.y/sum;
    center.z = center.z/sum;

    delete ct;
    ct = new ClusterTrack(mcells.at(0));
    for (int i=1;i< mcells.size();i++){
      ct->AddMSCell_anyway(mcells.at(i));
    }
    ct->SC_Hough(center);
    theta = ct->Get_Theta();
    phi = ct->Get_Phi();
    
    //std::cout << "Sort " << std::endl; 
    Sort();
    //std::cout << "Fill Points" << std::endl;
    fill_points();
    judge_cosmic();
  }

}


void WCP2dToy::WCCosmic::fill_points(){
  points.clear();
  
  //find the smallest point and largest point
  float min_dis = 20*units::m;
  Point min_point;
  float max_dis = -20*units::m;
  Point max_point;

  // std::cout << "qx: q "  << mcells.size() << std::endl;
  for (int i=0;i!=mcells.size();i++){
    MergeSpaceCell *mcell = mcells.at(i);
    if (i < 5){
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	float dis = cal_pos(cell);
	if (dis < min_dis){
	  min_dis = dis;
	  min_point.x = cell->x();
	  min_point.y = cell->y();
	  min_point.z = cell->z();
	}
      }
    }
    
    if (i +5 >= mcells.size()){
      //std::cout << "qx: q " << i << " " << mcells.size() << std::endl;
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	float dis = cal_pos(cell);
	//std::cout << "qx: " << dis/units::cm <<std::endl;
	if (dis > max_dis){
	  max_dis = dis;
	  max_point.x = cell->x();
	  max_point.y = cell->y();
	  max_point.z = cell->z();
	}
      }
    }
  }
  

  
  
  //at the beginning
  points.push_back(min_point);
  
  int nbin = (max_dis - min_dis)/(5*units::cm);
  
  if (nbin >0){
    // calculate middle points
    TH1F *hx = new TH1F("hx","hx",nbin,min_dis,max_dis);
    TH1F *hy = new TH1F("hy","hy",nbin,min_dis,max_dis);
    TH1F *hz = new TH1F("hz","hz",nbin,min_dis,max_dis);
    TH1F *hc = new TH1F("hc","hc",nbin,min_dis,max_dis);
    
    for (int i=0;i!=mcells.size();i++){
      MergeSpaceCell *mcell = mcells.at(i);
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	float dis = cal_pos(cell);
	hx->Fill(dis,cell->x());
	hy->Fill(dis,cell->y());
	hz->Fill(dis,cell->z());
	hc->Fill(dis,1);
      }
    }
    
    Point p;
    for (int i=0;i!=nbin;i++){
      if (hc->GetBinContent(i+1)!=0){
	p.x = hx->GetBinContent(i+1)/hc->GetBinContent(i+1);
	p.y = hy->GetBinContent(i+1)/hc->GetBinContent(i+1);
	p.z = hz->GetBinContent(i+1)/hc->GetBinContent(i+1);
	points.push_back(p);
      }
    }
    delete hx;
    delete hy;
    delete hz;
    delete hc;
  }

  //at the end
  points.push_back(max_point);

  // std::cout << "qx: " << min_dis << " " << min_point.x/units::cm << " " << min_point.y/units::cm << " " << min_point.z/units::cm << " " 
  // 	    << max_dis << " " << max_point.x/units::cm << " " << max_point.y/units::cm << " " << max_point.z/units::cm << std::endl;
}

void WCP2dToy::WCCosmic::Sort(){
  //Sorting 
  std::vector<WCP2dToy::MSC_Struct> msc_vector;
  for (int i=0;i!=mcells.size();i++){
    msc_vector.push_back(WCP2dToy::MSC_Struct(cal_pos(mcells.at(i)),mcells.at(i)));
  }
  
  std::sort(msc_vector.begin(),msc_vector.end(),WCP2dToy::less_than_key());
  
  mcells.clear();
  for (int i=0;i!=msc_vector.size();i++){
    mcells.push_back(msc_vector.at(i).mcell);
  }
}

float WCP2dToy::WCCosmic::cal_pos(SpaceCell *cell){
  TVector3 dir(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
  TVector3 dir1(cell->x() - center.x,
		cell->y() - center.y,
		cell->z() - center.z);
  float dis1 = dir.Dot(dir1);
  return dis1;
}

float WCP2dToy::WCCosmic::cal_pos(MergeSpaceCell *mcell1){
  TVector3 dir(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
  TVector3 dir1(mcell1->Get_Center().x - center.x,
		mcell1->Get_Center().y - center.y,
		mcell1->Get_Center().z - center.z);
  float dis1 = dir.Dot(dir1);
  return dis1;
}


float WCP2dToy::WCCosmic::cal_dist(WCP2dToy::WCCosmic *cosmic){
  TVector3 dir1(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
  TVector3 dir2(sin(cosmic->get_theta())*cos(cosmic->get_phi()),
		sin(cosmic->get_theta())*sin(cosmic->get_phi()),
		cos(cosmic->get_theta()));
  TVector3 pos1(center.x, center.y, center.z);
  TVector3 pos2(cosmic->get_center().x, cosmic->get_center().y, cosmic->get_center().z);
  
  TVector3 perp1 = dir1.Cross(dir2);
  TVector3 perp2 = pos1 - pos2;
  float abc = perp1.Dot(perp2);

  float dis=1e9;
  if (perp1.Mag() == 0){
    TVector3 perp3 = perp2.Cross(dir1);
    dis = perp3.Mag()/dir1.Mag();
  }else{
    dis = fabs(abc/perp1.Mag());
  }

  return dis;
}

float WCP2dToy::WCCosmic::cal_costh(WCP2dToy::WCCosmic *cosmic){
  TVector3 dir1(sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
  TVector3 dir2(sin(cosmic->get_theta())*cos(cosmic->get_phi()),
		sin(cosmic->get_theta())*sin(cosmic->get_phi()),
		cos(cosmic->get_theta()));
  return dir1.Dot(dir2)/dir1.Mag()/dir2.Mag();
}


WCP2dToy::WCCosmic::~WCCosmic(){
  if (ct != 0)
    delete ct;
}
