#include "WCPData/Singleton.h"
#include "WCPData/TPCParams.h"
#include "WCPData/ClusterTrack.h"
#include "TVector3.h"

using namespace WCP;


ClusterTrack::ClusterTrack(MergeSpaceCell *cell){
  all_mcells.push_back(cell);
  
}

ClusterTrack::~ClusterTrack(){
  // delete hough;
  // for (int i =0; i!=all_mcells.size();i++){
  //   delete all_mcells.at(i);
  // }
  // all_mcells.clear();
}

Point ClusterTrack::SC_IterativeHough(Point &p, float dis, int flag){
  // do three times Hough
  //first hough
  SC_Hough(p,dis, flag);
  float theta = Get_Theta();
  float phi = Get_Phi();
  Point p1;
  p1.x = p.x + dis * sin(theta) * cos(phi);
  p1.y = p.y + dis * sin(theta) * sin(phi);
  p1.z = p.z + dis * cos(theta);

  //second hough
  SC_Hough(p1,p,dis, flag);
  theta = Get_Theta();
  phi = Get_Phi();
  p1.x = p1.x + dis * sin(theta) * cos(phi);
  p1.y = p1.y + dis * sin(theta) * sin(phi);
  p1.z = p1.z + dis * cos(theta);

  //third hough
  SC_Hough(p1,p,dis,flag);
  theta = Get_Theta();
  phi = Get_Phi();
  p1.x = p1.x + dis * sin(theta) * cos(phi);
  p1.y = p1.y + dis * sin(theta) * sin(phi);
  p1.z = p1.z + dis * cos(theta);

    
  return p1;
}


int ClusterTrack::CrossNum(MergeSpaceCell *mcell1, float theta, float phi){
  int result = 0;

  float x1 = mcell1->Get_Center().x;
  float y1 = mcell1->Get_Center().y;
  float z1 = mcell1->Get_Center().z;
 
  float x2 = x1 + sin(theta) * cos(phi);
  float y2 = y1 + sin(theta) * sin(phi);
  float z2 = z1 + cos(theta);

  
 
  for (int i=0;i!=all_mcells.size();i++){
    MergeSpaceCell *mcell = all_mcells.at(i);

    bool result1 = false;
    
    double min_dis = 1e9;
    if (mcell == mcell1){
      result1 = true;
    }else{
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	
	
	float x,y,z;
	x = cell->x();
	y = cell->y();
	z = cell->z();
	
	double dis;
      
	TVector3 v1(x-x1,y-y1,z-z1);
	TVector3 v2(x-x2,y-y2,z-z2);
	TVector3 v3(x2-x1,y2-y1,z2-z1);
	TVector3 v4 = v1.Cross(v2);
	dis = v4.Mag()/v3.Mag();
	
	if (dis < min_dis ) min_dis = dis;
	//std::cout << "Xin1: " << dis/units::mm << std::endl;
	
	
	if (dis < Singleton<TPCParams>::Instance().get_pitch()){
	  result1 = true;
	  break;
	}
      }
    }
    
    //std::cout << i << " " << result1 << " " << min_dis/units::mm << std::endl;
    if (result1)
      result ++;
    
  }
  
  return result;
}




int ClusterTrack::CrossNum(Point &p, float theta, float phi){
  int result = 0;

  float x1 = p.x;
  float y1 = p.y;
  float z1 = p.z;
 
  float x2 = x1 + sin(theta) * cos(phi);
  float y2 = y1 + sin(theta) * sin(phi);
  float z2 = z1 + cos(theta);

  
 
  for (int i=0;i!=all_mcells.size();i++){
    MergeSpaceCell *mcell = all_mcells.at(i);

    bool result1 = false;
    
    double min_dis = 1e9;
  
    for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
      SpaceCell *cell = mcell->Get_all_spacecell().at(j);
      

      float x,y,z;
      x = cell->x();
      y = cell->y();
      z = cell->z();

      double dis;
      
      TVector3 v1(x-x1,y-y1,z-z1);
      TVector3 v2(x-x2,y-y2,z-z2);
      TVector3 v3(x2-x1,y2-y1,z2-z1);
      TVector3 v4 = v1.Cross(v2);
      dis = v4.Mag()/v3.Mag();
      
      if (dis < min_dis ) min_dis = dis;
      //std::cout << "Xin1: " << dis/units::mm << std::endl;

     
      if (dis < Singleton<TPCParams>::Instance().get_pitch()){
	result1 = true;
	break;
      }
    }
    
    //std::cout << i << " " << result1 << " " << min_dis/units::mm << std::endl;
    if (result1)
      result ++;
    
  }
  
  return result;
}

