// -------------------------------------------------------------------------
// -----                        CbmGeanePro source file                -----
// -----                  Created 08/12/06  by M. Al-Turany            -----
// -------------------------------------------------------------------------

#include "CbmGeanePro.h"

#include "CbmTrackParP.h"
#include "CbmTrackParH.h"
#include "CbmRunAna.h"
#include "CbmField.h"

#include "TGeant3TGeo.h"
#include "TVector3.h"
#include "TArrayD.h"
#include "TDatabasePDG.h"
#include "TGeoTorus.h"
#include "TMatrixFSym.h"
#include "TVirtualMC.h"
#include "TGeant3.h"

#include <iostream>

using std::cout;
using std::endl;

// -----   Default constructor   -------------------------------------------
CbmGeanePro::CbmGeanePro() : TNamed("Geane", "Propagate Tracks") 
{ 
	gMC3 = (TGeant3*) gMC;        
	nepred=1;
	fdbPDG= TDatabasePDG::Instance();
	fErrorMat= new TArrayD(15);
	afErtrio=gMC3->fErtrio;
	Pos=TVector3(0,  0 , 0);
	PosErr = TVector3(0,0,0);
	Mom=TVector3(0,0,0);
        fTrkPar= new CbmTrackPar();
        ProMode=0;
        CbmRunAna *fRun= CbmRunAna::Instance();
        fField= fRun->GetField();
	fPCA = 0;
}

// -----   Destructor   ----------------------------------------------------
CbmGeanePro::~CbmGeanePro() { }



Bool_t CbmGeanePro::Propagate(CbmTrackParH *TParam, CbmTrackParH *TEnd, Int_t PDG)
{
 // cout << "CbmGeanePro::Propagate(CbmTrackParH *TParam, CbmTrackParH &TEnd, Int_t PDG)" << endl;
  /**Propagate a helix track and return a helix*/
  Bool_t NeedSDSC=kFALSE;
  Float_t ch;   //        CHARGE OF PARTICLE

  Double_t fCov[15], fCovOut[15];
  TParam->GetCov(fCov);
  
  Init(TParam);
  Double_t Q = TParam->GetQ();
  if (Q!=0)ch= Q/TMath::Abs(Q);
  if (ProMode==1){ //Propagate to Volume  
     /** We have the right representation go further*/
     for(Int_t i=0;i<15;i++) {
	ein[i]=fCov[i]; 
        //cout << "ein  " <<   ein[i]     << endl;
     }
    if(fPropOption == "VE") {
      cout << "Propagate Helix to Volume" << endl;
      Int_t option;
      if(VEnter)option =1;
      else  option =2;
      gMC3->Eufilv(1, ein, (Char_t *)VName.Data(), &VCopyNo, &option);
    }
    else if(fPropOption == "LE") {
      if(fPCA == 0)  cout << "Propagate Helix to Length not yet implemented" << endl;
      else if(fPCA != 0){
	//	cout << "Propagate Helix to Minimum Distance" << endl;

	// max length estimate:
	// we calculate the geometrical distance of the start point
	// from the point/wire extremity and multiply it * 2 
	TVector3 start = TVector3(TParam->GetX(), TParam->GetY(), TParam->GetZ());
	Double_t maxdistance = 0;
	if(fPCA == 1) maxdistance = (fpoint - start).Mag();
	else if(fPCA == 2) {
	  Double_t distance1, distance2;
	  distance1 = (fwire1 - start).Mag();
	  distance2 = (fwire2 - start).Mag();
	  if(distance1 < distance2) maxdistance = distance2;
	  else maxdistance = distance1;
	}
	maxdistance *= 2.; 

	
	// output 
	FindPCA(fPCA, PDG, fpoint, fwire1, fwire2, maxdistance, Rad, vpf, vwi, Di, trklength);
	// reset parameters
	Init(TParam);
	gMC3->Eufill(nepred, ein, &trklength);
      }
    }
  }else if(ProMode ==3){ 
    cout << "Propagate Helix parameter to Plane is not implimented yet" << endl;
    return kFALSE;
  }
   /**Propagate */
  Propagate(PDG);
 
     for(Int_t i=0;i<15;i++) {
	fCovOut[i]=afErtrio->errout[i]; 
     }
  
     //  if(p2[0] == 0 && p2[1] == 0 && p2[2] == 0) return kFALSE; // CHECK da cancellare!!!!!!
     TEnd->SetTrackPar(x2[0], x2[1], x2[2],p2[0],p2[1],p2[2], ch ,fCovOut );              
     return kTRUE;

}                                                 
Bool_t CbmGeanePro::Propagate(CbmTrackParP *TStart, CbmTrackParH *TEnd, Int_t PDG)
{
  cout << "CbmGeanePro::Propagate(CbmTrackParP *TParam, CbmTrackParH &TEnd, Int_t PDG)" << endl;
}
// Bool_t CbmGeanePro::Propagate(CbmTrackParP *TStart, CbmTrackParP *TEnd, Int_t PDG)
// {
//  // cout << "CbmGeanePro::Propagate(CbmTrackParP *TParam, CbmTrackParP &TEnd, Int_t PDG)" << endl;

//   /**Propagate a parabola track and eturn a parabola */
//   Float_t ch;   //        CHARGE OF PARTICLE
//   Double_t fCov[15], fCovOut[15];
//   TStart->GetCov(fCov);
  
//   Init(TStart);
//   Double_t Q = TStart->GetQ() ;
//   cout << "START Q: " << Q << endl;
//   if (Q!=0)ch= Q/TMath::Abs(Q);
 
//   if (ProMode==1){ //Propagate to Volume  
//      cout << "Propagate Parabola parameter to Volume is not implimented yet" << endl;        
//     return kFALSE;
   
//   }else if(ProMode ==3){ 
//           /** We have the right representation go further*/
          
//      for(Int_t i=0;i<15;i++) {
// 	ein[i]=fCov[i]; 
//      //   cout << "ein  " <<   ein[i]     << endl;
//      }


//      gMC3->Eufilp(nepred, ein, pli, plo);

  
//   }
//    /**Propagate */
//   Propagate(PDG);
 
