#include <math.h>

#include <TTrajectoryPoint.hh>
#include <TLine3D.hh>

ClassImp(TLine3D)

//_____________________________________________________________________________
TLine3D::TLine3D() :
  _z0(0.0),
  _d0(0.0),
  _phi0(0.0)
{
  _sinTheta=1.0;
  _cosTheta=0.0;
  _sinPhi0=0.0;
  _cosPhi0=1.0;
}

//_____________________________________________________________________________
TLine3D::TLine3D(const TLine3D &right)
  :_sinTheta(right._sinTheta),
   _cosTheta(right._cosTheta),
   _z0(right._z0),
   _d0(right._d0),
   _phi0(right._phi0),
   _sinPhi0(right._sinPhi0),
   _cosPhi0(right._cosPhi0)
{
}

//_____________________________________________________________________________
TLine3D::TLine3D(const TVector3 &point, const TVector3 &direction) {
  Init(point,direction);
}

//_____________________________________________________________________________
void TLine3D::Init(const TVector3 &point, const TVector3 &direction) {

  if (direction.Perp2()<NUM_PREC) { 
					// This is the pathological case, 
					// parallel to z-axis.
    _sinTheta=0.0;
    _cosTheta = direction.Z()>0 ? 1.0:-1.0;
    _d0=point.Perp();
    _sinPhi0= -point.X()/_d0;
    _cosPhi0=  point.Y()/_d0;
    _phi0=atan2(_sinPhi0,_cosPhi0);
    _z0=0;
  } 
  else if (fabs(direction.Z())<NUM_PREC) { 

					// this is another one, perpendicular
    _sinTheta = 1.0;
    _cosTheta = 0.0;
    _d0 = (direction.Cross(point)).Z();
    _phi0             = direction.Phi();
    _sinPhi0          = sin(_phi0);
    _cosPhi0          = cos(_phi0);
    _z0 = point.Z();
  }
  else {
					// This is the normal case
    TVector3 dPerp=direction;
    dPerp.SetZ(0);
    dPerp.SetMag(1.0);
    
    double dirz = direction.Z();
    double theta = dirz>0 ? atan(direction.Perp()/dirz) 
                          : M_PI + atan(direction.Perp()/dirz);
    _sinTheta=sin(theta);
    _cosTheta=cos(theta);
    _d0               = (dPerp.Cross(point)).Z();
    _phi0             = direction.Phi();
    _sinPhi0          = sin(_phi0);
    _cosPhi0          = cos(_phi0);

    double s          = dPerp.Dot(point);
    double sprime     = s/_sinTheta;
    _z0               = (point - sprime*direction).Z();

#define  THESE_ARE_BOTH_BOGUS
#ifndef  THESE_ARE_BOTH_BOGUS
#ifdef ORIGINAL
    double x=-_d0*_sinPhi0;
    //    double y= _d0*_cosPhi0;
    double delta_x = x-point.X();
    //    double delta_y = y-point.Y();
    double alpha_x = direction.x();
    //    double alpha_y = direction.Y();
    double alpha_z = direction.Z();
    _z0 = point.z() + (delta_x/alpha_x)*alpha_z;
#else
    double x=-_d0*_sinPhi0;
    double y= _d0*_cosPhi0;
    double delta_x = x-point.X();
    double delta_y = y-point.Y();
    double trans = sqrt(delta_x*delta_x + delta_y*delta_y);
    _z0 = point.Z() + trans*_cosTheta/_sinTheta;
    
#endif
#endif
  }


}

