#include <TLine3D.hh>
#include <TTrajectory3D.hh>

#include <math.h>
#include <iostream>
#include <iomanip>
#include <climits>


const double TTrajectory3D::ECOARSE = 10.0;
const double TTrajectory3D::COARSE  =  1.0;
const double TTrajectory3D::FINE    =  0.01;
const double TTrajectory3D::EFINE   =  0.00001;
const double TTrajectory3D::EEFINE  =  0.000001;
const double TTrajectory3D::COMP_PREC  =  EEFINE/10.;

const double TTrajectory3D::NUM_PREC = 1.0e-15; // empirical

ClassImp(TTrajectory3D)
//_____________________________________________________________________________
TTrajectory3D::TTrajectory3D(){
}

//_____________________________________________________________________________
TTrajectory3D::~TTrajectory3D(){
}

//_____________________________________________________________________________
void TTrajectory3D::GetLocation(TTrajectory3D::Location & loc, double s) const {
  // Return a Location object containing position and location data. This
  // should be overriden in subclasses with obtimized versions -- the whole
  // point is to cut down on calculations common to getPosition and
  // getDirection.
  // If you find you're calling this default (and *slow*) routine, then
  // implement an optimized version for the trajectory concerned. See
  // THelix3D::getLocation(...) for an example.

  TVector3 pos;
  TVector3 dir; 

  GetPosition(pos,s);
  GetDirection(dir,s);
  loc.setLocation(s,pos,dir);
}

//_____________________________________________________________________________
void TTrajectory3D::getTangent(TLine3D& line, double s) const {
  TTrajectory3D::Location location;
  GetLocation(location,s);
  line.Init(location.position(),location.direction());
}

//_____________________________________________________________________________
void TTrajectory3D::GetPointAtRho(TTrajectoryPoint* point, double rho) const {
  double s = GetPathLengthAtRho(rho);
  GetPoint(point,s);
}  

//_____________________________________________________________________________
void TTrajectory3D::GetPointAtZ(TTrajectoryPoint* point, double z) const {
  double s = GetPathLengthAtZ(z);
  GetPoint(point,s);
}
  
//_____________________________________________________________________________
double TTrajectory3D::getPathLengthTo(const TVector3 &point) const {
  double s = 0;
  const int MAXTRIES=200;
  TTrajectory3D::Location location; 
  //  for (int i=0;i<10;i++) {
  for (int ntries=0 ; ntries<MAXTRIES ; ntries++) {
    GetLocation(location,s);
    TVector3 xp = location.position();
    double DeltaS = (point -xp)*location.direction();
    if ((float) fabs(s+DeltaS)!=(float) fabs(s)) {
      s += DeltaS;
    }
    else {
      return s;
    }
  }
  return 0;
}



//_____________________________________________________________________________
double TTrajectory3D::getPathLengthTo(const TTrajectory3D &traj) const {
  //-------------------------------------------------------------------------//
  //                                                                         //
  //   A bit more complicated than the basic method, {see                    //
  //   TLine3D::getPathLengthTo(const TLine3D &) const}because the procedure //
  //   becomes iterative on arbitrary trajectories whose tangent vector      //
  //   varies with path length.  But basically the same calculation within a //
  //   loop.                                                                 //
  //                                                                         //
  //-------------------------------------------------------------------------//
#ifdef __GNUG__
  double s0=0,s1=0,s0Prev=0,s1Prev=0,dXPrev=HUGE,f=1.0;
#else
  double s0=0,s1=0,s0Prev=0,s1Prev=0,dXPrev=std::numeric_limits<double>::max(),f=1.0;
#endif
  TTrajectory3D::Location x0Loc, x1Loc;
  for (int i=0;i<20;i++) {
    GetLocation(x0Loc,s0);
    traj.GetLocation(x1Loc,s1);
    TVector3   deltaX =x1Loc.position()-x0Loc.position();
    double       d0Dotd1=x0Loc.direction().Dot(x1Loc.direction());
    double       delta0 = (d0Dotd1*deltaX.Dot(x1Loc.direction())-
                           deltaX.Dot(x0Loc.direction()))/(d0Dotd1*d0Dotd1-1);
    double delta1 = (-d0Dotd1*deltaX.Dot(x0Loc.direction())+
                     deltaX.Dot(x1Loc.direction()))/(d0Dotd1*d0Dotd1-1);

    if (fabs(delta0)<EFINE && fabs(delta1)<EFINE)
      {
        return s0;
      }
    double dxMag = deltaX.Mag();
    
    // We have in fact increased.  Do not move forward, move back, reduce 
    // Reduce the factor and try again..
    //
    if (dxMag>dXPrev) {
      f /= 2.0;
      s0 = s0Prev;
      s1 = s1Prev;
      continue;
    }

    s0+=(f*delta0);
    s1+=(f*delta1);
    s0Prev=s0;
    s1Prev=s1;
    dXPrev=dxMag;
  }
  return s0;
}

