// PropXYCyl_t.cc

// Test PropXYCyl.

#include "PropXYCyl.h"
#include "trfbase/PropStat.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfxyp/SurfXYPlane.h"
#include "trfcyl/SurfCylinder.h"
#include "trfbase/TrackVector.h"
#include "trfbase/VTrack.h"
#include "trfbase/ETrack.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"
#include "spacegeom/SpacePoint.h"
#include "spacegeom/SpacePath.h"

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::ostringstream;
using std::istringstream;
using namespace trf;
#endif

namespace {
void  check_zero_propagation(const VTrack&trv0,const VTrack& trv,const PropStat& pstat);
void check_derivatives(const Propagator& prop,const VTrack& trv0,const Surface& srf) ;
void check_derivative(const Propagator& prop,const VTrack& trv0,const Surface& srf,int i,int j) ;
}

// Assign track parameter indices.
enum {
  IV     = SurfXYPlane::IV,
  IZC    = SurfXYPlane::IZ,
  IDVDU  = SurfXYPlane::IDVDU,
  IDZDU  = SurfXYPlane::IDZDU,
  IQP_XY = SurfXYPlane::IQP,

  IPHI   = SurfCylinder::IPHI,
  IZ     = SurfCylinder::IZ,
  IALF   = SurfCylinder::IALF,
  ITLM   = SurfCylinder::ITLM,
  IQPT   = SurfCylinder::IQPT
};

// Very well tested XY-Cyl propagator. Each new one can be tested against it

//**********************************************************************
// helpers
//**********************************************************************

// Private class STCalcXY_CHECK.
//
// An STCalcXY_CHECK_ object calculates sT (the signed transverse path length)
// and its derivatives w.r.t. alf1 and crv1.  It is constructed from
// the starting (r1, phi1, alf1, crv1) and final track parameters
// (r2, phi2, alf2) assuming these are consistent.  Methods are
// provided to retrieve sT and the two derivatives.

class STCalcXY_CHECK {
private:
  bool _big_crv;
  double _st;
  double _dst_dphi21;
  double _dst_dcrv1;
  double _dst_dr1;
  double _cnst1,_cnst2;
public:
  // constructor
  STCalcXY_CHECK();
  STCalcXY_CHECK(double r1, double phi1, double alf1, double crv,
         double r2, double phi2, double alf2);
  double st() const { return _st; };
  double d_st_dalp1(double d_phi2_dalf1, double d_alf2_dalf1 ) const;
  double d_st_dcrv1(double d_phi2_dcrv1, double d_alf2_dcrv1 ) const;
  double d_st_dr1(  double d_phi2_dr1,   double d_alf2_dr1   ) const; 
  double _crv1;
};

STCalcXY_CHECK::STCalcXY_CHECK() { }

STCalcXY_CHECK::STCalcXY_CHECK(double r1, double phi1, 
                               double alf1, double crv1,
                               double r2, double phi2, double alf2) {

  _crv1 = crv1;
  assert( r1 > 0.0 );
  assert( r2 > 0.0 );
  double rmax = r1+r2;

  // Calculate the change in xy direction
  double phi_dir_diff = fmod2(phi2+alf2-phi1-alf1,TWOPI);
  assert( fabs(phi_dir_diff) <= PI );

  // Evaluate whether |C| is" big"
  _big_crv = rmax*fabs(crv1) > 0.001;

  // If the curvature is big we can use 
  // sT = (phi_dir2 - phi_dir1)/crv1
  if ( _big_crv ) {
    assert( crv1 != 0.0 );
    _st = phi_dir_diff/crv1;
  }

  // Otherwise, we calculate the straight-line distance
  // between the points and use an approximate correction
  // for the (small) curvature.
  else {

    // evaluate the distance
    double d = sqrt( r1*r1 + r2*r2 - 2.0*r1*r2*cos(phi2-phi1) );
    double arg = 0.5*d*crv1;
    double arg2 = arg*arg;
    float st_minus_d = d*arg2*( 1.0/6.0 + 3.0/40.0*arg2 );
    _st = d + st_minus_d;

    // evaluate the sign
    // We define a metric xsign = abs( (dphid-d*C)/(d*C) ).
    // Because sT*C = dphid and d = abs(sT):
    // xsign = 0 for sT > 0
    // xsign = 2 for sT < 0
    // Numerical roundoff will smear these predictions.
    double xsign = fabs( (phi_dir_diff - _st*crv1) / (_st*crv1) );
    double sign = 0.0;
    if ( crv1 ) {
      if ( xsign < 0.5 ) sign = 1.0;
      if ( xsign > 1.5  &&  xsign < 3.0 ) sign = -1.0;
    }
    // If the above is indeterminate, assume zero curvature.
    // In this case abs(alpha) decreases monotonically
    // with sT.  Track passing through origin has alpha = 0 on one
    // side and alpha = +/-pi on the other.  If both points are on
    // the same side, we use dr/ds > 0 for |alpha|<pi/2.
    if ( ! sign ) {
      sign = 1.0;
      if ( fabs(alf2) > fabs(alf1) ) sign = -1.0;
      if ( fabs(alf2) == fabs(alf1) ) {
        if ( fabs(alf2) < PI2 ) {
          if ( r2 < r1 ) sign = -1.0;
        } else {
          if ( r2 > r1 ) sign = -1.0;
        }
      }
    }

    // Correct _st using the above sign.
    assert( fabs(sign) == 1.0 );
    _st = sign*_st;

    // save derivatives
    _dst_dcrv1 = sign*d*d*arg*( 1.0/6.0 + 3.0/20.0*arg2);
    double root = (1.0 + 0.5*arg*arg + 3.0/8.0*arg*arg*arg*arg );
    _dst_dphi21 = sign*(r1*r2*sin(phi2-phi1))*root/d;
    _dst_dr1= (1.+arg2/2.*(1+3./4.*arg2))/d*sign;
    _cnst1=r1-r2*cos(phi2-phi1);
    _cnst2=r1*r2*sin(phi2-phi1);
  }

}