//_____________________________________________________________________________
void TLine3D::Init(TTrajectoryPoint* Point) {
  // initialize line starting from a trajectory point

  TVector3* dir = Point->GetDirection();
  TVector3* pos = Point->GetPosition ();

  if (dir->Perp2()<NUM_PREC) { 
					// This is the pathological case, 
					// parallel to z-axis.
    _sinTheta =0.0;
    _cosTheta = dir->Z()>0 ? 1.0:-1.0;
    _d0       = pos->Perp();
    _sinPhi0  = -pos->X()/_d0;
    _cosPhi0  =  pos->Y()/_d0;
    _phi0     = atan2(_sinPhi0,_cosPhi0);
    _z0       = 0;
  } 
  else if (fabs(dir->Z())<NUM_PREC) { 
					// this is another one, perpendicular
    _sinTheta = 1.0;
    _cosTheta = 0.0;
    _d0       = (dir->Cross(*pos)).Z();
    _phi0     = dir->Phi();
    _sinPhi0  = sin(_phi0);
    _cosPhi0  = cos(_phi0);
    _z0       = pos->Z();
  }
  else {
					// This is the normal case
    TVector3 dPerp = *dir;
    dPerp.SetZ(0);
    dPerp.SetMag(1.0);
    
    double dirz  = dir->Z();
    double theta = dirz>0 ? atan(dir->Perp()/dirz) 
                          : M_PI + atan(dir->Perp()/dirz);
    _sinTheta    = sin(theta);
    _cosTheta    = cos(theta);
    _d0          = (dPerp.Cross(*pos)).Z();
    _phi0        = dir->Phi();
    _sinPhi0     = sin(_phi0);
    _cosPhi0     = cos(_phi0);

    double s      = dPerp.Dot(*pos);
    double sprime = s/_sinTheta;
    _z0           = pos->Z()-sprime*dir->Z();

#define  THESE_ARE_BOTH_BOGUS
#ifndef  THESE_ARE_BOTH_BOGUS
#ifdef ORIGINAL
    double x=-_d0*_sinPhi0;
    //    double y= _d0*_cosPhi0;
    double delta_x = x-pos->X();
    //    double delta_y = y-pos->Y();
    double alpha_x = dir->X();
    //    double alpha_y = dir->Y();
    double alpha_z = dir->Z();
    _z0 = pos->Z() + (delta_x/alpha_x)*alpha_z;
#else
    double x       =-_d0*_sinPhi0;
    double y       = _d0*_cosPhi0;
    double delta_x = x-pos->X();
    double delta_y = y-pos->Y();
    double trans = sqrt(delta_x*delta_x + delta_y*delta_y);
    _z0 = Point->Z() + trans*_cosTheta/_sinTheta;
    
#endif
#endif
  }
}


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


//_____________________________________________________________________________
const TLine3D & TLine3D::operator=(const TLine3D &right){
  if (this!=&right) {
    _sinTheta=right._sinTheta;
    _cosTheta=right._cosTheta;
    _z0=right._z0;
    _d0=right._d0;
    _phi0=right._phi0;
    _sinPhi0=right._sinPhi0;
    _cosPhi0=right._cosPhi0;
  } 
  return *this;
}

//_____________________________________________________________________________
void TLine3D::GetPosition(TVector3& pos, double s) const {
  pos.SetXYZ(-_d0*_sinPhi0+s*_cosPhi0*_sinTheta,
	     _d0*_cosPhi0+s*_sinPhi0*_sinTheta,
	     _z0+s*_cosTheta);
}

//_____________________________________________________________________________
void TLine3D::GetDirection(TVector3& dir, double s) const{
  dir.SetXYZ(_cosPhi0*_sinTheta,_sinPhi0*_sinTheta,_cosTheta);
}

//_____________________________________________________________________________
void TLine3D::GetLocation(TTrajectory3D::Location & loc, double s) const {
    double cP0sT = _cosPhi0*_sinTheta, sP0sT = _sinPhi0*_sinTheta;
    loc.setLocation(s,
		    -_d0*_sinPhi0+s*cP0sT,
                    _d0*_cosPhi0+s*sP0sT,
                    _z0+s*_cosTheta,
                    cP0sT,sP0sT,_cosTheta);
}

//_____________________________________________________________________________
void TLine3D::GetPoint(TTrajectoryPoint* point, double s) const {
  // leave ptotal unchanged

    double cP0sT = _cosPhi0*_sinTheta, sP0sT = _sinPhi0*_sinTheta;

    point->GetPosition()->SetXYZ(-_d0*_sinPhi0+s*cP0sT,
				  _d0*_cosPhi0+s*sP0sT,
				  _z0+s*_cosTheta);
    point->GetDirection()->SetXYZ(cP0sT,sP0sT,_cosTheta);
    point->SetS(s);
}

//_____________________________________________________________________________
double TLine3D::GetPathLengthAtRho(double rho) const {
  if ( _sinTheta != 0.0 ) {
    if ( rho > fabs( _d0 ) ) {
      return sqrt( rho*rho - _d0*_d0 ) / _sinTheta ;
    }
    else {
      return double(0);
    }
  }
  else {
    return double(0);
  }
}

//_____________________________________________________________________________
double TLine3D::GetPathLengthAtZ(double Z) const {
  Double_t s;
  if ( _cosTheta != 0.0 ) s = (Z-_z0)/_cosTheta ;
  else                    s = 0;
  return s;
}

//_____________________________________________________________________________
//  TTrajectory3D::Location* 
//  TLine3D::newIntersectionWith(const HepPlane3D &plane) const {
//    if (plane.normal().dot(getDirection(0))!=0.0){
//      double s=-(plane.distance(getPosition(0)))/(plane.normal().dot(getDirection(0)));
//      Location * tmp = new Location();
//      if (s>0)  {
//        getLocation(*tmp,s);
//        return tmp;
//      }
//      else {
//        delete tmp;
//      }      
//    }
//    return NULL;
//  }