//   for(Int_t i=0;i<15;i++) {
//       fCovOut[i]=afErtrio->errout[i]; 
//   }

 
//   TEnd->SetTrackPar(x2[0], x2[1], x2[2],p2[0],p2[1],p2[2], ch ,fCovOut );              
//   return kTRUE;

// }

// --------------- modifica 28 jul 2007 -------------------------
Bool_t CbmGeanePro::Propagate(CbmTrackParP *TStart, CbmTrackParP *TEnd, Int_t PDG)
{
 // cout << "CbmGeanePro::Propagate(CbmTrackParP *TParam, CbmTrackParP &TEnd, Int_t PDG)" << endl;

  /**Propagate a parabola track and return a parabola */
  Float_t ch;   //        CHARGE OF PARTICLE
  Double_t fCov[15], fCovOut[15];
  TStart->GetCov(fCov);
  
  Init(TStart);
  Double_t Q = TStart->GetQ() ;
  if (Q!=0)ch= Q/TMath::Abs(Q);
 
  if (ProMode==1){ //Propagate to Volume  
    cout << "Propagate Parabola parameter to Volume is not implimented yet" << endl;        
    return kFALSE;
  }
  else if(ProMode ==3){ 
    /** We have the right representation go further*/
    for(Int_t i=0;i<15;i++) {
      ein[i]=fCov[i]; 
      // cout << "ein  " <<   ein[i]     << endl;
    }

    if(fPropOption == "PE") gMC3->Eufilp(nepred, ein, pli, plo);
    else if(fPropOption == "LE") {
      if(fPCA == 0) cout << "Propagate Parabola to Parabola in Length not yet implemented" << endl;
      else if(fPCA != 0){
	//	cout << "Propagate Parabola to Parabola in Minimum Distance" << endl;
	// max length estimate:
	// we calculate the geometrical distance of the start point
	// from the point/wire extremity and multiply it * 2 
	TVector3 start = TVector3(TStart->GetX(), TStart->GetY(), TStart->GetZ());
	Double_t maxdistance = 0;
	if(fPCA == 1) maxdistance = (fpoint - start).Mag();
	else if(fPCA == 2) {
	  Double_t distance1, distance2;
	  distance1 = (fwire1 - start).Mag();
	  distance2 = (fwire2 - start).Mag();
	  if(distance1 < distance2) maxdistance = distance2;
	  else maxdistance = distance1;
	}
	maxdistance *= 2.; 

	// output 
	FindPCA(fPCA, PDG, fpoint, fwire1, fwire2, maxdistance, Rad, vpf, vwi, Di, trklength);
	// reset parameters
	Init(TStart);

	// find plane
	// unitary vector along distance
	// vpf on track, vwi on wire
	TVector3 fromwiretoextr = vpf - vwi;      // CHECK
	fromwiretoextr.SetMag(1.);
	// unitary vector along the wire
	TVector3 wiredirection = fwire2 - fwire1; // CHECK
	// check orthogonality	   
	if(fromwiretoextr * wiredirection > 1e-10) {
	  return kFALSE; // throw away the event
	  //cout << "PRODOTTO SCALARE " << fromwiretoextr * wiredirection << " " << flag << endl;
	  wiredirection = (fromwiretoextr.Cross(wiredirection)).Cross(fromwiretoextr);
	}
	wiredirection.SetMag(1.);

	TVector3 jver = TStart->GetJVer();;
	TVector3 kver = TStart->GetKVer();
	PropagateFromPlane(jver, kver);
	PropagateToPlane(vwi, fromwiretoextr, wiredirection);
	
	gMC3->Eufilp(nepred, ein, pli, plo);
      }
    }
  }
  /**Propagate */
  Propagate(PDG);
  
  for(Int_t i=0;i<15;i++) {
    fCovOut[i]=afErtrio->errout[i]; 
  }
  
  // TEnd->SetTrackPar(x2[0], x2[1], x2[2],p2[0],p2[1],p2[2], ch ,fCovOut );              
  // plane
  TVector3 origin(plo[6], plo[7], plo[8]);
  TVector3 dj(plo[0], plo[1], plo[2]);
  TVector3 dk(plo[3], plo[4], plo[5]);
  TVector3 di(plo[9], plo[10], plo[11]); // = dj.Cross(dk);
  
  if(fabs(p2[0])==0. && fabs(p2[1])==0. && fabs(p2[2])==0) return kFALSE;
  //  cout << "fCovOut " << fCovOut[0] << " " << fCovOut[5] << " " << fCovOut[9] << " " << fCovOut[12] << " " << fCovOut[14] << endl;
  TEnd->SetTrackPar(x2[0], x2[1], x2[2],p2[0],p2[1],p2[2], ch ,fCovOut, origin, di, dj, dk);
  
  return kTRUE;

}


Bool_t CbmGeanePro::Propagate(CbmTrackParH *TStart, CbmTrackParP *TEnd, Int_t PDG)
{
        cout << "CbmGeanePro::Propagate(CbmTrackParH *TParam, CbmTrackParP &TEnd, Int_t PDG)" << endl;
	return kTRUE;  
}

void CbmGeanePro::Propagate(Int_t PDG)
{
   GeantCode=fdbPDG->ConvertPdgToGeant3(PDG);
//    cout << "  x1 before " << x1[0] << " "<< x1[1]<< " " << x1[2] << endl;
//    cout << "  x2 before " << x2[0] << " "<< x2[1]<< " " << x2[2] << endl;
//    cout << "  p1 before " << p1[0] << " "<< p1[1]<< " " << p1[2] << endl;
//    cout << "  p2 before " << p2[0] << " "<< p2[1]<< " " << p2[2] << endl;
/*
   for(Int_t i=0;i<15;i++) {
	afErtrio->ertrsp[i] =0; 
   }
  */
   gMC3->Ertrak(x1,p1,x2,p2,GeantCode, fPropOption.Data());
 /*  cout << "  x1 " << x1[0] << " "<< x1[1]<< " " << x1[2] << endl;
   cout << "  x2 " << x2[0] << " "<< x2[1]<< " " << x2[2] << endl;
   cout << "  p1 " << p1[0] << " "<< p1[1]<< " " << p1[2] << endl;
   cout << "  p2 " << p2[0] << " "<< p2[1]<< " " << p2[2] << endl;
   cout << "GeantCode " <<  GeantCode << " PDG " <<   PDG <<  " fPropOption " <<  fPropOption.Data() << endl;
   for(Int_t i=0;i<15;i++) {
	//fCovOut[i]=afErtrio->errout[i]; 
        cout << "CbmGeanePro::Propagate " << "errout[" << i << "] = " << (Float_t) afErtrio->errout[i] << endl;
   }
*/
}