bool ClusterTrack::AddMSCell_anyway(MergeSpaceCell *cell){
  all_mcells.push_back(cell);
  return true;
}

bool ClusterTrack::AddMSCell(MergeSpaceCell *cell){
  bool result = false;
  if (all_mcells.size()==1){
    all_mcells.push_back(cell);
    result = true;
  }else{
    Point p1 = all_mcells.at(all_mcells.size()-2)->Get_Center();
    Point p2 = all_mcells.at(all_mcells.size()-1)->Get_Center();
    Point p3 = cell->Get_Center();

    float theta1_old = atan((p2.y-p1.y)/(p2.x-p1.x))/3.1415926*180.;
    float theta2_old = atan((p2.z-p1.z)/(p2.x-p1.x))/3.1415926*180.;

    float theta1_new = atan((p3.y-p2.y)/(p3.x-p2.x))/3.1415926*180.;
    float theta2_new = atan((p3.z-p2.z)/(p3.x-p2.x))/3.1415926*180.;
    // if (sqrt(pow(theta1_new-theta1_old,2)+pow(theta2_new-theta1_old,2))<360.){
    //   result = true;
    //   all_mcells.push_back(cell);
    // }else{
    //   result = false;
    // }

    if (cell->Get_all_spacecell().size()<3*all_mcells.at(all_mcells.size()-1)->Get_all_spacecell().size() 
        ){
      result = true;
      all_mcells.push_back(cell);
    }else{
      result = false;
    }



  }
  return result;
}

void ClusterTrack::SC_Hough(Point&p1, Point&p, float dis, int flag){
  TH2F *hough = new TH2F("","",180,0.,3.1415926,360,-3.1415926,3.1415926);
  
  double x0 = p.x;
  double y0 = p.y;
  double z0 = p.z;
  
  double x1 = p1.x;
  double y1 = p1.y;
  double z1 = p1.z;

  double x,y,z,q;
  for (int i=0;i!=all_mcells.size();i++){
    MergeSpaceCell *mcell = all_mcells.at(i);
    
    if (flag == 1){
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	
	x = cell->x();
	y = cell->y();
	z = cell->z();
	//q = cell->q();
	// set charge into 1
	q = 1;
	
	TVector3 vec(x-x1,y-y1,z-z1);
	// sc_theta.push_back(vec.Theta());
	// sc_phi.push_back(vec.Phi());
	// sc_q.push_back(q);
	if (dis <= 0){
	  hough->Fill(vec.Theta(),vec.Phi(),q);
	}else{
	  if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	    hough->Fill(vec.Theta(),vec.Phi(),q);
	}
      }
    }else if (flag==2){
      x = mcell->Get_Center().x;
      y = mcell->Get_Center().y;
      z = mcell->Get_Center().z;
      //q = mcell->Get_Charge();
      q = mcell->Get_all_spacecell().size();

      TVector3 vec(x-x1,y-y1,z-z1);
      // sc_theta.push_back(vec.Theta());
      // sc_phi.push_back(vec.Phi());
      // sc_q.push_back(q);
      if (dis <= 0){
	hough->Fill(vec.Theta(),vec.Phi(),q);
      }else{
	if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	  hough->Fill(vec.Theta(),vec.Phi(),q);
      }
    }

  }
  
  int maxbin = hough->GetMaximumBin();
  int a,b,c;
  hough->GetBinXYZ(maxbin,a,b,c);
  theta_hough = hough->GetXaxis()->GetBinCenter(a);
  phi_hough = hough->GetYaxis()->GetBinCenter(b);
  
  delete hough;
}