double 
STCalcXY_CHECK::d_st_dalp1(double dphi2_dalf1, double dalf2_dalf1) const {
  if ( _big_crv ) return ( dphi2_dalf1 + dalf2_dalf1 - 1.0 ) / _crv1;
  else return _dst_dphi21 * dphi2_dalf1;
}

double 
STCalcXY_CHECK::d_st_dcrv1(double dphi2_dcrv1, double dalf2_dcrv1) const {
  if ( _big_crv ) return ( dphi2_dcrv1 + dalf2_dcrv1 - _st ) / _crv1;
  else return _dst_dcrv1 + _dst_dphi21*dphi2_dcrv1;
}

double STCalcXY_CHECK::d_st_dr1(double dphi2_dr1, double dalf2_dr1) const {
  if ( _big_crv ) return ( dphi2_dr1 + dalf2_dr1 ) / _crv1;
  else return _dst_dr1*(_cnst1+_cnst2*dphi2_dr1);
}

//**********************************************************************

// The track parameters for a cylinder are:
// phi z alpha tan(lambda) curvature
// (NOT [. . . lambda .] as in TRF and earlier version of TRF++.)
//
// If pderiv is nonzero, return the derivative matrix there.
// On Cylinder:
// r (cm) is fixed 
// 0 - phi 
// 1 - z (cm)
// 2 - alp 
// 3 - tlam 
// 4 - q/pt   pt is transverse momentum of a track, q is its charge
// On XYPlane:
// u (cm) is fixed 
// 0 - v (cm)
// 1 - z (cm)
// 2 - dv/du 
// 3 - dz/du 
// 4 - q/p   p is momentum of a track, q is its charge