void CbmGeanePro::Init(CbmTrackPar *TParam)
{
  
  x1[0]=TParam->GetX();
  x1[1]=TParam->GetY();
  x1[2]=TParam->GetZ();
  p1[0]=TParam->GetPx();
  p1[1]=TParam->GetPy();
  p1[2]=TParam->GetPz();

//  cout << " Init(CbmTrackPar *TParam)  p1 " << p1[0] << " "<< p1[1]<< " " << p1[2] << endl;
  x2[0]=0;
  x2[1]=0;
  x2[2]=0;
  p2[0]=0;
  p2[1]=0;
  p2[2]=0;

}



Bool_t CbmGeanePro::PropagateFromPlane(TVector3 &v1, TVector3 &v2)
{
	TVector3 v1u=v1.Unit();
	TVector3 v2u=v2.Unit();
	pli[0]=v1u.X();
	pli[1]=v1u.Y();
	pli[2]=v1u.Z();
	pli[3]=v2u.X();
	pli[4]=v2u.Y();
	pli[5]=v2u.Z();
	return kTRUE;
}

Bool_t CbmGeanePro::PropagateToPlane(TVector3 &v0, TVector3 &v1, TVector3 &v2)
{
  
  //	for(Int_t i=0;i<15;i++) ein[i]=0.00; 
	TVector3 v1u=v1.Unit();
	TVector3 v2u=v2.Unit();
	

	plo[0]=v1u.X(); 
	plo[1]=v1u.Y();
	plo[2]=v1u.Z();
	plo[3]=v2u.X();
	plo[4]=v2u.Y();
	plo[5]=v2u.Z();
 
	plo[6]=v0.X();
	plo[7]=v0.Y();
	plo[8]=v0.Z();

	TVector3 v3=v1u.Cross(v2u); // CHECK CONTROLLA COSI' LA TERNA E' DESTRORSA

	plo[9]=v3(0);
	plo[10]=v3(1);
	plo[11]=v3(2);

	fPropOption="PE";
        ProMode=3;  //need errors in representation 3 (SD)(see Geane doc)
//	gMC3->Eufilp(nepred, ein, pli, plo);
	return kTRUE;
}
Bool_t CbmGeanePro::PropagateToVolume(TString VolName, Int_t CopyNo , Int_t option)
{
	for(Int_t i=0;i<15;i++) ein[i]=0.00; 
        VName= VolName;
        VCopyNo= CopyNo;
        if(option==1)VEnter=kTRUE;
	else  VEnter=kFALSE;
	fPropOption="VE";
        ProMode=1;  //need errors in representation 1 (SC) (see Geane doc)
	return kTRUE;
} 

Bool_t CbmGeanePro::PropagateToLength(Float_t length)
{
	Float_t xlf[1];
	xlf[0]=length;
	fPropOption="LE";
        ProMode=1; //need errors in representation 1 (SC)(see Geane doc)
	//gMC3->Eufill(nepred, ein,xlf);
	return kTRUE;
}
// ------------------------------- aggiunta jun 2007 -------------------------

Bool_t CbmGeanePro::SetWire(TVector3 extremity1, TVector3 extremity2)
{
  fwire1 = extremity1;
  fwire2 = extremity2;
  return kTRUE;
}

Bool_t CbmGeanePro::SetPoint(TVector3 pnt)
{
  fpoint = pnt;
  return kTRUE;
}

Bool_t CbmGeanePro::PropagateToPCA(Int_t pca)
{
  // through track length
  fPropOption="LE";
  ProMode=1; //need errors in representation 1 (SC)(see Geane doc)   
  fPCA = pca;
  // initialization (forse non necessario) CHECK!!!!
  Rad = 0.;
  Di = 0.;
  vpf = TVector3(0.,0.,0.);
  vwi = TVector3(0.,0.,0.);
  trklength = 0;
  return kTRUE;
}

Bool_t CbmGeanePro::PropagateToVirtualPlaneAtPCA(Int_t pca)
{
  // through track length
  fPropOption="LE";
  ProMode=3; //need errors in representation 3 (SD)(see Geane doc) 
  fPCA = pca;
  // initialization 
  Rad = 0.;
  Di = 0.;
  vpf = TVector3(0.,0.,0.);
  vwi = TVector3(0.,0.,0.);
  trklength = 0;
  return kTRUE;
}
//=====================

