// PropXYZ_t.cpp

// Test PropXYZ.

#include "PropXYZ.h"
#include "trfbase/PropStat.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfxyp/SurfXYPlane.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

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 XY-Z propagator. Each new one can be tested against it

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

  // v, z, dv/du,dz/du,q/p
  // b1 b2 b3    b4    b5

  // phi is polar angle of track momentum, R radius of the track in field B
  // dphi is angle of rotation of momentum
  // sphi is polar angle of the plane normal 
  //
  // 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 = u*cos_sphi - b1*sin_sphi;
  // a2 = b1*cos_sphi + u*sin_sphi;
  // a3 = cos_sphi/b4 - b3/b4*sin_sphi;
  // a4 = b3/b4*cos_sphi + sin_sphi/b4;
  // a5 = b5;
  //
  // dz = zpos - b2

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

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

 // 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;
 
 // Fetch the u and phi of the plane.
 int iphi  = SurfXYPlane::NORMPHI;
 int idist = SurfXYPlane::DISTNORM;

 double sphi = sxyp1.get_parameter(iphi);
 double    u = sxyp1.get_parameter(idist);

 TrackVector vec = trv.get_vector();
 double b1 = vec(IV);                  // v
 double b2 = vec(IZ);                  // z
 double b3 = vec(IDVDU);               // dv/du
 double b4 = vec(IDZDU);               // dz/du
 double b5 = vec(IQP_XY);              // q/p

 // check that dz != 0
 if(b4 == 0.) return pstat;

 double cos_sphi = cos(sphi);
 double sin_sphi = sin(sphi);

 double a1 = u*cos_sphi - b1*sin_sphi;
 double a2 = b1*cos_sphi + u*sin_sphi; 
 double a3 = cos_sphi/b4 - b3/b4*sin_sphi;
 double a4 = b3/b4*cos_sphi + sin_sphi/b4;
 double a5 = b5;

 // 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 and the starting track vector.
 int izpos  = SurfZPlane::ZPOS;

 double zpos = szp2.get_parameter(izpos);

 double dz = zpos - b2;

 // if delta_z * dz/du*du/dt < 0 - fail forward
 // if delta_z * dz/du*du/dt > 0 - fail backward
 
 // if dz/du*du/dt > 0 and forward : dz>0; dz/du*du/dt < 0 dz<0
 // if dz/du*du/dt < 0 and backward : dz>0; dz/du*du/dt > 0 dz<0  

 int sign_du = 0;
 if(trv.is_forward())  sign_du =  1;
 if(trv.is_backward()) sign_du = -1;
 if(sign_du == 0){
  cerr << "vec_propxyz: Unknown direction of a track "<< endl;
  abort();
 }
 
 int sign_dz;
 if(sign_du*b4>0.) sign_dz =  1;
 if(sign_du*b4<0.) sign_dz = -1;

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

  case Propagator::FORWARD:

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

  case Propagator::BACKWARD:

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

  default:
   cerr << "vec_propxyz: 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 / db
  
 double ddphi_db2 = -B*a5*sqrt(a34_hat2+1.)*sign_dz;

  // da3 / db

 double da3_db3 = -sin_sphi/b4; 
 double da3_db4 = (-cos_sphi + b3*sin_sphi)/(b4*b4);


  // da4 / db

 double da4_db3 =  cos_sphi/b4;
 double da4_db4 = ( -sin_sphi - b3*cos_sphi)/(b4*b4); 
 
  // da1/ db

 double da1n_db1 = -sin_sphi; 
 double da1n_db2 = -Rsin_phi*sin_dphi*ddphi_db2 + Rcos_phi*cos_dphi*ddphi_db2;
 double da1n_db3 = da1n_da3*da3_db3 + da1n_da4*da4_db3;
 double da1n_db4 = da1n_da3*da3_db4 + da1n_da4*da4_db4;
 double da1n_db5 = da1n_da5;

  // da2/ db

 double da2n_db1 = cos_sphi; 
 double da2n_db2 = Rcos_phi*sin_dphi*ddphi_db2 + Rsin_phi*cos_dphi*ddphi_db2;
 double da2n_db3 = da2n_da3*da3_db3 + da2n_da4*da4_db3;
 double da2n_db4 = da2n_da3*da3_db4 + da2n_da4*da4_db4;
 double da2n_db5 = da2n_da5;

  // da3/ db

 double da3n_db1 = 0.; 
 double da3n_db2 = -a3*sin_dphi*ddphi_db2 - a4*cos_dphi*ddphi_db2;
 double da3n_db3 = da3n_da3*da3_db3 + da3n_da4*da4_db3;
 double da3n_db4 = da3n_da3*da3_db4 + da3n_da4*da4_db4;
 double da3n_db5 = da3n_da5;
 
  // da4/ db

 double da4n_db1 = 0.; 
 double da4n_db2 = a3*cos_dphi*ddphi_db2 - a4*sin_dphi*ddphi_db2;
 double da4n_db3 = da4n_da3*da3_db3 + da4n_da4*da4_db3;
 double da4n_db4 = da4n_da3*da3_db4 + da4n_da4*da4_db4;
 double da4n_db5 = da4n_da5;

  // da5n 
  
 double da5n_db1 = 0.;
 double da5n_db2 = 0.;
 double da5n_db3 = 0.; 
 double da5n_db4 = 0.;
 double da5n_db5 = 1.;

 TrackDerivative& deriv = *pderiv;

 deriv(IX,IV)        =da1n_db1;
 deriv(IX,IZ)        =da1n_db2;
 deriv(IX,IDVDU)     =da1n_db3;
 deriv(IX,IDZDU)     =da1n_db4;
 deriv(IX,IQP_XY)    =da1n_db5;
 deriv(IY,IV)        =da2n_db1;
 deriv(IY,IZ)        =da2n_db2;
 deriv(IY,IDVDU)     =da2n_db3;
 deriv(IY,IDZDU)     =da2n_db4;
 deriv(IY,IQP_XY)    =da2n_db5;
 deriv(IDXDZ,IV)     =da3n_db1;
 deriv(IDXDZ,IZ)     =da3n_db2;
 deriv(IDXDZ,IDVDU)  =da3n_db3;
 deriv(IDXDZ,IDZDU)  =da3n_db4;
 deriv(IDXDZ,IQP_XY) =da3n_db5;
 deriv(IDYDZ,IV)     =da4n_db1;
 deriv(IDYDZ,IZ)     =da4n_db2;
 deriv(IDYDZ,IDVDU)  =da4n_db3;
 deriv(IDYDZ,IDZDU)  =da4n_db4;
 deriv(IDYDZ,IQP_XY) =da4n_db5;
 deriv(IQP_Z,IV)     =da5n_db1;
 deriv(IQP_Z,IZ)     =da5n_db2;
 deriv(IQP_Z,IDVDU)  =da5n_db3;
 deriv(IQP_Z,IDZDU)  =da5n_db4;
 deriv(IQP_Z,IQP_XY) =da5n_db5;

 return pstat; 
}
//**********************************************************************
int main( ) {

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

  cout << ok_prefix
       << "-------- Testing component PropXYZ. --------" << 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;
  PropXYZ 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;

 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[]   ={  3.,       3.,      3.,      3.,      3.,      3.    };
 double dvdu[]={ 1.5,     -2.3,     0.,      0.,     -1.5,      0.    };
 double dzdu[]={-2.3,      1.5,    -2.3,     1.5,     2.3,     -1.5   };
 double qp[]  ={ 0.01,    -0.01,    0.01,   -0.01,   -0.01,     0.01  };

 double z2[]  ={ 10.,      20.,     10.,     10.,     10.,     10.};
 double z2b[] ={-10.,     -20.,    -20.,    -10.,    -10.,    -10.};

 double maxdiff = 1.e-10;
 int ntrk = 6;
 int i;
 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;
  PropStat pstat;
  SurfXYPlane sxyp1(u1[i],phi1[i]);
  SurfZPlane   szp2(z2[i]);
  SurfZPlane  szp2b(z2b[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 << " starting: " << trv1 << endl;

  VTrack trv2f = trv1;
  pstat = prop.vec_dir_prop(trv2f,szp2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "  forward: " << trv2f << endl;
  VTrack trv2b = trv1;
  pstat = prop.vec_dir_prop(trv2b,szp2b,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;

  // Check against correct propagator that I checked somewhere else
  VTrack trv2fc = trv1;
  pstat = vec_propxyz(BFIELD,trv2fc,szp2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "  forward: " << trv2fc << endl;
  VTrack trv2bc = trv1;
  pstat = vec_propxyz(BFIELD,trv2bc,szp2b,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2bc << endl;
 
  double difff = 
  szp2.vec_diff(trv2f.get_vector(),trv2fc.get_vector()).amax();
  double diffb = 
  szp2b.vec_diff(trv2b.get_vector(),trv2bc.get_vector()).amax();
  cout << "diffs: " << difff << ' ' << diffb << endl;
  assert( difff < maxdiff );
  assert( diffb < maxdiff );
 }

 // Repeat the above with errors.
 cout << ok_prefix << "Check reversibility 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  };

 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;
  PropStat pstat;
  SurfXYPlane sxyp1(u1[i],phi1[i]);
  SurfZPlane   szp2(z2[i]);
  SurfZPlane  szp2b(z2b[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 << " starting: " << trv1 << endl;

  ETrack trv2f = trv1;
  pstat = prop.err_dir_prop(trv2f,szp2,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  forward: " << trv2f << endl;
  ETrack trv2b = trv1;
  pstat = prop.err_dir_prop(trv2b,szp2b,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;

  // Check against correct propagator that I checked somewhere else
  ETrack trv2fc = trv1;
  TrackDerivative deriv;
  pstat = vec_propxyz(BFIELD,trv2fc,szp2,Propagator::FORWARD,&deriv);
  assert( pstat.forward() );
  TrackError err = trv2fc.get_error();
  trv2fc.set_error( err.Xform(deriv) );
  cout << "  forward: " << trv2fc << endl;

  ETrack trv2bc = trv1;
  pstat = vec_propxyz(BFIELD,trv2bc,szp2b,Propagator::BACKWARD,&deriv);
  assert( pstat.backward() );
  err = trv2bc.get_error();
  trv2bc.set_error( err.Xform(deriv) ); 
  cout << " backward: " << trv2bc << endl;

  double difff = 
  szp2.vec_diff(trv2f.get_vector(),trv2fc.get_vector()).amax();
  double diffb = 
  szp2b.vec_diff(trv2b.get_vector(),trv2bc.get_vector()).amax();
  cout << "diffs: " << difff << ' ' << diffb << endl;
  assert( difff < maxdiff );
  assert( diffb < maxdiff );

  TrackError dfc = trv2f.get_error() - trv2fc.get_error();
  TrackError dbc = trv2b.get_error() - trv2bc.get_error();
  double edifff = dfc.amax();
  double ediffb = dbc.amax();
  cout << "err diffs: " << edifff << ' ' << ediffb << endl;
  assert( edifff < maxdiff );
  assert( ediffb < maxdiff );

 }

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

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

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

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

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

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

   double u=10.,phi=0.1;
   double z=13.;
   SurfacePtr psrf(new SurfXYPlane(u,phi));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfXYPlane::IV) = 0.1;
   vec(SurfXYPlane::IZ) = 15.;
   vec(SurfXYPlane::IDVDU) = 0.1;
   vec(SurfXYPlane::IDZDU) = 2.;
   vec(SurfXYPlane::IQP) = 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(15.0);
   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 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 = 
     PropXYZ(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);
 }
   
 //********************************************************************

  ObjTable::register_type<PropXYZ>();

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