//_____________________________________________________________________________
double TTrajectory3D::getDzeroTo(const TVector3 &point) const {
  return 0;
}


// This is a signed quantity.  The sign is the *opposite* of the
// angular momentum of this trajectory about the POCA of the 
// second trajectory, in the direction of the second
// trajectory. It's the same convention as the CDF D0, if you 
// consider the D0 to be the signed POCA w.r.t a trajectory 
// moving towards the positive z axis along the beam.
//
// Oh, subclasses!  You are exhorted to follow this convention!
// Exhort! Exhort! Exhort! Exhort! Exhort! Exhort! Exhort! Exhort! 
//_____________________________________________________________________________
double TTrajectory3D::getDzeroTo(const TTrajectory3D &traj) const {

  double s0=0,s1=0;
  TTrajectory3D::Location x0Loc,x1Loc;
  for (int i=0;i<100;i++) {
    GetLocation(x0Loc,s0);
    traj.GetLocation(x1Loc,s1);    
    TVector3   deltaX=x1Loc.position()-x0Loc.position();
    double       d0Dotd1=x0Loc.direction().Dot(x1Loc.direction());
    double       delta0 = (d0Dotd1*deltaX.Dot(x1Loc.direction())
                           -deltaX.Dot(x0Loc.direction()))/(d0Dotd1*d0Dotd1-1);
    double delta1 = (-d0Dotd1*deltaX.Dot(x0Loc.direction())
                     +deltaX.Dot(x1Loc.direction()))/(d0Dotd1*d0Dotd1-1);
    
    if (fabs(delta0)<EFINE && fabs(delta1)<EFINE)
      {
        TVector3 R=x0Loc.position()-x1Loc.position();
        TVector3 likeZ=x1Loc.direction();
        TVector3 rotR=x0Loc.direction().Cross(R);
        double sgn = rotR.Dot(likeZ) > 0 ? 1.0:-1.0;
        return sgn*rotR.Mag();
        //        return s0;
      }
    s0+=delta0;
    s1+=delta1;
  }
  return 0.0;
} 


//_____________________________________________________________________________
//  TTrajectory3D::Location* 
//  TTrajectory3D::newIntersectionWith(const HepPlane3D &plane) const {
//    double deltaS=1.0,s=0.0;
//    TVector3 normal = plane.normal();
//    TTrajectory3D::Location *ploc = new TTrajectory3D::Location ;
//    for (int iteration=0;iteration<100;iteration++) {
//      if (((float)fabs(s+deltaS)!=(float)fabs(s))){
//        getLocation(*ploc,s);
//        deltaS=((-(plane.distance(ploc->position())))/
//  	      (normal.dot(ploc->direction()))); 
//        s+=deltaS;
//      }
//      else {
//        if (s>0) return ploc;
//      }
//    }
//    delete ploc;
//    return NULL;
//  }

//_____________________________________________________________________________
void TTrajectory3D::Location::Print(Option_t* opt) {
  printf("%12.5e : ",__s);
  printf(" %12.5e %12.5e %12.5e, ",
	 _position.X(),
	 _position.Y(),
	 _position.Z());
  printf(" %12.5e %12.5e %12.5e ",
	 _direction.X(),
	 _direction.Y(),
	 _direction.Z());
}