// find the point of closest approach of the track to a point(measured position) or to a line(wire)
int CbmGeanePro::FindPCA(Int_t pca, Int_t PDGCode, TVector3 point, TVector3 wire1, TVector3 wire2, Double_t maxdistance, Double_t &Rad, TVector3 &vpf, TVector3 &vwi, Double_t &Di, Float_t &trklength)
{
  // INPUT ----------------------------------------
  // .. pca = ic = 1 closest approach to point
  //        = 2 closest approach to wire
  //        = 0 no closest approach
  // .. PDGCode = pdg code of the particle
  // .. point point with respect to which calculate the closest approach
  // .. wire, wire2 line with respect to which calculate the closest approach
  // .. maxdistance = geometrical distance[start - point/wire extr] * 2
  // OUTPUT ----------------------------------------
  // .. Rad radius if the found circle
  // .. vpf: point of closest approach on track
  // .. vwi: point of closest approach on wire
  // .. Di : distance between track and wire in the PCA
  // .. trklength : track length to add to the GEANE one

  Float_t pf[3] = {point.X(), point.Y(), point.Z()};
  Float_t w1[3] = {wire1.X(), wire1.Y(), wire1.Z()};
  Float_t w2[3] = {wire2.X(), wire2.Y(), wire2.Z()};

  GeantCode=fdbPDG->ConvertPdgToGeant3(PDGCode);
  
  // flags delle funzioni di rotondi
  int flg;
  
 // cl track length to the three last points of closest approach
  // dst assigned distance between initial point in ERTRAK and PFINAL along straight line (currently noy used)
  Float_t cl[3],dst;
  
  // punti riempiti da GEANE
  Float_t po1[3],po2[3],po3[3];
  
  // cl track length to the three last points of closest approach
  Float_t clen[3];
  
  // track length to add to GEANE computed one 
  Double_t Le;
  Double_t dist1,dist2;
  //      cout << "Find point of closest approach" << endl;
      
      // initialization of some variables
      dst = 999.;
      cl[0] = 0; cl[1] = 0; cl[2] = 0;
      
      // punti riempiti da GEANE
      po1[0]=0;po1[1]=0;po1[2]=0;
      po2[0]=0;po2[1]=0;po2[2]=0;
      po3[0]=0;po3[1]=0;po3[2]=0;
     
      gMC3->SetClose(pca,pf,dst,w1,w2,po1,po2,po3,cl);
    
      // maximum distance calculated 2 * geometric distance
      // start point - end point (the point to which we want
      // to find the PCA)
      Float_t stdlength[1] = {maxdistance};
   
      gMC3->Eufill(1, ein, stdlength);
      gMC3->Ertrak(x1,p1,x2,p2,GeantCode, fPropOption.Data());
      gMC3->GetClose(po1,po2,po3,clen);
      



      //  Double_t pc[3],rc[15],h[3],ch,dj[3],dk[3],spu,pd[3],rd[15];
      if(pca == 1) 
	{
	  if(      po1[0] == po2[0] && po1[1] == po2[1] && po1[2] == po2[2]
		   || po2[0] == po3[0] && po2[1] == po3[1] && po2[2] == po3[2])
	    {
	      Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(pf),vpf,Di,Le);
	    }
	  else
	    {
		Track3ToPoint(TVector3(po1),TVector3(po2),TVector3(po3),TVector3(pf),vpf,flg,Di,Le,Rad);
		if(flg==1) Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(pf),vpf,Di,Le);
	    }
	}
      else if(pca == 2) 
	{
	  if(      po1[0] == po2[0] && po1[1] == po2[1] && po1[2] == po2[2]
		   || po2[0] == po3[0] && po2[1] == po3[1] && po2[2] == po3[2])
	    {
	      Track2ToLine(TVector3(po1),TVector3(po3),TVector3(w1),
			   TVector3(w2),vpf,vwi,flg,Di,Le);
	      if(flg==1) 
		{
		  dist1 = (vwi-TVector3(w1)).Mag();
		  dist2 = (vwi-TVector3(w2)).Mag();
		  
		  dist1<dist2?Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w1),vpf,Di,Le):
		    Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w2),vpf,Di,Le);
		}
	      else if(flg==2)
		{
		  cout << "No solution exist!" << endl;
		  return 1;
		}
	    }
	  else 
	    {     
	      Track3ToLine(TVector3(po1),TVector3(po2),TVector3(po3),
			   TVector3(w1),TVector3(w2),vpf,vwi,flg,Di,Le,Rad);
	      if(flg==1)
		{
		  Track2ToLine(TVector3(po1),TVector3(po3),TVector3(w1),
			       TVector3(w2),vpf,vwi,flg,Di,Le);
		  if(flg==1) 
		    {
		      dist1 = (vwi-TVector3(w1)).Mag();
		      dist2 = (vwi-TVector3(w2)).Mag();
		      
		      dist1<dist2?Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w1),vpf,Di,Le):
			Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w2),vpf,Di,Le);
		    }
		  else if(flg==2)
		    {
		      cout << "No solution exist!" << endl;
		      return 1;
		    }
		}
	      else if(flg==2)
		{
		  dist1 = (vwi-TVector3(w1)).Mag();
		  dist2 = (vwi-TVector3(w2)).Mag();
		  
		  dist1<dist2?Track3ToPoint(TVector3(po1),TVector3(po2),TVector3(po3),TVector3(w1),vpf,flg,Di,Le,Rad):
		    Track3ToPoint(TVector3(po1),TVector3(po2),TVector3(po3),TVector3(w2),vpf,flg,Di,Le,Rad);
		}
	      else if(flg==3)
		{
		  dist1 = (vwi-TVector3(w1)).Mag();
		  dist2 = (vwi-TVector3(w2)).Mag();
		  
		  dist1<dist2?Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w1),vpf,Di,Le):
		    Track2ToPoint(TVector3(po1),TVector3(po3),TVector3(w2),vpf,Di,Le);
		  
		}
	      else if(flg==4)
		{
		  Track2ToLine(TVector3(po1),TVector3(po3),TVector3(w1),
			       TVector3(w2),vpf,vwi,flg,Di,Le);
		  
		}
	      
	    }
	}
      
      // calculated track length corresponding 
      // to the point of closest approach
      trklength = clen[0]+Le;
      
      //      cout << "CLOSEST: clen " << clen[0] << " Le " << Le << " position " << vpf[0] << " " << vpf[1] << " " << vpf[2] << endl;
      //      cout << "FLAG " << flg << endl;
      flag = flg;

      return 0;
}