void ClusterTrack::SC_Hough(Point& p, float dis, int flag){
  TH2F *hough = new TH2F("","",180,0.,3.1415926,360,-3.1415926,3.1415926);
  double x0 = p.x;
  double y0 = p.y;
  double z0 = p.z;
  double x,y,z,q;
  for (int i=0;i!=all_mcells.size();i++){
    MergeSpaceCell *mcell = all_mcells.at(i);

    if (flag == 1){
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	
	x = cell->x();
	y = cell->y();
	z = cell->z();
	//q = cell->q();
	q = 1;
	
	
	TVector3 vec(x-x0,y-y0,z-z0);
	// sc_theta.push_back(vec.Theta());
	// sc_phi.push_back(vec.Phi());
	// sc_q.push_back(q);
	if (dis <= 0){
	  hough->Fill(vec.Theta(),vec.Phi(),q);
	}else{
	  if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	    hough->Fill(vec.Theta(),vec.Phi(),q);
	}
      }
    }else if (flag == 2){
      x = mcell->Get_Center().x;
      y = mcell->Get_Center().y;
      z = mcell->Get_Center().z;
      //q = mcell->Get_Charge();
      q = mcell->Get_all_spacecell().size();
	
	
      TVector3 vec(x-x0,y-y0,z-z0);
      // sc_theta.push_back(vec.Theta());
      // sc_phi.push_back(vec.Phi());
      // sc_q.push_back(q);
      if (dis <= 0){
	hough->Fill(vec.Theta(),vec.Phi(),q);
      }else{
	if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	  hough->Fill(vec.Theta(),vec.Phi(),q);
      }
    }else if (flag == 3){
      if (fabs(mcell->Get_Center().x - p.x) < 0.1 * mcell->thickness()) continue;
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	
	x = cell->x();
	y = cell->y();
	z = cell->z();
	//q = cell->q();
	q = 1;
	
	
	TVector3 vec(x-x0,y-y0,z-z0);
	// sc_theta.push_back(vec.Theta());
	// sc_phi.push_back(vec.Phi());
	// sc_q.push_back(q);
	if (dis <= 0){
	  hough->Fill(vec.Theta(),vec.Phi(),q);
	}else{
	  if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	    hough->Fill(vec.Theta(),vec.Phi(),q);
	}
      }
    }else if (flag == 4){
      for (int j=0;j!=mcell->Get_all_spacecell().size();j++){
	SpaceCell *cell = mcell->Get_all_spacecell().at(j);
	
	x = cell->x();
	y = cell->y();
	z = cell->z();
	//q = cell->q();
	q = 1/mcell->Get_all_spacecell().size();
	
	
	TVector3 vec(x-x0,y-y0,z-z0);
	// sc_theta.push_back(vec.Theta());
	// sc_phi.push_back(vec.Phi());
	// sc_q.push_back(q);
	if (dis <= 0){
	  hough->Fill(vec.Theta(),vec.Phi(),q);
	}else{
	  if (sqrt(pow(x-x0,2)+pow(y-y0,2)+pow(z-z0,2))<dis)
	    hough->Fill(vec.Theta(),vec.Phi(),q);
	}
      }
    }

  }

  //std::cout << hough->GetSum() << std::endl;

  int maxbin = hough->GetMaximumBin();
  int a,b,c;
  hough->GetBinXYZ(maxbin,a,b,c);
  theta_hough = hough->GetXaxis()->GetBinCenter(a);
  phi_hough = hough->GetYaxis()->GetBinCenter(b);


  delete hough;
  
}

float ClusterTrack::Get_Theta(){
  return theta_hough;
}


float ClusterTrack::Get_Phi(){
  
  return phi_hough;
}


float ClusterTrack::Get_CTheta(Point &p){
  double x0 = p.x;
  double y0 = p.y;
  double z0 = p.z;

  Point p1 = all_mcells.front()->Get_Center();
  Point p2 = all_mcells.back()->Get_Center();
  
  double x,y,z;
  
  if (sqrt(pow(p1.x-x0,2)+pow(p1.y-y0,2)+pow(p1.z-z0,2))<
      sqrt(pow(p2.x-x0,2)+pow(p2.y-y0,2)+pow(p2.z-z0,2))){
    x = p2.x;
    y = p2.y;
    z = p2.z;
  }else{
    x = p1.x;
    y = p1.y;
    z = p1.z;
  }
  TVector3 vec(x-x0,y-y0,z-z0);
  return vec.Theta();
  
  
}

float ClusterTrack::Get_CPhi(Point &p){
  double x0 = p.x;
  double y0 = p.y;
  double z0 = p.z;

  Point p1 = all_mcells.front()->Get_Center();
  Point p2 = all_mcells.back()->Get_Center();
  
  double x,y,z;
  
  if (sqrt(pow(p1.x-x0,2)+pow(p1.y-y0,2)+pow(p1.z-z0,2))<
      sqrt(pow(p2.x-x0,2)+pow(p2.y-y0,2)+pow(p2.z-z0,2))){
    x = p2.x;
    y = p2.y;
    z = p2.z;
  }else{
    x = p1.x;
    y = p1.y;
    z = p1.z;
  }
  TVector3 vec(x-x0,y-y0,z-z0);
  return vec.Phi();
}
