#include "Helix.hh"
//#include "Trajectory/Helix.hh"
//#include "Trajectory/Line.hh"
//#include "CLHEP/Geometry/Point3D.h"
//#include "CLHEP/Geometry/Vector3D.h"
//#include "ErrorLogger_i/gERRLOG.hh"

//#ifdef DEFECT_OLD_STDC_HEADERS
//extern "C" {
//#include <math.h>
//}
//#else
//#include <cmath>
//#endif




Helix::~Helix()
{

}


double Helix::getInverseRadius() const
{
  return _curvature*2.0;
}

double Helix::getRadius() const
{
  return fabs(1.0/getInverseRadius());
}


double Helix::getCurvature() const
{
  return _curvature;
}

double Helix::getHelicity() const
{
  return _curvature>0 ? 1.0 : -1.0 ;
}

double Helix::getCotTheta() const
{
  return _cotTheta;
}

double Helix::getPhi0() const
{
  return _phi0;
}

double Helix::getD0() const
{
  return _d0;
}

double Helix::getSignLz() const
{
  return _d0>0 ? -1.0 : 1.0 ;
}

double Helix::getZ0() const
{
  return _z0;
}



double Helix::getPhiAtR(double rho) const {
  double c = getCurvature();
  double d = getD0();
  double a = c/(1.0+2.0*c*d);
  double b = d - a*d*d;
  double arcsin = a*rho+b/rho;
//  double arcsin = (c*rho+(1.0+c*d)*(d/rho))/(1+2.0*c*d);
  if (arcsin>1.0 || arcsin<-1.0) {
//    errlog.setSubroutine("Helix::getPhiAtR(rho)");
//    errlog(ELwarning, "[rho out of bounds]") << endmsg;
    return arcsin > 0. ? M_PI : -M_PI;
  }
  double phi = getPhi0() + asin(arcsin);
  return phi;
}

double Helix::getZAtR(double rho) const {
  return _z0 + getCotTheta()*getL2DAtR(rho);
}

double Helix::getL2DAtR(double rho) const {
  double L2D;

  double c = getCurvature();
  double d = getD0();
//  errlog.setSubroutine("Helix::getZAtR(rho)");

  if (c!=0.0) {
    double rad = (rho*rho-d*d)/(1.0+2.0*c*d);
    double rprime;
    if (rad<0.0) {
//      errlog(ELwarning, "[rho out of bounds]") << endmsg;
      rprime = 0.0;
    }
    else {
      rprime = sqrt( rad );
    }
    if (c*rprime > 1.0 || c*rprime < -1.0) {
//      errlog(ELwarning, "[workaround illegal asin argument]") << endmsg;
      L2D = c*rprime > 0. ? M_PI/c : -M_PI/c;
    }
    else
      L2D = asin(c*rprime)/c;
  } else {
    L2D = rho;
  }
  return L2D;
}

double Helix::getCosAlphaAtR(double rho) const {
  double c = getCurvature();
  double d = getD0();
  double a = c/(1.0+2.0*c*d);
  //AML: not necessary? not used: double b = d - a*d*d;

  double phi = getPhiAtR(rho);

  double x = rho*cos(phi);
  double y = rho*sin(phi);

  double dir_x0 = (1.0+2.0*c*d) * (1.0-2.0*a*y);
  double dir_y0 = (1.0+2.0*c*d) * 2.0*a*x;

  double phi0 = getPhi0();
  double dir_x = dir_x0*cos(phi0) - dir_y0*sin(phi0);
  double dir_y = dir_x0*sin(phi0) + dir_y0*cos(phi0);

  double cosalpha = (x*dir_x + y*dir_y)/rho;
  return cosalpha;
}