void CbmGeanePro::Track2ToLine( TVector3 x1,  TVector3 x2,  TVector3 w1,
		   TVector3 w2,  TVector3 &Pfinal, TVector3 &Pwire,
                   Int_t &Iflag, Double_t &Dist, Double_t &Length) {

  // Closest approach to a line from 2 GEANE points
  //
  // METHOD: the nearest points on the two lines
  //         x1,x2 and w1,w2 is found.
  //         The method is described in:
  //  http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
  //  http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf.
  //
  // INPUT: x1, x2   closest appoach GEANE points 
  //        w1, w2   points of the line  (wire)  to approach 
  //
  // OUTPUT: Pfinal  point of closest approach on the track
  //         Pwire    point of closest approach on the wire
  //         Dist    distance between Pfian and w1
  //         Length  arc length to add to the GEANE length of x1
  //         Iflag   =1 when Pwire is outside [w1,w2]
  //                 In this case, when w1 and w2 are the extremes
  //                 of the wire, the user could remake the procedure
  //                 by calling Track3ToPoint or Track2ToPoint, where the
  //                 Point is w1 or w2;
  //                 = 2 when the two lines are parallel and the solution
  //                 does not exists.
  //
  // Authors: Andrea Fontana and Alberto Rotondi 20 MAy 2007
  //


  TVector3 x21, x32, w21;
  TVector3 xw1, xw2;

  Double_t a1, b1, c1, d1, e1, t1, s1;
  Double_t Delta1;

  Double_t Eps = 1.E-08;
  
  Iflag =0;

  // line-line distance
  x21 = x2-x1;
  w21 = w2-w1;

  xw1 = x1-w1;
  xw2 = x2-w1;

  a1 =  x21.Mag2();  
  b1 =  x21.Dot(w21);
  c1 =  w21.Mag2();    
  d1 =  xw1.Dot(x21); 
  e1 =  xw1.Dot(w21);
 
  Delta1 = a1*c1-b1*b1;

  if(Delta1 > Eps) {
    t1 = (a1*e1-b1*d1)/Delta1;
    s1 = (b1*e1-c1*d1)/Delta1;

    Pfinal = (x1 + x21*s1);
    Pwire  = (w1 + w21*t1);
    Length = s1*x21.Mag();
    Dist= (Pfinal-Pwire).Mag();
    
  }
  else {
    // lines are paralllel, no solution does exist
    Pfinal.SetXYZ(0.,0.,0.);
    Pwire.SetXYZ(0.,0.,0.);
    Dist=0.;
    Length=0.;
    Iflag = 2;
    return;
  }
  // flag when the point on the wire is outside (w1,w2)
  if((((Pwire[0]<w1[0] && Pwire[0]<w2[0]) || (w2[0]<Pwire[0] && w1[0]<Pwire[0])) 
      && (fabs(Pwire[0]-w1[0]) > 1e-11 && fabs(Pwire[0]- w2[0]) > 1e-11))
     || (((Pwire[1]<w1[1] && Pwire[1]<w2[1]) || (w2[1]<Pwire[1] && w1[1]<Pwire[1])) 
	 && (fabs(Pwire[1]-w1[1]) > 1e-11 && fabs(Pwire[1]- w2[1]) > 1e-11))
     || (((Pwire[2]<w1[2] && Pwire[2]<w2[2]) || (w2[2]<Pwire[2] && w1[2]<Pwire[2])) 
	 && (fabs(Pwire[2]-w1[2]) > 1e-11 && fabs(Pwire[2]- w2[2]) > 1e-11)))
   {
	Iflag=1;  
   }
}



void CbmGeanePro::Track2ToPoint( TVector3 x1,  TVector3 x2,  TVector3 w1, TVector3 &Pfinal, 
                    Double_t &Dist, Double_t &Length)  {

  //
  // Closest approach to a point from 2 GEANE points
  //
  // METHOD: the nearest point to w1 on the line x1,x2
  //         is found. Elementary vector calculus is used!
  //
  // INPUT: x1, x2     closest appoach GEANE points 
  //        w1         point to approach w1
  //
  // OUTPUT: Pfinal  point of closest approach 
  //         Dist    distance between Pfinal and w1
  //         Length  arc length to add to the GEANE length of x1
  //
  // Authors: Andrea Fontana and Alberto Rotondi  May 2007
  //


  TVector3 u21;

  Double_t a, t1;

  // w-point - x-line distance

  a= 1./(x2-x1).Mag();
  u21 = (x2-x1).Unit();

  // output
  Dist= ((w1-x1).Cross(u21)).Mag();
  t1  =  a*(w1-x1).Dot(u21);
  Pfinal = (x2-x1)*t1 + x1;
  Length = (x2-x1).Mag()*t1;
}