PropStat 
vec_propxycyl( double B, VTrack& trv, const Surface& srf,
                    const Propagator::PropDir dir,
                    TrackDerivative* pderiv =0 ) {

  // construct return status
  PropStat pstat;

  // fetch the originating surface and vector
  const Surface& srf1 = *trv.get_surface();
  TrackVector vec1 = trv.get_vector();

  // Check origin is a XYPlane.
  assert( srf1.get_pure_type() == SurfXYPlane::get_static_type() );
  if ( srf1.get_pure_type( ) != SurfXYPlane::get_static_type() )
    return pstat;
  const SurfXYPlane& sxyp1 = (const SurfXYPlane&) srf1;

  // Check destination is a cylinder.
  assert( srf.get_pure_type() == SurfCylinder::get_static_type() );
  if ( srf.get_pure_type( ) != SurfCylinder::get_static_type() )
    return pstat;
  const SurfCylinder& scy2 = (const SurfCylinder&) srf;


 // Fetch the dist and phi of the plane and the starting track vector.
 int iphi  = SurfXYPlane::NORMPHI;
 int idist = SurfXYPlane::DISTNORM;
 double phi = sxyp1.get_parameter(iphi);
 double    r = sxyp1.get_parameter(idist);

 TrackVector vec = trv.get_vector();
 double v = vec(IV);                  // v
 double z = vec(IZC);                  // z
 double b = vec(IDVDU);               // dv/du
 double a = vec(IDZDU);               // dz/du
 double e = vec(IQP_XY);              // q/p

 // Fetch the radii and the starting track vector.
  int irad = SurfCylinder::RADIUS;
  double r2 = scy2.get_parameter(irad);
  
 int sign_du = 0;
 if(trv.is_forward())  sign_du =  1;
 if(trv.is_backward()) sign_du = -1;
 if(sign_du == 0){
  cerr << "PropXYCyl::_vec_propagate: Unknown direction of a track "<< endl;
  abort();
 }

 // Calculate cylindrical coordinates
  
  double cnst=sign_du>0?0.:PI;
  double atn=atan(v/r);
  assert(r!=0.);

  double phi1= fmod2(phi+atn,TWOPI);
  double r1=sqrt(r*r+v*v);
  double z1=z;
  double alp1= fmod2(atan(b)-atn+cnst,TWOPI);
  double tlm1= a*sign_du/sqrt(1+b*b);
  double qpt1=e*sqrt((1+a*a+b*b)/(1+b*b));


  // Check alpha range.
  alp1 = fmod2( alp1, TWOPI );
  assert( fabs(alp1) <= PI );
  if ( trv.is_forward() ) assert( fabs(alp1) <= PI2 );
  else assert( fabs(alp1) > PI2 );

  // Calculate the cosine of lambda.
  //double clam1 = 1.0/sqrt(1+tlm1*tlm1);

  // Calculate curvature: C = _bfac*(q/p)/cos(lambda)
  // and its derivatives
  // assert( clam1 != 0.0 );
  // double dcrv1_dqp1 = B/clam1;
  // double crv1 = dcrv1_dqp1*qp1;
  // double dcrv1_dtlm1 = crv1*clam1*clam1*tlm1;

  // Calculate the curvature = _bfac*(q/pt)
  double dcrv1_dqpt1 = B;
  double crv1 = dcrv1_dqpt1*qpt1;
  //double dcrv1_dtlm1 = 0.0;

  // Evaluate the new track vector.
  // See dla log I-044

  // lambda and curvature do not change
  double tlm2 = tlm1;
  double crv2 = crv1;
  double qpt2 = qpt1;

  // We can evaluate sin(alp2), leaving two possibilities for alp2
  // 1st solution: alp21, phi21, phid21, tht21
  // 2nd solution: alp22, phi22, phid22, tht22
  // evaluate phi2 to choose
  double salp1 = sin( alp1 );
  double calp1 = cos( alp1 );
  double salp2 = r1/r2*salp1 + 0.5*crv1/r2*(r2*r2-r1*r1);
  // if salp2 > 1, track does not cross cylinder
  if ( fabs(salp2) > 1.0 ) return pstat;
  double alp21 = asin( salp2 );
  double alp22 = alp21>0 ? PI-alp21 : -PI-alp21;
  double calp21 = cos( alp21 );
  double calp22 = cos( alp22 );
  double phi20 = phi1 + atan2( salp1-r1*crv1, calp1 );
  double phi21 = phi20 - atan2( salp2-r2*crv2, calp21 );   // phi position
  double phi22 = phi20 - atan2( salp2-r2*crv2, calp22 );
  // Construct an sT object for each solution.
  STCalcXY_CHECK sto1(r1,phi1,alp1,crv1,r2,phi21,alp21);
  STCalcXY_CHECK sto2(r1,phi1,alp1,crv1,r2,phi22,alp22);
  // Check the two solutions are nonzero and have opposite sign 
  // or at least one is nonzero.

  // Choose the correct solution
  bool use_first_solution;
  switch (dir) {
  case Propagator::NEAREST:
    use_first_solution = abs(sto2.st()) > abs(sto1.st());
    break;
  case Propagator::FORWARD:
    use_first_solution = sto1.st() > 0.0;
    break;
  case Propagator::BACKWARD:
    use_first_solution = sto1.st() < 0.0;
    break;
  default:
    cerr << "PropCyl::_vec_propagate: Unknown direction." << endl;
    abort();
  }

  // Assign phi2, alp2 and sto2 for the chosen solution.
  double phi2, alp2;
  STCalcXY_CHECK sto;
  double calp2;
  if ( use_first_solution ) {
    sto = sto1;
    phi2 = phi21;
    alp2 = alp21;
    calp2 = calp21;
   } else {
    sto = sto2;
    phi2 = phi22;
    alp2 = alp22;
    calp2 = calp22;
   }

  // fetch sT.
  double st = sto.st();

  // use sT to evaluate z2
  double z2 = z1 + tlm1*st;

  // Check alpha range.
  assert( fabs(alp2) <= PI );

  // put new values in vec
  vec(IPHI) = phi2;
  vec(IZ)   = z2;
  vec(IALF) = alp2;
  vec(ITLM) = tlm2;
  vec(IQPT) = qpt2;

  // Update trv
  trv.set_surface(SurfacePtr(srf.new_pure_surface()));
  trv.set_vector(vec);
  if ( fabs(alp2) <= PI2 ) trv.set_forward();
  else trv.set_backward();

  // Set the return status.
  st > 0 ? pstat.set_path_distance(1) : pstat.set_path_distance(-1);

  // exit now if user did not ask for error matrix.
  if ( ! pderiv ) return pstat;

  // Calculate derivatives.
  // dphi1

  double dphi1_dv= r/(r*r+v*v);

  // dz1

  double dz1_dz=1.;

  // dalf1

  double dalp1_db= 1./(1.+b*b);
  double dalp1_dv= -r/(r*r+v*v);

  // dr1

  double dr1_dv= v/sqrt(v*v+r*r);

  // dtlm1

  double dtlm1_da= sign_du/sqrt(1+b*b);
  double dtlm1_db= -a*sign_du*b/(1+b*b)/sqrt(1+b*b);

  // dcrv1

  double dcrv1_de= sqrt((1+a*a+b*b)/(1+b*b))*B;
  double dcrv1_da= a*e/sqrt((1+b*b)*(1+a*a+b*b))*B;
  double dcrv1_db= -e*b*a*a/sqrt(1+a*a+b*b)/sqrt(1+b*b)/(1+b*b)*B;

  // alpha_2
  double da2da1 = r1*calp1/r2/calp2;
  double da2dc1 = (r2*r2-r1*r1)*0.5/r2/calp2;
  double da2dr1 = (salp1-crv2*r1)/r2/calp2;

  // phi2
  double rcsal1 = r1*crv1*salp1;
  double rcsal2 = r2*crv2*salp2;
  double den1 = 1.0 + r1*r1*crv1*crv1 - 2.0*rcsal1;
  double den2 = 1.0 + r2*r2*crv2*crv2 - 2.0*rcsal2;
  double dp2dp1 = 1.0;
  double dp2da1 = (1.0-rcsal1)/den1 - (1.0-rcsal2)/den2*da2da1;
  double dp2dc1 = -r1*calp1/den1 + r2*calp2/den2 
                  - (1.0-rcsal2)/den2*da2dc1;
  double dp2dr1= -crv1*calp1/den1-(1.0-rcsal2)*da2dr1/den2;

  // z2
  double dz2dz1 = 1.0;
  double dz2dl1 = st;
  double dz2da1 = tlm1*sto.d_st_dalp1(dp2da1, da2da1);
  double dz2dc1 = tlm1*sto.d_st_dcrv1(dp2dc1, da2dc1);
  double dz2dr1 = tlm1*sto.d_st_dr1(  dp2dr1, da2dr1);


  // final derivatives

  // phi2
  double dphi2_dv=dp2dp1*dphi1_dv+dp2da1*dalp1_dv+dp2dr1*dr1_dv;
  double dphi2_db=dp2da1*dalp1_db+dp2dc1*dcrv1_db;
  double dphi2_da=dp2dc1*dcrv1_da;
  double dphi2_de=dp2dc1*dcrv1_de;

  // alp2
  double dalp2_dv= da2da1*dalp1_dv+da2dr1*dr1_dv;
  double dalp2_db= da2da1*dalp1_db+da2dc1*dcrv1_db;
  double dalp2_da= da2dc1*dcrv1_da;
  double dalp2_de= da2dc1*dcrv1_de;

  // crv2
  double dcrv2_da=dcrv1_da;
  double dcrv2_db=dcrv1_db;
  double dcrv2_de=dcrv1_de;

  // tlm2
  double dtlm2_da= dtlm1_da;
  double dtlm2_db= dtlm1_db;

  // z2
  double dz2_dz= dz2dz1*dz1_dz;
  double dz2_dv= dz2dr1*dr1_dv+dz2da1*dalp1_dv;
  double dz2_db= dz2da1*dalp1_db+dz2dl1*dtlm1_db+dz2dc1*dcrv1_db;
  double dz2_da= dz2dl1*dtlm1_da+dz2dc1*dcrv1_da;
  double dz2_de= dz2dc1*dcrv1_de;


  // Build derivative matrix.
  TrackDerivative& deriv = *pderiv;
  deriv.fill( 0.0 );
  deriv(IPHI,IV) =dphi2_dv;
  deriv(IPHI,IDVDU)= dphi2_db;
  deriv(IPHI,IDZDU)= dphi2_da;
  deriv(IPHI,IQP_XY)= dphi2_de;
  deriv(IZ,IV)   = dz2_dv;
  deriv(IZ,IZC)  = dz2_dz;
  deriv(IZ,IDVDU) =dz2_db;
  deriv(IZ,IDZDU) =dz2_da;
  deriv(IZ,IQP_XY)=dz2_de; 
  deriv(IALF,IV) = dalp2_dv;
  deriv(IALF,IDVDU) = dalp2_db;
  deriv(IALF,IDZDU) = dalp2_da;
  deriv(IALF,IQP_XY) = dalp2_de;
  deriv(ITLM,IDVDU) = dtlm2_db;
  deriv(ITLM,IDZDU) = dtlm2_da;
  deriv(IQPT,IDVDU) = dcrv2_db/B;
  deriv(IQPT,IDZDU) = dcrv2_da/B;
  deriv(IQPT,IQP_XY) = dcrv2_de/B;
  return pstat;

}