///////////////////////////////////////////////////////////////////////////////////////
// KCDF: analytical computation of helix/plane intersection.
//
// What we really compute is the intersection of a line and a circle (projected helix)
// in the x-y plane.
//
//     >>>>>>>>>>  W A R N I N G    W A R N I N G    W A R N I N G  <<<<<<<<<<
//     >                                                                     <
//     > We assume the plane to be parallel or perpendicular                 <
//     > to the z axis (i.e. the B-field),                                   <
//     > since otherwise there is no analytical solution. (One would end up  <
//     > with an equation of type cos(alpha) == alpha.)                      <
//     > Although we know this assumption doesn´t hold exactly, we think it  <
//     > is a reasonable first approximation.                                <
//     > In cases when our assumption has to be dropped, one can use the     <
//     > intersection point computed here as a *good* starting point of a    <
//     > numerical method, or one uses another analytical method to compute  <
//     > the intersection of the tangent line at the point and the plane.    <
//     > We plan to use one of these approaches in the near future, but      <
//     > this is NOT YET IMPLEMENTED!                                        <
//     > For the time being, we invoke the old numerical                     < 
//     > Trajectory::newIntersectionWith in such circumstances.              <
//     >                                                                     <
//     >>>>>>>>>>  W A R N I N G    W A R N I N G    W A R N I N G  <<<<<<<<<<
//
// Kurt Rinnert,  08/31/1998
////////////////////////////////////////////////////////////////////////////////////////


Helix::Helix(double Pperp, double Pphi, double Pz, 
	     double x, double y, double z,
	     double q, 
	     double BFieldTesla) {


  double CotTheta,W,Z0,D0;
  double Phi0;
  if (BFieldTesla != 0.0 && q != 0.0) {
    double CurvatureConstant=0.0029979;
//    double Helicity       = -1.0 * fabs(BFieldTesla) * fabs(q) / (BFieldTesla*q);
    double Helicity       = fabs(BFieldTesla) * fabs(q) / (BFieldTesla*q);
    double Radius         = fabs(Pperp/(CurvatureConstant*BFieldTesla*q));
    
    if(Radius==0.0) W = HUGE_VAL;   else     W        = Helicity/Radius;
    double phi1     = Pphi;
//    double x        = PositionCm.x(),        y        = PositionCm.y(),    z = PositionCm.z(); 
    double sinPhi1  = sin(phi1),             cosPhi1  = cos(phi1);
    double gamma    = atan((x*cosPhi1 + y*sinPhi1)/(x*sinPhi1-y*cosPhi1 -1/W));
    Phi0            = phi1+gamma;
    D0              = ((1/W + y*cosPhi1 - x*sinPhi1) /cos(gamma) - 1/W);
    CotTheta        = Pz/Pperp;
    Z0              = z + gamma*CotTheta/W;
  }
  else {
/*    Line  auxLine(PositionCm,MomentumGev.unit());  
    Z0       = auxLine.getZ0();
    Phi0     = auxLine.getPhi0();
    CotTheta = auxLine.getCotTheta();
    D0       = auxLine.getD0();
    W        = 0.0;
    //    Hep3Vector direction          = MomentumGev.unit(); 
    //    Hep3Vector projectedDirection = Hep3Vector(direction.x(),direction.y(),0.0).unit();
    //    double s                      = projectedDirection.dot(PositionCm);
    //    double sprime                 = s/sin(direction.theta());
    //    Z0                            = (PositionCm - sprime*direction).z();
    //    Phi0                          = MomentumGev.phi();
    //    CotTheta                      = MomentumGev.z()/MomentumGev.perp();
    //    W                             = 0.0;
    //    D0                           = (PositionCm.y()*cos(Phi0) - PositionCm.x()*sin(Phi0));
*/
  } 
  
  // The line commented out below doesn't appear to work (Msm and Dsw Jul 2000)
  //  Helix(CotTheta,W/2,Z0,D0,Phi0);
  // So replace it with the contents of the constructor
   _cotTheta=CotTheta;
   _curvature=W/2;
   _z0=Z0;
   _d0=D0;
   _phi0=Phi0;

}