void CbmGeanePro::Track3ToLine(TVector3 x1, TVector3 x2, TVector3 x3, 
			       TVector3 w1, TVector3 w2,
			       // output
			       TVector3 &Pfinal, TVector3 &Wire, 
			       Int_t &Iflag, Double_t &Dist, 
			       Double_t &Length, Double_t &Radius) {

  // Find the closest approach points between a curve (helix) 
  // and a line (wire)
  //
  // METHOD:the classical Eberly method is used: see
  //  http://www.geometrictools.com/Documentation/DistanceLine3Circle.pdf.
  //        (see also www.geometrictools.com for the other formulae used 
  //        in this interface).
  //        The 4-degree polynomial resulting for the line parameter t is
  //        solved with the efficient SolveQuartic Root routine.
  //        The minimal distance solution is found by using our Track3ToPoint
  //        routine
  //
  // INPUT: x1, x2, x3  closest appoach GEANE points 
  //        w1, w2      points of the line (wire) to approach 
  //
  // OUTPUT: Pfinal  track point of closest approach 
  //         Wire    line point of closest approach
  //         Iflag   = 1, the points are on a straight line
  //                 within the precision of the method (20 micron).
  //                 In this case the user should recall Track2ToPoint
  //                 = 2, Pwire is outside [w1,w2]
  //                 In this case, when w1 and w2 are the extremes
  //                 of the wire, the user could remake the procedure
  //                 by calling Track3ToPoint, where the Point is w1 or w2.
  //                 = 3 both conditions 1 and 2 are encountered
  //                 = 4, the method failed for mathematical reasons.
  //                 In this case the user should use Track2ToLine where
  //                 x1=x1 and x2=x3.
  //         Dist    distance between Pfinal and Wire
  //         Length  arc length to add to the GEANE length of x1
  //         Radius  radius of the found circle 
  //
  // Authors: Andrea Fontana and Alberto Rotondi 20 June  2007
  //

  TVector3 xp1, xp2, xp3, xp32;
  TVector3 x21, x31;
  TVector3 e1, e2, e3, aperp;
  TVector3 Ppfinal, Pwire;
  TVector3 wp1, wp2, wpt, xR, xpR;
  TVector3 xw1, xw2;
  TVector3 p1, p2, px; 

  Double_t T[3][3], TM1[3][3];

  TVector3 N, M, D, B, Pointw;
  Double_t a0, a1, b0, b1, c0, c1, c2;
  Double_t d0, d1, d2, d3, d4, sol4[4], dist4[4], dmin;
  Double_t Angle;
  Double_t dx;
  Int_t it, imin;

  Iflag = 0;

  // go to the circle plane: matrix of director cosines

  x21 = x2-x1;
  e1 = x21.Unit();
  T[0][0] = e1.X();
  T[0][1] = e1.Y();
  T[0][2] = e1.Z();

  x31 = x3-x1;
  e3 = e1.Cross(x31);
  e3 = e3.Unit();
  
  T[2][0] = e3.X();
  T[2][1] = e3.Y();
  T[2][2] = e3.Z();

  e2 = e3.Cross(e1);
  T[1][0] = e2.X();
  T[1][1] = e2.Y();
  T[1][2] = e2.Z();
 
  // new coordinates
  for(Int_t i=0; i<3; i++) {
    xp1[i]=0.;
    xp2[i]=0.;
    xp3[i]=0.;
    wp1[i]=0.;
    wp2[i]=0.;
  }
  for(Int_t i=0; i<3; i++) {
    xp1[i] = 0.;
    for(Int_t j=0; j<3; j++) {
      TM1[i][j] = T[j][i];
      xp2[i] += T[i][j] * (x2[j]-x1[j]);
      xp3[i] += T[i][j] * (x3[j]-x1[j]);
      wp1[i] += T[i][j] * (w1[j]-x1[j]);
      wp2[i] += T[i][j] * (w2[j]-x1[j]);
    }
  }

  // radius and center

  xp32= xp3 - xp2;
  xpR[0] = 0.5*xp2[0];
  xpR[1] = 0.5*(xp32[0]*xp3[0]/xp3[1]+ xp3[1]);
  xpR[2] = 0.; 
  Radius = sqrt(pow(xpR[0]-xp1[0],2)+pow(xpR[1]-xp1[1],2));
 
  // Eberly's method

  B = wp1;
  M = wp2 - wp1;
  D = wp1-xpR; 
  N.SetXYZ(0.,0.,1.);

  a0 = M.Dot(D);
  a1 = M.Dot(M);
  b0 = M.Dot(D) -(N.Dot(M))*(N.Dot(D));
  b1 = M.Dot(M) -(N.Dot(M))*(N.Dot(M));
  c0 = D.Dot(D) -(N.Dot(D))*(N.Dot(D));
  c1 = b0;
  c2 = b1;

  d0 = a0*a0*c0 -b0*b0*Radius*Radius;
  d1 = 2.*(a0*a1*c0+a0*a0*c1-b0*b1*Radius*Radius);
  d2 = a1*a1*c0+4.*a0*a1*c1+a0*a0*c2-b1*b1*Radius*Radius;
  d3 = 2.*(a1*a1*c1+a0*a1*c2);
  d4 = a1*a1*c2; 
 
  // solve the quartic equation
  for(Int_t k=0; k<4; k++){ 
    sol4[k] =0.; 
  }
  if(d4 == 0.) {
    Iflag = 4;
    return;
  }

  TGeoTorus t; 
  it = t.SolveQuartic(d3/d4,d2/d4,d1/d4,d0/d4,sol4);

  if(it==0) {
    Iflag = 4;
    return;
  }
  
  // select the right solution
  dmin = 1.e+08;
  imin=-1;
  for(Int_t j=0; j<it; j++){
    Pointw[0] = B[0] + sol4[j]*M[0];
    Pointw[1] = B[1] + sol4[j]*M[1];
    Pointw[2] = B[2] + sol4[j]*M[2];
    Track3ToPoint(xp1,xp2,xp3, Pointw, px, Iflag, dx, Length, Radius);
    if(dx<dmin){
      dmin=dx;
      imin=j;
    }
  }

  // final solution
  Pwire[0] = B[0] + sol4[imin]*M[0];
  Pwire[1] = B[1] + sol4[imin]*M[1];
  Pwire[2] = B[2] + sol4[imin]*M[2];
  Track3ToPoint(xp1,xp2,xp3, Pwire, px, Iflag, dx, Length, Radius);

  // output: distance and points in the circle plane reference

  Dist = dx;
  Ppfinal = px;

  //
  // back to lab coordinates
  //

  xR[0]=0.;
  xR[1]=0.;
  xR[2]=0.;
  Pfinal[0]=0.;
  Pfinal[1]=0.;
  Pfinal[2]=0.;
  Wire[0]=0.;
  Wire[1]=0.;
  Wire[2]=0.;

  for(Int_t i=0; i<3; i++) {
   for(Int_t j=0; j<3; j++) {
     Pfinal[i] += TM1[i][j] * Ppfinal[j];
     Wire[i]   += TM1[i][j] * Pwire[j];
     xR[i]     += TM1[i][j] * xpR[j];
   }
  }
  Pfinal = Pfinal+x1;
  Wire   = Wire + x1;
  xR     = xR + x1;

  // now find the length
  Angle = TMath::ACos((x1-xR).Dot(Pfinal-xR)/((x1-xR).Mag()*(Pfinal-xR).Mag()));
  Length = Radius*Angle;
  if((x2-x1).Dot(Pfinal-x1) < 0.) Length = -Length;

 // flag straight points within 20 microns
  Double_t epsi = Radius*(1.-TMath::Cos(0.5*(x3-x1).Mag()/Radius));
  if(epsi < 0.0020) Iflag=1;

  // flag when the point on the wire is outside (w1,w2)
  if((((Wire[0]<w1[0] && Wire[0]<w2[0]) || (w2[0]<Wire[0] && w1[0]<Wire[0]))
      && (fabs(Wire[0]-w1[0]) > 1e-11 && fabs(Wire[0]- w2[0]) > 1e-11))
     || (((Wire[1]<w1[1] && Wire[1]<w2[1]) || (w2[1]<Wire[1] && w1[1]<Wire[1])) 
	 && (fabs(Wire[1]-w1[1]) > 1e-11 && fabs(Wire[1]- w2[1]) > 1e-11))
     || (((Wire[2]<w1[2] && Wire[2]<w2[2]) || (w2[2]<Wire[2] && w1[2]<Wire[2])) 
	 && (fabs(Wire[2]-w1[2]) > 1e-11 && fabs(Wire[2]- w2[2]) > 1e-11)))
    { 
        Iflag+=2;
    }  
}