//_____________________________________________________________________________
TVector3 TLine3D::getSecondDerivative(double ) const{
  return TVector3(0,0,0);
}

//_____________________________________________________________________________
double TLine3D::getPathLengthTo(const TVector3 &point) const {
  TVector3 pos;
  TVector3 dir;

  GetPosition(pos);
  GetDirection(dir);
  return (point-pos).Dot(dir);
}

//_____________________________________________________________________________
double TLine3D::getDzeroTo(const TVector3 &point) const {  

  //  double opposite = getDzeroTo(point);
  double adjacent = getPathLengthTo(point);

  TVector3 hypvec;
  GetPosition(hypvec);

  hypvec.SetXYZ(point.X()-hypvec.X(),point.Y()-hypvec.Y(),point.Z()-hypvec.Z());

  double hypo = hypvec.Mag();
  //  return sqrt(hypo*hypo-opposite*opposite);
  // Think this quantity is signed right. See TTrajectory3D::getDzeroTo(const TTrajectory3D &traj)
  //  return sqrt(hypo*hypo-adjacent*adjacent)*(getDirection(0).cross(hypvec)>0.)?-1.0:1.0;
  return sqrt(hypo*hypo-adjacent*adjacent);//*(getDirection(0).cross(hypvec)>0.)?-1.0:1.0;
}

// double TLine3D::getPathLengthTo(const THelix3D &helix) {
//   // For speed, do a specific function for line/helix, and worry about the
//   // rest later.
//   // 24th Nov 1997. This is a *really* simple algorithm that assumes:
//   // a) the poca is along the positive helix-s direction;
//   // b) the poca is the first local minimum along the helix;
//   // Both these *should* be true for a sensible helix and a silicon strip
//   // line (r-phi or r-z).
//   // If THelix3D::getArcLengthAtRhoEquals were implemented, we'd have a far closer seed.

//   int ntries = 0;
//   const int MAXtries = 200;

//   // starting positions
//   TVector3 pointCurr;
//   TVector3 pointB = helix.getPosition(0);

//   double delta = 0.0;
//   double old_delta = 0.0;
//   TVector3 old_pointB; // may not need this

//   do { 

//     old_delta=delta;

//     pointCurr=getPosition(getPathLengthTo(pointB));
//     old_pointB=pointB;
//     pointB=helix.getPosition(helix.getPathLengthTo(pointCurr));
    
//     delta = (pointB - pointCurr).mag();

//     if (ntries && (delta > old_delta)) { 
//       // divide the distance between pointB and
//       // old_pointB into bits and work backwards
//       double start = getPathLengthTo(pointB);
//       double end = getPathLengthTo(old_pointB);
//       double step = EFINE*((end>start)?1.0:-1.0);
//       old_delta=delta;
//       for ( double posn = start+step; (end-posn)>step ; posn+=step ) {
//         delta = (pointB - pointCurr).mag();
//         if (delta < EEFINE || delta > old_delta) break;
//         old_delta=delta;
//       }

//       pointB = old_pointB;
//     }

//   } while (ntries++ < MAXtries && delta >= EEFINE);
  
//   if (ntries == MAXtries) {
//     cout << "Problem with TLine3D.cc. MAXtries exceeded. Check or mail greenc@fnal.gov" << endl;
//   }

//   return getPathLengthTo(pointCurr);

// }

//_____________________________________________________________________________
double TLine3D::getPathLengthTo(const TLine3D &line) {

  TVector3  x1;
  TVector3  x2;
  TVector3  d1;
  TVector3  d2;
  TVector3  dx;

  double d1Dotd2, path;

  GetPosition(x1,0);
  line.GetPosition(x2,0);
  GetDirection(d1,0);
  line.GetDirection(d2,0);

  dx      = x2-x1;
  d1Dotd2 = d1.Dot(d2);

  path = (d1Dotd2*dx.Dot(d2)-dx.Dot(d1))/(d1Dotd2*d1Dotd2-1);
  return path;
}

//_____________________________________________________________________________
double TLine3D::getDzeroTo(const TLine3D &line) {

  // *think* this is signed right. 
  // See TTrajectory3D::getDzeroTo(const TTrajectory3D &traj)
  
  TVector3  x1;
  TVector3  x2;
  TVector3  d1;
  TVector3  d2;
  double    d0;

  GetPosition(x1,0);
  line.GetPosition(x2,0);
  GetDirection(d1,0);
  line.GetDirection(d2,0);

  d0 = d1.Cross(d2).Unit().Dot(x2-x1);
  return d0;
}