//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TSimpleVertexSelector						//
//                                                                      //
// Selector class to estimate the geometric intersection of two tracks  //
// Intended to provide the initial vertex position for V0 fits          //
//                                                                      //
// Author List:                                                         //
// Marcel Kunze,  RUB, Feb. 99                                          //
// Copyright (C) 1999-2001, Ruhr-University Bochum.                     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "RhoSelector/TSimpleVertexSelector.h"
//#include "RhoManager/TPersistenceManager.h"
#include "RhoBase/TRho.h"
#include "RhoBase/TCandidate.h"

ClassImp(TSimpleVertexSelector)

TBuffer &operator>>(TBuffer &buf, TSimpleVertexSelector *&obj)
{
   obj = (TSimpleVertexSelector *) buf.ReadObject(TSimpleVertexSelector::Class());
   return buf;
}

#include <iostream>
using namespace std;

TSimpleVertexSelector::TSimpleVertexSelector(const char *name, Double_t d, Double_t a, Double_t r1, Double_t r2) :
  VAbsVertexSelector(name)//, fQC(0)
{
    fDoca = d;
    fVtxip = a;
    fRmin = r1;
    fRmax = r2;
}

TSimpleVertexSelector::~TSimpleVertexSelector() {};// delete fQC; }

Bool_t TSimpleVertexSelector::Accept(TCandidate& a, TCandidate& b) 
{
    if (&a==0 || &b==0) return kFALSE;
    
    Float_t bField = TRho::Instance()->GetMagnetField();	// Retrieve the B-Field
    
    // Position vectors
    TVector3 position1 = a.GetPosition();
    TVector3 position2 = b.GetPosition();
    
    // Momentum vectors
    TVector3 ap3 = a.P3();
    Double_t pPerp1 = ap3.Perp();
    TVector3 d1 = ap3;
    d1.SetZ(0);
    d1*=1.0/pPerp1;
    
    TVector3 bp3 = b.P3();
    Double_t pPerp2 = bp3.Perp();
    TVector3 d2 = bp3;
    d2.SetZ(0);
    d2*=1.0/pPerp2;
    
    
    TVector3 dB(0,0,1.0);
    // Radius and center
    Double_t rho1 = 100. * pPerp1/(0.3*bField); // Radius in cm
    TVector3 r1=d1.Cross(dB);
    r1 *= -a.Charge()*rho1;
    TVector3 center1 = position1 - r1;
    center1.SetZ(0);
    
    Double_t rho2 = 100. * pPerp2/(0.3*bField); // Radius in cm
    TVector3 r2=d2.Cross(dB);
    r2 *= -b.Charge()*rho2;
    TVector3 center2 = position2 - r2;
    center2.SetZ(0);
    
    // distance and angle of the axis between the two centers
    TVector3 ab = center2 - center1;
    Double_t dab = ab.Perp();
    Double_t cosTheAB = ab.X()/dab;
    Double_t sinTheAB = ab.Y()/dab;
    
    
    // x value of intersect at reduced system 
    Double_t x = dab/2 + ( rho1*rho1 - rho2*rho2 )/(2*dab); 
    
    // y*y value of intersect at reduced system for helix A
    Double_t y2 = (rho1+x)*(rho1-x); 
    
    // both circles do not intersect (only one solution)
    Int_t nSolMax=1;
    Double_t y=0; 
    
    if (y2 > 0) {
	nSolMax=2;
	y = sqrt(y2);
    }else{
	//if( fabs(dab-rho1-rho2) > fActualDoca);
	//return kFALSE;
    }
    
    // now we compute the solution(s)
    
    TVector3 newap3[2];
    TVector3 newbp3[2];
    TVector3 newapos[2];
    TVector3 newbpos[2];
    Int_t best=0;
    fActualDoca=1.E8;
    for (Int_t ns=0; ns<nSolMax; ns++){      // loop on the solutions
	
	// radius vector of intersection point
	Double_t sign = ns ? 1.0 : -1.0;
	TVector3 rs1( cosTheAB*x - sinTheAB*y * sign, sinTheAB*x + cosTheAB*y * sign, 0);  
	TVector3 rs2( rs1-ab );
	
	// are we moving forward or backward?
	Double_t adir=(rs1-r1).Dot(ap3)>0 ? 1.0 : -1.0;
	Double_t aangle=adir * r1.Angle(rs1);
	// intersection point
	Double_t newaz=position1.Z() + rho1*aangle/pPerp1 * ap3.Z();
	newapos[ns].SetX( center1.X() + rs1.X() );
	newapos[ns].SetY( center1.Y() + rs1.Y() );
	newapos[ns].SetZ( newaz );
	
	// adjust momentum
	newap3[ns]=rs1.Cross(dB);
	newap3[ns]*=a.Charge()/rho1*pPerp1;
	newap3[ns].SetZ(ap3.Z());
	
	
	// same for b
	Double_t bdir=(rs2-r2).Dot(bp3)>0 ? 1.0 : -1.0;
	Double_t bangle=bdir * r2.Angle(rs2);
	Double_t newbz=position2.Z() + rho2*bangle/pPerp2 * bp3.Z();
	newbpos[ns].SetX( center2.X() + rs2.X());   // ==newapos[ns].X()
	newbpos[ns].SetY( center2.Y() + rs2.Y());   // ==newapos[ns].Y()
	newbpos[ns].SetZ( newbz );
	newbp3[ns]=rs2.Cross(dB);
	newbp3[ns]*=b.Charge()/rho2*pPerp2;
	newbp3[ns].SetZ(bp3.Z());
	
	Double_t delta = (newapos[ns]-newbpos[ns]).Mag();
	
	// take the solution of minimal deltaZ
	if ( delta < fActualDoca ) {
	    best=ns;
	    fActualDoca  = delta;
	}
    }
    
    fVertex=0.5*(newapos[best]+newbpos[best]);
    fMomA=newap3[best];
    fMomB=newbp3[best];
    
    // Now reconstruct original flight path and
    // calculate the angle to vertex-ip
    
    TVector3 p=fMomA+fMomB;
    TVector3 vtxip = fVertex-fPrimaryVertex;
    fActualVtxip = vtxip.Angle(p);
    fActualR = vtxip.Mag();
    
    //if (fQC!=0) fQC->Accumulate((Float_t)fActualDoca,(Float_t)fActualVtxip);
    
    if ( fActualDoca < fDoca && 
	 fActualR >= fRmin && fActualR < fRmax && 
	 fActualVtxip < fVtxip ){
      
      return kTRUE;
      
    }else{
      fMomA=ap3;
      fMomB=bp3;
      fVertex = fPrimaryVertex;
      return kFALSE;
    }
    return kFALSE; //get rid of warnings
}

/*
void TSimpleVertexSelector::ActivateQualityControl()
{
    TPersistenceManager *persmgr = TRho::Instance()->GetPersistenceManager();
    if (persmgr == 0) {
	std::cerr << "TVertexSelector: Can not activate QC w/o PersistenceManager." << std::endl;
	return;
    }

    // Make a quality control directory, if needed and cd into it

    persmgr->SetDir("QC/TVertexSelector");    
    fQC = persmgr->Histogram(GetName(),100,0.0,10.,100,0.0,3.14159265358979323846);
}
*/