/*  OLD ONE 
    void CbmGeanePro::Track2ToLine( TVector3 x1,  TVector3 x2,  TVector3 w1,
    TVector3 w2,  TVector3 &Pfinal, TVector3 &Pwire,
    Int_t &Iflag, Double_t &Dist, Double_t &Length) {
    
    // Closest approach to a line from 2 GEANE points
    //
    // METHOD: the nearest points on the two lines
    //         x1,x2 and w1,w2 is found.
    //         The method is described in:
    //  http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
    //
    // INPUT: x1, x2   closest appoach GEANE points 
    //        w1, w2   points of the line  (wire)  to approach 
    //
    // OUTPUT: Pfinal  point of closest approach on the track
    //         Pwire    point of closest approach on the wire
    //         Dist    distance between Pfian and w1
    //         Length  arc length to add to the GEANE length of x1
    //         Iflag   is =1 when Pwire is outside [w1,w2]
    //                 In this case, when w1 and w2 are the extremes
    //                 of the wire, the user could remake the procedure
    //                 by calling Track3ToPoint or Track2ToPoint, where the
    //                 Point is w1 or w2.
    //
    // Authors: Andrea Fontana and Alberto Rotondi 20 May 2007
    //
    
    
    TVector3 x21, x32, w21;
    TVector3 xw1, xw2;
    
    Double_t a1, b1, c1, d1, e1, t1, s1;
    Double_t Delta1;
    
    Double_t Eps = 1.E-08;
    
    Iflag = 0;
    
    // line-line distance
    x21 = x2-x1;
    w21 = w2-w1;
    
    xw1 = x1-w1;
    xw2 = x2-w1;
    
    a1 =  x21.Mag2();  
    b1 =  x21.Dot(w21);
    c1 =  w21.Mag2();    
    d1 =  xw1.Dot(x21); 
    e1 =  xw1.Dot(w21);
    
    Delta1 = a1*c1-b1*b1;
    
    if(Delta1 > Eps) {
    t1 = (a1*e1-b1*d1)/Delta1;
    s1 = (b1*e1-c1*d1)/Delta1;
    
    Pfinal = (x1 + x21*s1);
    Pwire  = (w1 + w21*t1);
    Length = s1*x21.Mag();
    Dist= (Pfinal-Pwire).Mag();
    
    }
    else {
    // lines are paralllel, no solution does exist
    Pfinal.SetXYZ(0.,0.,0.);
    Pwire.SetXYZ(0.,0.,0.);
    Dist=0.;
    Length=0.;
    }
    // flag when the point on the wre is outside (w1,w2)
    if((Pwire[0]<w1[0] || w2[0]<Pwire[0]) ||
    (Pwire[1]<w1[1] || w2[1]<Pwire[1]) ||
    (Pwire[2]<w1[2] || w2[2]<Pwire[2])    ) Iflag=1;  
    }
    
    void CbmGeanePro::Track2ToPoint( TVector3 x1,  TVector3 x2,  TVector3 w1, TVector3 &Pfinal, 
    Double_t &Dist, Double_t &Length)  {
    
    //
    // Closest approach to a point from 2 GEANE points
    //
    // METHOD: the nearest point to w1 on the line x1,x2
    //         is found. Elementary vector calculus is used!
    //
    // INPUT: x1, x2     closest appoach GEANE points 
    //        w1         point to approach w1
    //
    // OUTPUT: Pfinal  point of closest approach 
    //         Dist    distance between Pfinal and w1
    //         Length  arc length to add to the GEANE length of x1
    //
    // Authors: Andrea Fontana and Alberto Rotondi  May 2007
    //
    
    
    TVector3 u21;
    
    Double_t a, t1;
    
    // w-point - x-line distance
    
    a= 1./(x2-x1).Mag();
    u21 = (x2-x1).Unit();
    
    // output
    Dist= ((w1-x1).Cross(u21)).Mag();
    t1  =  a*(w1-x1).Dot(u21);
    Pfinal = (x2-x1)*t1 + x1;
    Length = (x2-x1).Mag()*t1;
    }
    
    
    void CbmGeanePro::Track3ToLine(TVector3 x1, TVector3 x2, TVector3 x3, 
    TVector3 w1, TVector3 w2,
    // output
    TVector3 &Pfinal, TVector3 &Wire, Int_t &Iflag1, Int_t &Iflag2, 
    Double_t &Dist, Double_t &Length, Double_t &Radius) {
    
    // Find the closest approach points between a curve (helix) 
    // and a line (wire)
    //
    // METHOD: the aproximate point on the circle is found  
    //         by the routine Track3ToLineAx (see description there).
    //         Then, an iterative procedure starts to find the closest
    //         approach point by successive calls to Track3ToPoint to
    //         find the point on the circle and to Track2ToPoint to find 
    //         the point on the wire. The iteration stops when the
    //         distance is at the minimum .
    //
    // INPUT: x1, x2, x3  closest appoach GEANE points 
    //        w1, w2      points of the line (wire) to approach 
    //
    // OUTPUT: Pfinal  track point of closest approach 
    //         Wire    line point of closest approach
    //         Iflag1  is =1 when Pwire is outside [w1,w2]
    //                 In this case, when w1 and w2 are the extremes
    //                 of the wire, the user could remake the procedure
    //                 by calling Track3ToPoint or Track2ToPoint, where the
    //                 Point is w1 or w2.
    //         Iflag2  when =1 the points are on a straight line
    //                 within the precision of the method (20 micron).
    //                 In this case the user should recall Track2ToLine
    //         Dist    distance between Pfinal and Wire
    //         Length  arc length to add to the GEANE length of x1
    //
    // Authors: Andrea Fontana and Alberto Rotondi
    //
    
    TVector3 xp1, xp2, xp3, xp32;
    TVector3 x21, x31;
    TVector3 e1, e2, e3, aperp;
    TVector3 Ppfinal, Pwire, x32, Wire1;
    TVector3 wp1, wp2, wpt, xR, xpR;
    TVector3 xw1, xw2;
    TVector3 p1, p2, px; 
    TVector3 xc1, xc2, xc3, wc1, wc2, xcc;
    
    
    Iflag1 =0;
    Iflag2 =0;
    // find the first appproximation
    
    int flg;
    Track3ToLineAx(x1, x2, x3, w1, w2, Pfinal, Wire, Dist, Length, Radius); 
    
    // find the closest pont to Wire on the circle
    Track3ToPoint(x1, x2, x3, Wire, Pfinal, flg, Dist, Length, Radius);
    
    // iterative procedure to optimize the solution
    Double_t Prec = 1.e+07;
    for(Int_t i=0; i<20; i++) {
    Track2ToPoint(w1, w2, Pfinal, Wire, Dist, Length);
    Track3ToPoint(x1, x2, x3, Wire, Pfinal, flg, Dist, Length, Radius);
    if(TMath::Abs((Dist-Prec)/Prec) <1.e-04) break;
    Prec=Dist;
    }
    
    // flag straight points within 20 microns
    Double_t epsi = Radius*(1.-TMath::Cos(0.5*(x3-x1).Mag()/Radius));
    if(epsi < 0.0020) Iflag2=1;
    
    // flag when the point on the wire is outside [w1,w2]
    if((Wire[0]<w1[0] || w2[0]<Wire[0]) ||
    (Wire[1]<w1[1] || w2[1]<Wire[1]) ||
    (Wire[2]<w1[2] || w2[2]<Wire[2])    ) Iflag1=1;  
    }
*/


