// PropCylZ_t.cc

// Test PropCylZ.

#include "PropCylZ.h"
#include "trfbase/PropStat.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfcyl/SurfCylinder.h"
#include "trfzp/SurfZPlane.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

// Assign track parameter indices.
enum {
  IPHI   = SurfCylinder::IPHI, 
  IZ_CYL = SurfCylinder::IZ, 
  IALF   = SurfCylinder::IALF, 
  ITLM   = SurfCylinder::ITLM, 
  IQPT   = SurfCylinder::IQPT,

  IX = SurfZPlane::IX,
  IY   = SurfZPlane::IY,
  IDXDZ = SurfZPlane::IDXDZ,
  IDYDZ = SurfZPlane::IDYDZ,
  IQP_Z  = SurfZPlane::IQP
};

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) ;
}
// Very well tested Cyl-Z propagator. Each new one can be tested against it

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

  // phi, z, alpha=phi_dir-phi, tan(lambda)=pz/pt, q/pt
  // c1   c2   c3                c4                 c5
  //
  // x   y   dx/dz dy/dz q/p
  // a1  a2  a3    a4    a5
  //
  // phi is polar angle of track momentum, R radius of the track in field B
  // dphi is angle of rotation of momentum
  // Rcyl is cylinder radius
  //
  // a1n = a1 + Rsin_phi*(cos_dphi - 1) + Rcos_phi*sin_dphi; 
  // a2n = a2 - Rcos_phi*(cos_dphi - 1) + Rsin_phi*sin_dphi;
  // a3n = a3*cos_dphi - a4*sin_dphi;
  // a4n = a3*sin_dphi + a4*cos_dphi;
  // a5n = a5;

  // a1 = Rcyl*cos_c1;
  // a2 = Rcyl*sin_c1;
  // a3 = cos(c1+c3)/c4;
  // a4 = sin(c1+c3)/c4;
  // a5 = c5/sqrt(1+c4*c4);
  //
  // dz = zpos - c2


 // 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 Cylinder.
 assert( srf1.get_pure_type() == SurfCylinder::get_static_type() );
 if ( srf1.get_pure_type( ) != SurfCylinder::get_static_type() )
   return pstat;
 const SurfCylinder& scy1 = (const SurfCylinder&) srf1;


 // Fetch the R of the cylinder and the starting track vector.
 int ir  = SurfCylinder::RADIUS;
 double Rcyl = scy1.get_parameter(ir);
 
 TrackVector vec = trv.get_vector();
 double c1 = vec(IPHI);                 // phi
 double c2 = vec(IZ_CYL);               // z
 double c3 = vec(IALF);                 // alpha
 double c4 = vec(ITLM);                 // tan(lambda)
 double c5 = vec(IQPT);                 // q/pt

 // check that dz != 0
 if(c4 == 0.) return pstat;
 
 double cos_c1 = cos(c1);
 double sin_c1 = sin(c1);
 double cos_dir = cos(c1+c3);
 double sin_dir = sin(c1+c3);
 double c4_hat2 = 1+c4*c4;
 double c4_hat  = sqrt(c4_hat2);

 double a1 = Rcyl*cos_c1;
 double a2 = Rcyl*sin_c1;
 double a3 = cos_dir/c4;
 double a4 = sin_dir/c4;
 double a5 = c5/c4_hat;


 // if tan(lambda)=dz/dst > 0 : dz>0; dzdst < 0 dz<0

 int sign_dz;
 if(c4 > 0) sign_dz =  1;
 if(c4 < 0) sign_dz = -1;


 // Check destination is a ZPlane.
 assert( srf.get_pure_type() == SurfZPlane::get_static_type() );
 if ( srf.get_pure_type( ) != SurfZPlane::get_static_type() )
   return pstat;
 const SurfZPlane& szp2 = (const SurfZPlane&) srf;

 // Fetch the zpos's of the planes.
 int izpos  = SurfZPlane::ZPOS;

 double zpos = szp2.get_parameter(izpos);

 double dz = zpos - c2;

 switch (dir) {
  case Propagator::NEAREST:
   cerr << 
   "vec_propcylz: Propagation in NEAREST direction is undefined" 
   << endl;
   abort();
  break;

  case Propagator::FORWARD:

    //   cyl-z propagation failed 
   if(sign_dz*dz<0.)  return pstat;
  break;

  case Propagator::BACKWARD:

    //   cyl-z propagation failed 
   if(sign_dz*dz>0.) return pstat;
  break;

  default:
   cerr << "vec_propcylz: Unknown direction." << endl;
   abort();
 }

 double a34_hat = sign_dz*sqrt(a3*a3 + a4*a4);
 double a34_hat2 = a34_hat*a34_hat;
 double dphi = B*dz*a5*sqrt(1.+a34_hat2)*sign_dz;
 double cos_dphi = cos(dphi);
 double sin_dphi = sin(dphi);

 double Rcos_phi = 1./(B*a5)*a3/sqrt(1.+a34_hat2)*sign_dz;
 double Rsin_phi = 1./(B*a5)*a4/sqrt(1.+a34_hat2)*sign_dz; 

 double a1n = a1 + Rsin_phi*(cos_dphi - 1) + Rcos_phi*sin_dphi; 
 double a2n = a2 - Rcos_phi*(cos_dphi - 1) + Rsin_phi*sin_dphi;
 double a3n =  a3*cos_dphi - a4*sin_dphi;
 double a4n =  a3*sin_dphi + a4*cos_dphi;
 double a5n = a5;

 vec(IX)    = a1n;
 vec(IY)    = a2n;
 vec(IDXDZ) = a3n;
 vec(IDYDZ) = a4n;
 vec(IQP_Z) = a5n;

 // Update trv
 trv.set_surface(SurfacePtr(srf.new_pure_surface()));
 trv.set_vector(vec);

 // set new direction of the track

 if(sign_dz ==  1) trv.set_forward();
 if(sign_dz == -1) trv.set_backward();

 // Set the return status.

 switch (dir) {
  case Propagator::FORWARD:
   pstat.set_path_distance(1);
  break;
  case Propagator::BACKWARD:
   pstat.set_path_distance(-1);
  break;
 default: ;
 }

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

  //da34_hat

 double da34_hat_da3;
 double da34_hat_da4;
 if(abs(a3)>=abs(a4))
 {
  int sign3=1;
  if(a3<0) sign3 = -1;
  if(a3 !=0.){
   da34_hat_da3 = sign_dz*sign3/sqrt(1.+(a4/a3)*(a4/a3) );
   da34_hat_da4 = sign_dz*(a4/abs(a3))/sqrt(1.+(a4/a3)*(a4/a3) );
  }
  else {
   da34_hat_da3 = 0.;
   da34_hat_da4 = 0.;
  }
 }
 if(abs(a4)>abs(a3))
 {
  int sign4=1;
  if(a4<0) sign4 = -1;
  if(a4 !=0.){
  da34_hat_da3 = sign_dz*(a3/abs(a4))/sqrt(1.+(a3/a4)*(a3/a4) );
  da34_hat_da4 = sign_dz*sign4/sqrt(1.+(a3/a4)*(a3/a4) );
  }
  else {
   da34_hat_da3 = 0.;
   da34_hat_da4 = 0.;
  }
 }

  // ddphi / da

 double ddphi_da3 = B*dz*a5*a34_hat*sign_dz/sqrt(1.+a34_hat2)*da34_hat_da3;
 double ddphi_da4 = B*dz*a5*a34_hat*sign_dz/sqrt(1.+a34_hat2)*da34_hat_da4;
 double ddphi_da5 = B*dz*sqrt(a34_hat2+1.)*sign_dz;

  // dRsin_phi

 double dRsin_phi_da3 = -Rsin_phi*a34_hat/(1.+a34_hat2)*da34_hat_da3;
 double dRsin_phi_da4 = -Rsin_phi*a34_hat/(1.+a34_hat2)*da34_hat_da4 +
                            sign_dz/(B*a5)/sqrt(1.+a34_hat2);
 double dRsin_phi_da5 = -Rsin_phi/a5;

  // dRcos_phi

 double dRcos_phi_da3 = -Rcos_phi*a34_hat/(1.+a34_hat2)*da34_hat_da3 +
                            sign_dz/(B*a5)/sqrt(1.+a34_hat2);
 double dRcos_phi_da4 = -Rcos_phi*a34_hat/(1.+a34_hat2)*da34_hat_da4;
 double dRcos_phi_da5 = -Rcos_phi/a5;

  // da1n first two are simple because dR,dphi _da1,_da2 = 0. 

 // double da1n_da1 = 1.;
 double da1n_da3 = dRsin_phi_da3*(cos_dphi-1.) + dRcos_phi_da3*sin_dphi 
                 - Rsin_phi*sin_dphi*ddphi_da3 + Rcos_phi*cos_dphi*ddphi_da3;
 double da1n_da4 = dRsin_phi_da4*(cos_dphi-1.) + dRcos_phi_da4*sin_dphi 
                 - Rsin_phi*sin_dphi*ddphi_da4 + Rcos_phi*cos_dphi*ddphi_da4;
 double da1n_da5 = dRsin_phi_da5*(cos_dphi-1.) + dRcos_phi_da5*sin_dphi 
                 - Rsin_phi*sin_dphi*ddphi_da5 + Rcos_phi*cos_dphi*ddphi_da5;

  // da2n first two are simple because dR,dphi _da1,_da2 = 0. 

 // double da2n_da2 = 1.;
 double da2n_da3 = -dRcos_phi_da3*(cos_dphi-1.) + dRsin_phi_da3*sin_dphi
                  + Rcos_phi*sin_dphi*ddphi_da3 + Rsin_phi*cos_dphi*ddphi_da3;
 double da2n_da4 = -dRcos_phi_da4*(cos_dphi-1.) + dRsin_phi_da4*sin_dphi
                  + Rcos_phi*sin_dphi*ddphi_da4 + Rsin_phi*cos_dphi*ddphi_da4;
 double da2n_da5 = -dRcos_phi_da5*(cos_dphi-1.) + dRsin_phi_da5*sin_dphi
                  + Rcos_phi*sin_dphi*ddphi_da5 + Rsin_phi*cos_dphi*ddphi_da5;
  
  // da3n first two are simple because dphi _da1,_da2 = 0.

 double da3n_da3 = cos_dphi - a3*sin_dphi*ddphi_da3 - a4*cos_dphi*ddphi_da3;
 double da3n_da4 = - sin_dphi - a3*sin_dphi*ddphi_da4 - a4*cos_dphi*ddphi_da4;
 double da3n_da5 = - a3*sin_dphi*ddphi_da5 - a4*cos_dphi*ddphi_da5;

  // da4n first two are simple because dphi _da1,_da2 = 0.

 double da4n_da3 =   sin_dphi + a3*cos_dphi*ddphi_da3 - a4*sin_dphi*ddphi_da3;
 double da4n_da4 =   cos_dphi + a3*cos_dphi*ddphi_da4 - a4*sin_dphi*ddphi_da4;
 double da4n_da5 =   a3*cos_dphi*ddphi_da5 - a4*sin_dphi*ddphi_da5;

  // da5n 
  
 // double da5n_da5 = 1.;

  // ddphi / dc
  
 double ddphi_dc2 = -B*a5*sqrt(a34_hat2+1.)*sign_dz;

  // da1 / dc

 double da1_dc1 = -Rcyl*sin_c1; 

  // da2 / dc

 double da2_dc1 =  Rcyl*cos_c1; 

  // da3 / dc

 double da3_dc1 = -sin_dir/c4; 
 double da3_dc3 = -sin_dir/c4;
 double da3_dc4 = -cos_dir/(c4*c4);

  // da4 / dc

 double da4_dc1 =  cos_dir/c4;
 double da4_dc3 =  cos_dir/c4;
 double da4_dc4 = -sin_dir/(c4*c4); 

  // da5 / dc

 double da5_dc4 =  -c5*c4/(c4_hat*c4_hat2);
 double da5_dc5 =   1./c4_hat;
 
  // da1n/ dc

 double da1n_dc1 = da1_dc1 + da1n_da3*da3_dc1 + da1n_da4*da4_dc1; 
 double da1n_dc2 = -Rsin_phi*sin_dphi*ddphi_dc2 + Rcos_phi*cos_dphi*ddphi_dc2;
 double da1n_dc3 = da1n_da3*da3_dc3 + da1n_da4*da4_dc3;
 double da1n_dc4 = da1n_da3*da3_dc4 + da1n_da4*da4_dc4 + da1n_da5*da5_dc4;
 double da1n_dc5 = da1n_da5*da5_dc5;

  // da2n/ dc

 double da2n_dc1 = da2_dc1 + da2n_da3*da3_dc1 + da2n_da4*da4_dc1; 
 double da2n_dc2 = Rcos_phi*sin_dphi*ddphi_dc2 + Rsin_phi*cos_dphi*ddphi_dc2;
 double da2n_dc3 = da2n_da3*da3_dc3 + da2n_da4*da4_dc3;
 double da2n_dc4 = da2n_da3*da3_dc4 + da2n_da4*da4_dc4 + da2n_da5*da5_dc4;
 double da2n_dc5 = da2n_da5*da5_dc5;

  // da3n/ dc

 double da3n_dc1 = da3n_da3*da3_dc1 + da3n_da4*da4_dc1; 
 double da3n_dc2 = -a3*sin_dphi*ddphi_dc2 - a4*cos_dphi*ddphi_dc2;
 double da3n_dc3 = da3n_da3*da3_dc3 + da3n_da4*da4_dc3;
 double da3n_dc4 = da3n_da3*da3_dc4 + da3n_da4*da4_dc4 + da3n_da5*da5_dc4;
 double da3n_dc5 = da3n_da5*da5_dc5;
 
  // da4n/ dc

 double da4n_dc1 = da4n_da3*da3_dc1 + da4n_da4*da4_dc1; 
 double da4n_dc2 = a3*cos_dphi*ddphi_dc2 - a4*sin_dphi*ddphi_dc2;
 double da4n_dc3 = da4n_da3*da3_dc3 + da4n_da4*da4_dc3;
 double da4n_dc4 = da4n_da3*da3_dc4 + da4n_da4*da4_dc4 + da4n_da5*da5_dc4;
 double da4n_dc5 = da4n_da5*da5_dc5;

  // da5n/ dc 
  
 double da5n_dc1 = 0.;
 double da5n_dc2 = 0.;
 double da5n_dc3 = 0.; 
 double da5n_dc4 = da5_dc4;
 double da5n_dc5 = da5_dc5;

 TrackDerivative& deriv = *pderiv;

 deriv(IX,IPHI)      =da1n_dc1;
 deriv(IX,IZ_CYL)    =da1n_dc2;
 deriv(IX,IALF)      =da1n_dc3;
 deriv(IX,ITLM)      =da1n_dc4;
 deriv(IX,IQPT)      =da1n_dc5;
 deriv(IY,IPHI)      =da2n_dc1;
 deriv(IY,IZ_CYL)    =da2n_dc2;
 deriv(IY,IALF)      =da2n_dc3;
 deriv(IY,ITLM)      =da2n_dc4;
 deriv(IY,IQPT)      =da2n_dc5;
 deriv(IDXDZ,IPHI)   =da3n_dc1;
 deriv(IDXDZ,IZ_CYL) =da3n_dc2;
 deriv(IDXDZ,IALF)   =da3n_dc3;
 deriv(IDXDZ,ITLM)   =da3n_dc4;
 deriv(IDXDZ,IQPT)   =da3n_dc5;
 deriv(IDYDZ,IPHI)   =da4n_dc1;
 deriv(IDYDZ,IZ_CYL) =da4n_dc2;
 deriv(IDYDZ,IALF)   =da4n_dc3;
 deriv(IDYDZ,ITLM)   =da4n_dc4;
 deriv(IDYDZ,IQPT)   =da4n_dc5;
 deriv(IQP_Z,IPHI)   =da5n_dc1;
 deriv(IQP_Z,IZ_CYL) =da5n_dc2;
 deriv(IQP_Z,IALF)   =da5n_dc3;
 deriv(IQP_Z,ITLM)   =da5n_dc4;
 deriv(IQP_Z,IQPT)   =da5n_dc5;

 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 = "PropCylZ (I): ";
  string error_prefix = "PropCylZ test (E): ";

  cout << ok_prefix
       << "-------- Testing component PropCylZ. --------" << 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;
  PropCylZ prop(ObjDoublePtr(new ObjDouble(-BFIELD)));
  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;

 PropCylZ propcylz(ObjDoublePtr(new ObjDouble(-BFIELD/BFAC)));

 double phi[]   ={  PI/5,   PI/6,   PI/6,   4/3*PI,  5/3*PI,  5/3*PI };
 double z[]     ={  1.5,   -2.3,    0.,     1.5,    -1.5,    -1.5    };
 double alpha[] ={  0.1,   -0.1,    0.,     0.2,    -0.2,     0.     };
 double tlm[]   ={  2.3,   -1.5,   -2.3,    2.3,    -2.3,     2.3    };
 double qpt[]   ={  0.01,  -0.01,   0.01,  -0.01,   -0.01,    0.01   };

 double z2[]    ={  5.5,   -6.0,   -5.0,    6.0,    -5.7,     4.0    };
 double z2b[]   ={ -4.0,    4.0,    5.0,   -3.0,     3.0,    -6.0    };

 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;
  SurfCylinder scy1(10.);
  SurfZPlane   szp2(z2[i]);
  SurfZPlane   szp2b(z2b[i]);

  TrackVector vec1; 

  vec1(IPHI)   = phi[i];            // phi
  vec1(IZ_CYL) = z[i];              // z
  vec1(IALF)   = alpha[i];          // alpha
  vec1(ITLM)   = tlm[i];            // tan(lambda)
  vec1(IQPT)   = qpt[i];            // q/pt

  VTrack trv1(SurfacePtr(scy1.new_pure_surface()),vec1);

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

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

  VTrack trv2f_my = trv1;
  pstat = vec_propcylz(BFIELD,trv2f_my,szp2,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 = propcylz.vec_dir_prop(trv2b,szp2b,Propagator::BACKWARD);
  assert( pstat.backward() );  
  cout << "\n  backward: " << trv2b << endl;

  VTrack trv2b_my = trv1;
  pstat = vec_propcylz(BFIELD,trv2b_my,szp2b,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 epp[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double epz[] = {  0.01,  -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,   0.25  };
 double epa[] = {  0.003, -0.003,  0.003, -0.003,  0.003, -0.003,  0.003 };
 double eza[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double eaa[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double epl[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double eal[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double ezl[] = {  0.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04,   0.04  };
 double ell[] = {  0.02,   0.02,   0.02,   0.02,   0.02,   0.02,   0.02  };
 double epc[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double ezc[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double eac[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double elc[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 double ecc[] = {  0.01,   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;
  SurfCylinder scy1(10.);
  SurfZPlane   szp2(z2[i]);
  SurfZPlane   szp2b(z2b[i]);

  TrackVector vec1; 

  vec1(IPHI)   = phi[i];            // phi
  vec1(IZ_CYL) = z[i];              // z
  vec1(IALF)   = alpha[i];          // alpha
  vec1(ITLM)   = tlm[i];            // tan(lambda)
  vec1(IQPT)   = qpt[i];            // q/pt

  TrackError err1;
 
  err1(IPHI,IPHI)     = epp[i];
  err1(IPHI,IZ_CYL)   = epz[i];
  err1(IZ_CYL,IZ_CYL) = ezz[i];
  err1(IPHI,IALF)     = epa[i];
  err1(IZ_CYL,IALF)   = eza[i];
  err1(IALF,IALF)     = eaa[i];
  err1(IPHI,ITLM)     = epl[i];
  err1(IZ_CYL,ITLM)   = ezl[i];
  err1(IALF,ITLM)     = eal[i];
  err1(ITLM,ITLM)     = ell[i];
  err1(IPHI,IQPT)     = epc[i];
  err1(IZ_CYL,IQPT)   = ezc[i];
  err1(IALF,IQPT)     = eac[i];
  err1(ITLM,IQPT)     = elc[i];
  err1(IQPT,IQPT)     = ecc[i];

  ETrack trv1(SurfacePtr(scy1.new_pure_surface()),vec1,err1);

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

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

  ETrack trv2f_my = trv1;
  TrackDerivative deriv; 
  pstat = vec_propcylz(BFIELD,trv2f_my,szp2,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 = propcylz.err_dir_prop(trv2b,szp2b,Propagator::BACKWARD);
  assert( pstat.backward() );  
  cout << "\n  backward: " << trv2b << endl;

  ETrack trv2b_my = trv1;
  pstat = vec_propcylz(BFIELD,trv2b_my,szp2b,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 << "Test Zero Field Propagation." << endl;
 {
   PropCylZ prop0(ObjDoublePtr(new ObjDouble(0.0)));
   cout << prop0 << endl;
   assert( prop0.get_bfield() == 0. );

   double z=6.;
   SurfacePtr psrf(new SurfCylinder(13.0));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfCylinder::IPHI) = 0.1;
   vec(SurfCylinder::IZ) = 10.;
   vec(SurfCylinder::IALF) = 0.1;
   vec(SurfCylinder::ITLM) = 2.;
   vec(SurfCylinder::IQPT) = 0.;   
   trv0.set_vector(vec);
   trv0.set_forward();
   SurfacePtr psrf_to(new SurfZPlane(z));

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

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

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

   psrf_to = new SurfZPlane(10.);
   trv = trv0;
   trv_der=trv;
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   cout<<trv<<'\n';
   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 SurfCylinder(10.)));
   TrackError err;
   for(int i=0;i<5;i++) 
     err(i,i)=1.;
   TrackVector vec;
   vec(SurfCylinder::IPHI)=0.1;
   vec(SurfCylinder::IZ)=0.1;
   vec(SurfCylinder::IALF)=0.1;
   vec(SurfCylinder::ITLM)=0.1;
   vec(SurfCylinder::IQPT)=0.1;
   tre.set_vector(vec);
   tre.set_error(err);
   tre.set_forward();
   PropStat pstat = 
     PropCylZ(ObjDoublePtr(new ObjDouble(0.0))).err_prop(tre,SurfZPlane(15.));
   assert(pstat.success());
   cout<<tre<<endl;
   /*
   assert( fabs(tre.get_error(SurfZPlane::IQP,SurfZPlane::IQP)-1) < 1e-10);
   assert( fabs(tre.get_error(SurfZPlane::IQP,SurfZPlane::IX)) < 1e-10);
   assert( fabs(tre.get_error(SurfZPlane::IQP,SurfZPlane::IY)) < 1e-10);
   assert( fabs(tre.get_error(SurfZPlane::IQP,SurfZPlane::IDXDZ)) < 1e-10);
   assert( fabs(tre.get_error(SurfZPlane::IQP,SurfZPlane::IDYDZ)) < 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<PropCylZ>();

  cout << ok_prefix << "Write object data." << endl;
  {
    ObjPtr pobj( new PropCylZ(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( "PropCylZ ",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 PropCylZ bfield=24.6 ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const PropCylZ* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() == PropCylZ::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 )
    assert( fabs((dydx - didj)/didj) < 1e-4 );
  else
    assert( fabs(dydx) < 1e-4 );
}
}