//************************************************************************

// compare two tracks  without errors 

int compare(VTrack &trv1,VTrack &trv2,double *diff)
{
 const Surface &srf = *trv2.get_surface();

 assert(*trv1.get_surface()==srf);

 *diff = srf.vec_diff(trv2.get_vector(),trv1.get_vector()).amax();

 return 0;
}

//**********************************************************************
// compare two tracks  with errors 

int compare(ETrack &trv1,ETrack &trv2,double *diff,double *ediff)
{
 const Surface &srf = *trv2.get_surface();

 assert(*trv1.get_surface()==srf);

 *diff = srf.vec_diff(trv2.get_vector(),trv1.get_vector()).amax();

 TrackError dfc = trv2.get_error() - trv1.get_error();
 *ediff = dfc.amax();

 cout<<"\nerr diff: \n"<<dfc<<endl;
 return 0;
}

//************************************************************************
int main( ) {

  string ok_prefix = "PropXYCyl (I): ";
  string error_prefix = "PropXYCyl test (E): ";

  cout << ok_prefix
       << "-------- Testing component PropXYCyl. --------" << endl;

  // Make sure assert is enabled.
  bool assert_flag = false;
  assert ( ( assert_flag = true, assert_flag ) );
  if ( ! assert_flag ) {
    cerr << "Assert is disabled" << endl;
    return 1;
  }

  //********************************************************************
  cout << trf_format ;

  cout << ok_prefix << "Test constructor." << endl;
  double BFIELD = 2.0*BFAC;
  PropXYCyl prop(ObjDoublePtr(new ObjDouble(-BFIELD/BFAC)));
  cout << prop << endl;

 //********************************************************************

  // Here we propagate some tracks both forward and backward and then
  // the same track forward and backward but using method
  // that we checked very thoroughly before.

 cout << ok_prefix << "Check against correct propagation." << endl;

 PropXYCyl propxycyl(ObjDoublePtr(new ObjDouble(-BFIELD/BFAC)));

 double u1[]     ={10.,      10.,     10.,     10.,     10.,     10.     };
 double phi1[]   ={5*PI/6.,  5*PI/6.,  PI/6.,  PI/6.,   5/3*PI,  7/4*PI  }; 
 int sign_du[]   ={ -1,        1,      -1,       1,       1,      -1     };
 double v[]      ={ -2.,       2.,      5.,      5.,     -2.,     -2.    };
 double z[]      ={103.,     103.,    103.,    103.,    103.,    103.    };
 double dvdu[]   ={ 1.5,     -2.3,     0.,      0.,     -1.5,      0.    };
 double dzdu[]   ={-2.3,      1.5,    -2.3,     1.5,      0.,     -1.5   };
 double qp[]     ={ 0.01,    -0.01,    0.01,   -0.01,   -0.01,     0.01  };

 double rcyl2[]  ={ 20.,      20.,     20.,     20.,     20.,     20.    };
 double rcyl2b[] ={ 20.,      20.,     20.,     20.,     20.,     20.    };

 double maxdiff = 1.e-10;
 double diff;
 int ntrk = 6;
 int i;

 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;

  PropStat pstat;

  SurfXYPlane  sxyp1(u1[i],phi1[i]);
  SurfCylinder scy2(rcyl2[i]);
  SurfCylinder scy2b(rcyl2b[i]);

  TrackVector vec1; 

  vec1(SurfXYPlane::IV)    = v[i];     // v
  vec1(SurfXYPlane::IZ)    = z[i];     // z
  vec1(SurfXYPlane::IDVDU) = dvdu[i];  // dv/du
  vec1(SurfXYPlane::IDZDU) = dzdu[i];  // dz/du
  vec1(SurfXYPlane::IQP)   = qp[i];    //  q/p

  VTrack trv1(SurfacePtr(sxyp1.new_pure_surface()),vec1);
  (sign_du[i]==1)?trv1.set_forward():trv1.set_backward();

  cout << "\n starting: " << trv1 << endl;
 
  cout<<"\n----------------------------------------------------------"<<endl;

  VTrack trv2f = trv1;
  pstat = propxycyl.vec_dir_prop(trv2f,scy2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "\n  forward: " << trv2f << endl;

  VTrack trv2f_my = trv1;
  pstat = vec_propxycyl(BFIELD,trv2f_my,scy2,Propagator::FORWARD);
  assert( pstat.forward() ); 
  cout << "\n  forward my: " << trv2f_my << endl;
  compare(trv2f_my,trv2f,&diff);
  
  cout << "\n diff: " << diff << endl;
  assert( diff < maxdiff );

  cout<<"\n----------------------------------------------------------"<<endl;

  VTrack trv2b = trv1;
  pstat = propxycyl.vec_dir_prop(trv2b,scy2b,Propagator::BACKWARD);
  assert( pstat.backward() );  
  cout << "\n  backward: " << trv2b << endl;

  VTrack trv2b_my = trv1;
  pstat = vec_propxycyl(BFIELD,trv2b_my,scy2b,Propagator::BACKWARD);
  assert( pstat.backward() ); 
  cout << "\n  backward my: " << trv2b_my << endl;
  compare(trv2b_my,trv2b,&diff);
  
  cout << "\n diff: " << diff << endl;
  assert( diff < maxdiff );

  cout<<"\n----------------------------------------------------------"<<endl;

 }
 //********************************************************************

  // Repeat the above with errors.
 cout << ok_prefix << "Check against correct propagation with errors." << endl;

 double evv[] =   {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double evz[] =   {  0.01,  -0.01,   0.01,  -0.01,   0.01,  -0.01  };
 double ezz[] =   {  0.25,   0.25,   0.25,   0.25,   0.25,   0.25, };
 double evdv[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezdv[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvdv[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double evdz[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvdz[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezdz[] =  {  0.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04  };
 double edzdz[] = {  0.02,   0.02,   0.02,   0.02,   0.02,   0.02  };
 double evqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvqp[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edzqp[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double eqpqp[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };

 maxdiff = 1.e-8;
 double ediff;
 ntrk = 6;

 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;

  PropStat pstat;

  SurfXYPlane  sxyp1(u1[i],phi1[i]);
  SurfCylinder scy2(rcyl2[i]);
  SurfCylinder scy2b(rcyl2b[i]);

  TrackVector vec1; 

  vec1(SurfXYPlane::IV)    = v[i];     // v
  vec1(SurfXYPlane::IZ)    = z[i];     // z
  vec1(SurfXYPlane::IDVDU) = dvdu[i];  // dv/du
  vec1(SurfXYPlane::IDZDU) = dzdu[i];  // dz/du
  vec1(SurfXYPlane::IQP)   = qp[i];    //  q/p

  TrackError err1;
 
  err1(SurfXYPlane::IV,SurfXYPlane::IV)       = evv[i];
  err1(SurfXYPlane::IV,SurfXYPlane::IZ)       = evz[i];
  err1(SurfXYPlane::IZ,SurfXYPlane::IZ)       = ezz[i];
  err1(SurfXYPlane::IV,SurfXYPlane::IDVDU)    = evdv[i];
  err1(SurfXYPlane::IZ,SurfXYPlane::IDVDU)    = ezdv[i];
  err1(SurfXYPlane::IDVDU,SurfXYPlane::IDVDU) = edvdv[i];
  err1(SurfXYPlane::IV,SurfXYPlane::IDZDU)    = evdz[i];
  err1(SurfXYPlane::IZ,SurfXYPlane::IDZDU)    = ezdz[i];
  err1(SurfXYPlane::IDVDU,SurfXYPlane::IDZDU) = edvdz[i];
  err1(SurfXYPlane::IDZDU,SurfXYPlane::IDZDU) = edzdz[i];
  err1(SurfXYPlane::IV,SurfXYPlane::IQP)      = evqp[i];
  err1(SurfXYPlane::IZ,SurfXYPlane::IQP)      = ezqp[i];
  err1(SurfXYPlane::IDVDU,SurfXYPlane::IQP)   = edvqp[i];
  err1(SurfXYPlane::IDZDU,SurfXYPlane::IQP)   = edzqp[i];
  err1(SurfXYPlane::IQP,SurfXYPlane::IQP)     = eqpqp[i];

  ETrack trv1(SurfacePtr(sxyp1.new_pure_surface()),vec1,err1);
  (sign_du[i]==1)?trv1.set_forward():trv1.set_backward();

  cout << "\n starting: " << trv1 << endl;
 
  cout<<"\n----------------------------------------------------------"<<endl;

  ETrack trv2f = trv1;
  pstat = propxycyl.err_dir_prop(trv2f,scy2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "\n  forward: " << trv2f << endl;

  ETrack trv2f_my = trv1;
  TrackDerivative deriv; 
  pstat = vec_propxycyl(BFIELD,trv2f_my,scy2,Propagator::FORWARD,&deriv);
  assert( pstat.forward() );
  TrackError err = trv2f_my.get_error();
  trv2f_my.set_error( err.Xform(deriv) );  
  cout << "\n  forward my: " << trv2f_my << endl;
  compare(trv2f_my,trv2f,&diff,&ediff);
  
  cout << "\n diff: " << diff << ' ' << "ediff: "<< ediff << endl;
  assert( diff < maxdiff );
  assert( ediff < maxdiff );

  cout<<"\n----------------------------------------------------------"<<endl;

  ETrack trv2b = trv1;
  pstat = propxycyl.err_dir_prop(trv2b,scy2b,Propagator::BACKWARD);
  assert( pstat.backward() );  
  cout << "\n  backward: " << trv2b << endl;

  ETrack trv2b_my = trv1;
  pstat = vec_propxycyl(BFIELD,trv2b_my,scy2b,Propagator::BACKWARD,&deriv);
  assert( pstat.backward() );
  err = trv2b_my.get_error();
  trv2b_my.set_error( err.Xform(deriv) );  
  cout << "\n  backward my: " << trv2b_my << endl;
  compare(trv2b_my,trv2b,&diff,&ediff);
  
  cout << "\n diff: " << diff << ' ' << "ediff: "<< ediff << endl;
  assert( diff < maxdiff );
  assert( ediff < maxdiff );

  cout<<"\n----------------------------------------------------------"<<endl;

 }

//********************************************************************

 cout << ok_prefix << "Propagation to the same surface." << endl; 
 {
   SurfXYPlane xyp(10.,1.);
   Propagator& prop = propxycyl;
   TrackVector vec;
   vec(IV)=0.1;
   vec(IZ)=12.;
   vec(IDVDU)=1.5;
   vec(IDZDU)=0.8;
   vec(IQP_XY) = 0.000000006;
   SurfacePtr psrf(xyp.new_pure_surface());
   SurfacePtr psrf_to(new SurfCylinder(sqrt(100.01)));
   VTrack trv(psrf,vec);
   trv.set_forward();
   VTrack trv_der=trv;
   PropStat pstat = prop.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   check_derivatives(prop,trv_der,*psrf_to);
 }
 {
   SurfXYPlane xyp(10.,1.);
   Propagator& prop = propxycyl;
   TrackVector vec;
   vec(IV)=0.1;
   vec(IZ)=12.;
   vec(IDVDU)=1.5;
   vec(IDZDU)=0.8;
   vec(IQP_XY) = 0.006;
   SurfacePtr psrf(xyp.new_pure_surface());
   SurfacePtr psrf_to(new SurfCylinder(sqrt(100.01)));
   VTrack trv(psrf,vec);
   trv.set_forward();
   VTrack trv_der=trv;
   PropStat pstat = prop.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   check_derivatives(prop,trv_der,*psrf_to);
 }
 //********************************************************************

 cout << ok_prefix << "Test Zero Field Propagation." << endl;
 {
   PropXYCyl prop0(ObjDoublePtr(new ObjDouble(0.0)));
   cout << prop0 << endl;
   assert( prop0.get_bfield() == 0. );

   double u=10.,phi=0.1;
   SurfacePtr psrf(new SurfXYPlane(u,phi));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfXYPlane::IV) = 2.;
   vec(SurfXYPlane::IZ) = 10.;
   vec(SurfXYPlane::IDVDU) = 4.;
   vec(SurfXYPlane::IDZDU) = 2.;
   trv0.set_vector(vec);
   trv0.set_forward();
   SurfacePtr psrf_to(new SurfCylinder(13.0));

   VTrack trv = trv0;
   VTrack trv_der=trv;
   PropStat pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );

   assert( pstat.forward() );
   assert(trv.get_surface()->pure_equal(*psrf_to));

   check_zero_propagation(trv0,trv,pstat);
   check_derivatives(prop0,trv_der,*psrf_to);

   psrf_to = new SurfCylinder(trv0.space_point().rxy());
   trv = trv0;
   trv_der=trv;
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );

   assert( pstat.same() );
   assert(trv.get_surface()->pure_equal(*psrf_to));

   check_zero_propagation(trv0,trv,pstat);
   check_derivatives(prop0,trv_der,*psrf_to);

   trv0.set_backward();
   trv = trv0;
   trv_der=trv;
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );

   assert( pstat.same() );
   assert(trv.get_surface()->pure_equal(*psrf_to));

   check_zero_propagation(trv0,trv,pstat);
   check_derivatives(prop0,trv_der,*psrf_to);
 }
 {
   // check that with zero field all danything/dqpt derivatives are zero.
   ETrack tre(SurfacePtr(new SurfXYPlane(10.,0.1)));
   TrackError err;
   for(int i=0;i<5;i++) 
     err(i,i)=1.;
   TrackVector vec;
   vec(SurfXYPlane::IV)=0.1;
   vec(SurfXYPlane::IZ)=0.1;
   vec(SurfXYPlane::IDVDU)=0.1;
   vec(SurfXYPlane::IDZDU)=0.1;
   vec(SurfXYPlane::IQP)=0.1;
   tre.set_vector(vec);
   tre.set_error(err);
   tre.set_forward();
   PropStat pstat = 
     PropXYCyl(ObjDoublePtr(new ObjDouble(0.0))).err_prop(tre,SurfCylinder(15.));
   assert(pstat.success());
   cout<<tre<<endl;
   /*
   assert( fabs(tre.get_error(SurfCylinder::IQPT,SurfCylinder::IQPT)-1) < 1e-10);
   assert( fabs(tre.get_error(SurfCylinder::IQPT,SurfCylinder::IPHI)) < 1e-10);
   assert( fabs(tre.get_error(SurfCylinder::IQPT,SurfCylinder::IZ)) < 1e-10);
   assert( fabs(tre.get_error(SurfCylinder::IQPT,SurfCylinder::IALF)) < 1e-10);
   assert( fabs(tre.get_error(SurfCylinder::IQPT,SurfCylinder::ITLM)) < 1e-10);
   */
 }
 //********************************************************************

 cout << ok_prefix << "Test cloning." << endl;
 assert( prop.new_propagator() != 0 );

 //********************************************************************

 cout << ok_prefix << "Test the field." << endl;
 assert( prop.get_bfield() == -2.0 );

 //********************************************************************

  ObjTable::register_type<PropXYCyl>();

  cout << ok_prefix << "Write object data." << endl;
  {
    ObjPtr pobj( new PropXYCyl(ObjDoublePtr(new ObjDouble(12.3))));
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("obj1",pobj);
    cout << mystream.str() << endl;
    assert( ObjTable::has_object_name("obj1") );
    string::size_type pos = 0;
    assert( (pos=mystream.str().find( "obj1 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "PropXYCyl ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "bfield",pos)) != string::npos );
    assert( (pos=mystream.str().find( "12.3 ",pos)) != string::npos );
  }

  //********************************************************************

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ obj2 PropXYCyl bfield=24.6 ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const PropXYCyl* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() == PropXYCyl::get_static_type() );
      assert( is_equal( pobj->get_bfield(), 24.6 ) );
    }
  }

  //********************************************************************

 cout << ok_prefix
     << "------------- All tests passed. -------------" << endl;
 return 0;

 //********************************************************************
}



namespace {
void  check_zero_propagation(const VTrack&trv0,const VTrack& trv,const PropStat& pstat) {

   SpacePoint sp = trv.space_point();
   SpacePoint sp0 = trv0.space_point();

   SpacePath sv = trv.space_vector();
   SpacePath sv0 = trv0.space_vector();

   assert( fabs(sv0.dx() - sv.dx())<1e-7 );
   assert( fabs(sv0.dy() - sv.dy())<1e-7 );
   assert( fabs(sv0.dz() - sv.dz())<1e-7 );
   
   double x0 = sp0.x();
   double y0 = sp0.y();
   double z0 = sp0.z();
   double x1 = sp.x();
   double y1 = sp.y();
   double z1 = sp.z();

   double dx = sv.dx();
   double dy = sv.dy();
   double dz = sv.dz();

   double prod = dx*(x1-x0)+dy*(y1-y0)+dz*(z1-z0);
   double moda = sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0) + (z1-z0)*(z1-z0));
   double modb = sqrt(dx*dx+dy*dy+dz*dz);
   double st = pstat.path_distance();
   assert( fabs(prod-st) < 1.e-7 );
   assert( fabs(fabs(prod) - moda*modb) < 1.e-7 );
 }