void CbmGeanePro::Track3ToPoint( TVector3 x1, TVector3 x2, TVector3 x3, TVector3 w1, 
				 // output
				 TVector3 &Pfinal, Int_t &Iflag, 
				 Double_t &Dist, Double_t &Length, Double_t &Radius) {
  
  // Closest approach to a point from 3 GEANE points
  //
  // METHOD: first, we go on the circle plane to have
  //         x1=(0,0), x2=(x2,0), x3=(x3,y3).
  //         Then, the point on the circle is found as the
  //         intersection between the circle and the line 
  //         joining the circle center and the projection of
  //         the point on the circle plane.
  //         The 3D distance is found between w1 and this
  //         point on the circle.
  // 
  // INPUT: x1, x2, x3 closest appoach GEANE points 
  //        w1         point to approach 
  //
  // OUTPUT: Pfinal  point of closest approach on the track
  //         Dist    distance between Pfinal and w1
  //         Length  arc length to add to the GEANE length of x1
  //         Iflag   when =1 the points are on a straight line
  //                 within the precision of the method (20 micron).
  //                 In this case the user should recall Track2ToPoint
  //
  // Authors: Andrea Fontana and Alberto Rotondi 20 May 2007
  //


  TVector3 xp1, xp2, xp3, xp32;
  TVector3 x21, x31;
  TVector3 e1, e2, e3;
  TVector3 Ppfinal, x32;
  TVector3 wp1, wpt, xR, xpR;
  TVector3 xw1, xw2;

  TVector3 xc1, xc2, xc3, wc1;

  Double_t m1, m3, Rt;
  Double_t T[3][3], TM1[3][3];

  Double_t Angle;


  Iflag = 0;

  // go to the circle plane with origin in x1 prime (xp1):
  // matrix of director cosines

  x21 = x2-x1;
  m1 = 1./x21.Mag();
  e1 = m1*x21;
  T[0][0] = e1.X();
  T[0][1] = e1.Y();
  T[0][2] = e1.Z();

  x31 = x3-x1;
  e3 = e1.Cross(x31);
  m3 = 1./e3.Mag();
  e3 = m3*e3;
  T[2][0] = e3.X();
  T[2][1] = e3.Y();
  T[2][2] = e3.Z();

  e2 = e3.Cross(e1);
  T[1][0] = e2.X();
  T[1][1] = e2.Y();
  T[1][2] = e2.Z();

  // new coordinates
  for(Int_t i=0; i<3; i++){
    xp1[i]=0.;
    xp2[i]=0.;
    xp3[i]=0.;
    wp1[i]=0.;
  }
  for(Int_t i=0; i<3; i++) {
    for(Int_t j=0; j<3; j++) {
      TM1[i][j] = T[j][i];
      xp1[i] += 0.;
      xp2[i] += T[i][j] * (x2[j]-x1[j]);
      xp3[i] += T[i][j] * (x3[j]-x1[j]);
      wp1[i] += T[i][j] * (w1[j]-x1[j]);
    }
  }

  // radius Radius and center xpR

  xp32= xp3 - xp2;
  xpR[0] = 0.5*xp2[0];
  xpR[1] = 0.5*(xp32[0]*xp3[0]/xp3[1]+ xp3[1]);
  xpR[2] = 0.;
 
  Radius = sqrt( pow(xpR[0]-xp1[0],2) + pow(xpR[1]-xp1[1],2) );

  // distance and points
  wpt = wp1;
  wpt[2] =0.;   // point projection on the circle plane
  Rt = Radius/(wpt-xpR).Mag();
  Ppfinal = (wpt-xpR)*Rt + xpR;
  Dist = (wp1-Ppfinal).Mag();
  
  // back to lab coordinates: 
  //from Ppfinal to Pfinal and from xpR to xR

  xR[0]=0.;
  xR[1]=0.;
  xR[2]=0.;
  Pfinal[0]=0.;
  Pfinal[1]=0.;
  Pfinal[2]=0.;
  for(Int_t i=0; i<3; i++) {
   for(Int_t j=0; j<3; j++) {
     Pfinal[i] += TM1[i][j] * Ppfinal[j];
     xR[i]     += TM1[i][j] * xpR[j];
   }
  }
  Pfinal = Pfinal+x1;
  xR = xR +x1;

  // now find the length
  Angle = TMath::ACos((x1-xR).Dot(Pfinal-xR)/((x1-xR).Mag()*(Pfinal-xR).Mag()));
  Length = Radius*Angle;

  // flag straight points within 20 microns
  Double_t epsi = Radius*(1.-TMath::Cos(0.5*(x3-x1).Mag()/Radius));
  if(epsi < 0.0020) Iflag=1;
}






ClassImp(CbmGeanePro)