void check_derivatives(const Propagator& prop,const VTrack& trv0,const Surface& srf) {
  for(int i=0;i<4;++i)
    for(int j=0;j<4;++j)
      check_derivative(prop,trv0,srf,i,j);
}

void check_derivative(const Propagator& prop,const VTrack& trv0,const Surface& srf,int i,int j) {

  double dx = 1.e-3;
  VTrack trv = trv0;
  TrackVector vec = trv.get_vector();
  bool forward = trv0.is_forward();

  VTrack trv_0 = trv0;
  TrackDerivative der;
  PropStat pstat = prop.vec_prop(trv_0,srf,&der);
  assert(pstat.success());

  TrackVector tmp=vec;
  tmp(j)+=dx;
  trv.set_vector(tmp);
  if(forward) trv.set_forward();
  else trv.set_backward();

  VTrack trv_pl = trv;
  pstat = prop.vec_prop(trv_pl,srf);
  assert(pstat.success());

  TrackVector vecpl = trv_pl.get_vector();

  tmp=vec;
  tmp(j)-=dx;
  trv.set_vector(tmp);
  if(forward) trv.set_forward();
  else trv.set_backward();

  VTrack trv_mn = trv;
  pstat = prop.vec_prop(trv_mn,srf);
  assert(pstat.success());

  TrackVector vecmn = trv_mn.get_vector();

  double dy = (vecpl(i)-vecmn(i))/2.;

  double dydx = dy/dx;

  double didj = der(i,j);

  if( fabs(didj) > 1e-10 )
    { 
      if( fabs((dydx - didj)/didj) >= 1e-4 )
	cout<<"i="<< i <<" j="<< j << " "<<dydx << " "<< didj<<'\n';
	assert( fabs((dydx - didj)/didj) < 1e-4 );
    }
  else
    { assert( fabs(dydx) < 1e-4 ) ;}
 }
}