// PropCylXY_t.cc

// Test PropCylXY.

#include "PropCylXY.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,
  IZ     = SurfXYPlane::IZ,
  IDVDU  = SurfXYPlane::IDVDU,
  IDZDU  = SurfXYPlane::IDZDU,
  IQP_XY = SurfXYPlane::IQP,

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


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

PropStat 
vec_propcylxy( 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 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);                   // z
 double c3 = vec(IALF);                 // alpha
 double c4 = vec(ITLM);                 // tan(lambda)
 double c5 = vec(IQPT);                 // q/pt


 // Check destination is a XYPlane.
 assert( srf.get_pure_type() == SurfXYPlane::get_static_type() );
 if ( srf.get_pure_type( ) != SurfXYPlane::get_static_type() )
   return pstat;
 const SurfXYPlane& sxyp2 = (const SurfXYPlane&) srf;
 
 // Fetch the u and phi of the plane.
 int iphi  = SurfXYPlane::NORMPHI;
 int idist = SurfXYPlane::DISTNORM;

 double phi_n = sxyp2.get_parameter(iphi);
 double   u_n = sxyp2.get_parameter(idist);

 // rotate coordinate system on phi_n

 c1 -= phi_n;
 if(c1 < 0.) c1 += TWOPI;

 double cos_c1 = cos(c1);

 double u = Rcyl*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);

 // check if du == 0 ( that is track moves parallel to the destination plane )
 // du = pt*cos_dir 
 if(cos_dir/c5 == 0.) return pstat;

 double tan_dir = sin_dir/cos_dir;

 double b1 = Rcyl*sin_c1;
 double b2 = c2;
 double b3 = tan_dir;
 double b4 = c4/cos_dir;
 double b5 = c5/c4_hat;
 
 int sign_du;
 if(cos_dir > 0) sign_du =  1;
 if(cos_dir < 0) sign_du = -1;

 double b3_hat = sqrt(1 + b3*b3);
 double b34_hat = sqrt(1 + b3*b3 + b4*b4);
 double b3_hat2 = b3_hat*b3_hat;
 double b34_hat2 = b34_hat*b34_hat;
 double r = 1/(b5*B)*b3_hat/b34_hat;
 double cosphi =         -b3*sign_du/b3_hat;
 double sinphi =             sign_du/b3_hat;
 double rsinphi =  1./(b5*B)*sign_du/b34_hat;
 double rcosphi = -b3/(b5*B)*sign_du/b34_hat;

 double du = u_n - u;
 double norm = du/r - cosphi;

 // cyl-xy propagation failed : noway to the new plane
 if(abs(norm)>1.) return pstat;

 double cat = sqrt(1.-norm*norm);
 int sign_dphi;
 switch (dir) {
  case Propagator::NEAREST:
   cerr << 
   "PropCylXY::_vec_prop: Propagation in NEAREST direction is undefined" 
   << endl;
   abort();
  break;
  case Propagator::FORWARD:
   if(b5>0.) sign_dphi =  1;
   if(b5<0.) sign_dphi = -1;
  break;
  case Propagator::BACKWARD:
   if(b5>0.) sign_dphi = -1;
   if(b5<0.) sign_dphi =  1;
  break;
  default:
   cerr << "PropCylXY::_vec_prop: Unknown direction." << endl;
   abort();
 }

 int sign_cat;
 if(du*sign_dphi*b5>0.) sign_cat =  1;
 if(du*sign_dphi*b5<0.) sign_cat = -1;
 if(du == 0.){
  if(sinphi >=0. ) sign_cat =  1;
  if(sinphi < 0. ) sign_cat = -1;
 }

 double sin_dphi =  norm*sinphi + cat*cosphi*sign_cat;
 double cos_dphi = -norm*cosphi + cat*sinphi*sign_cat;

 int sign_sindphi;
 if(sin_dphi> 0.) sign_sindphi =  1;
 if(sin_dphi< 0.) sign_sindphi = -1;
 if(sin_dphi == 0.) sign_sindphi = sign_dphi;

 double dphi = PI*(sign_dphi - sign_sindphi) + sign_sindphi*acos(cos_dphi);

  // check if dun == 0 ( that is track moves parallel to the destination plane)
 double du_n_du = cos_dphi - b3*sin_dphi;
 if(du_n_du==0.) return pstat;

 double a_hat_dphi = 1./du_n_du;
 double a_hat_dphi2 = a_hat_dphi*a_hat_dphi;
 double c_hat_dphi =     sin_dphi + b3*cos_dphi ;

 double b1_n = b1 + rsinphi*(1-cos_dphi) - rcosphi*sin_dphi;
 double b2_n = b2 + b4/(b5*B)*sign_du/b34_hat*dphi;
 double b3_n = c_hat_dphi*a_hat_dphi;
 double b4_n =         b4*a_hat_dphi;
 double b5_n = b5;

 int sign_dun;
 if(du_n_du*sign_du > 0) sign_dun =  1;
 if(du_n_du*sign_du < 0) sign_dun = -1;

 vec(IV)     = b1_n;
 vec(IZ)     = b2_n;
 vec(IDVDU)  = b3_n;
 vec(IDZDU)  = b4_n;
 vec(IQP_XY) = b5_n;

 // 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: ;
 } 

 // Update trv
 trv.set_surface(SurfacePtr(srf.new_pure_surface()));
 trv.set_vector(vec);
 // set new direction of the track
 if(sign_dun ==  1) trv.set_forward();
 if(sign_dun == -1) trv.set_backward();

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

// du_dc

 double du_dc1 = -Rcyl*sin_c1;

 // db1_dc

 double db1_dc1 = Rcyl*cos_c1;

 // db2_dc

 double db2_dc2 = 1.;

 // db3_dc

 double db3_dc1 = 1./(cos_dir*cos_dir);
 double db3_dc3 = 1./(cos_dir*cos_dir);

 // db4_dc

 double db4_dc1 = b4*tan_dir;
 double db4_dc3 = b4*tan_dir;
 double db4_dc4 = 1/cos_dir;

 // db5_dc

 double db5_dc4 = -c4*c5/(c4_hat*c4_hat2);
 double db5_dc5 = 1./c4_hat; 


 // dr_db

 double dr_db3 = r*b3*b4*b4/(b3_hat2*b34_hat2);
 double dr_db4 = -r*b4/b34_hat2;
 double dr_db5 = -r/b5;

 // dcosphi_db

 double dcosphi_db3 = - sign_du/b3_hat - cosphi*b3/b3_hat2;

 // dsinphi_db
 
 double dsinphi_db3 = - sinphi*b3/b3_hat2;
 
 // dcat_db

 double dcat_db3 = norm/cat*(du/(r*r)*dr_db3 + dcosphi_db3 );
 double dcat_db4 = norm/cat* du/(r*r)*dr_db4;
 double dcat_db5 = norm/cat* du/(r*r)*dr_db5;
 double dcat_du  = norm/(cat*r);

 // dnorm_db

 double dnorm_db3 = - du/(r*r)*dr_db3 - dcosphi_db3;
 double dnorm_db4 = - du/(r*r)*dr_db4;
 double dnorm_db5 = - du/(r*r)*dr_db5;
 double dnorm_du  = - 1./r;

 // dcos_dphi_db

 double dcos_dphi_db3 = - cosphi*dnorm_db3 - norm*dcosphi_db3 +
                         sign_cat*(sinphi*dcat_db3 + cat*dsinphi_db3);
 double dcos_dphi_db4 = - cosphi*dnorm_db4 + sign_cat*sinphi*dcat_db4;
 double dcos_dphi_db5 = - cosphi*dnorm_db5 + sign_cat*sinphi*dcat_db5;
 double dcos_dphi_du  = - cosphi*dnorm_du  + sign_cat*sinphi*dcat_du;

 // dsin_dphi_db

 double dsin_dphi_db3 = sinphi*dnorm_db3 + norm*dsinphi_db3 +
                        sign_cat*(cosphi*dcat_db3 + cat*dcosphi_db3);
 double dsin_dphi_db4 = sinphi*dnorm_db4 + sign_cat*cosphi*dcat_db4; 
 double dsin_dphi_db5 = sinphi*dnorm_db5 + sign_cat*cosphi*dcat_db5;
 double dsin_dphi_du  = sinphi*dnorm_du  + sign_cat*cosphi*dcat_du;

 // ddphi_db

 double ddphi_db3;
 double ddphi_db4;
 double ddphi_db5;
 double ddphi_du;
 if(abs(sin_dphi)>0.5)
 {
  ddphi_db3 = - dcos_dphi_db3/sin_dphi;
  ddphi_db4 = - dcos_dphi_db4/sin_dphi;
  ddphi_db5 = - dcos_dphi_db5/sin_dphi;
  ddphi_du  = - dcos_dphi_du /sin_dphi;
 }
 else
 {
  ddphi_db3 = dsin_dphi_db3/cos_dphi;
  ddphi_db4 = dsin_dphi_db4/cos_dphi;
  ddphi_db5 = dsin_dphi_db5/cos_dphi;
  ddphi_du  = dsin_dphi_du /cos_dphi;
 }

 // da_hat_dphi_db

 double da_hat_dphi_db3 = - a_hat_dphi2*
                             (dcos_dphi_db3 - sin_dphi - b3*dsin_dphi_db3); 
 double da_hat_dphi_db4 = - a_hat_dphi2*(dcos_dphi_db4 - b3*dsin_dphi_db4);
 double da_hat_dphi_db5 = - a_hat_dphi2*(dcos_dphi_db5 - b3*dsin_dphi_db5);
 double da_hat_dphi_du  = - a_hat_dphi2*(dcos_dphi_du  - b3*dsin_dphi_du );

 // dc_hat_dphi_db

 double dc_hat_dphi_db3 = b3*dcos_dphi_db3 + dsin_dphi_db3 + cos_dphi;
 double dc_hat_dphi_db4 = b3*dcos_dphi_db4 + dsin_dphi_db4;
 double dc_hat_dphi_db5 = b3*dcos_dphi_db5 + dsin_dphi_db5;
 double dc_hat_dphi_du  = b3*dcos_dphi_du  + dsin_dphi_du ;

 // db1_n_db

 double db1_n_db1 = 1;
 double db1_n_db3 = (dr_db3*sinphi+r*dsinphi_db3)*(1-cos_dphi) 
                     - rsinphi*dcos_dphi_db3
                     - dr_db3*cosphi*sin_dphi - r*dcosphi_db3*sin_dphi
                     - rcosphi*dsin_dphi_db3;
 double db1_n_db4 = dr_db4*sinphi*(1-cos_dphi) - rsinphi*dcos_dphi_db4 
                     - dr_db4*cosphi*sin_dphi - rcosphi*dsin_dphi_db4;
 double db1_n_db5 = dr_db5*sinphi*(1-cos_dphi) - rsinphi*dcos_dphi_db5 
                     - dr_db5*cosphi*sin_dphi - rcosphi*dsin_dphi_db5;
 double db1_n_du  = - rsinphi*dcos_dphi_du - rcosphi*dsin_dphi_du;

 // db2_n_db

 double db2_n_db2 = 1.;
 double db2_n_db3 = 1./(b5*B)*b4*sign_du/b34_hat*
                    ( - dphi*b3/b34_hat2 + ddphi_db3);
 double db2_n_db4 = 1./(b5*B)*sign_du/b34_hat*
                    ( - dphi*b4*b4/b34_hat2 + b4*ddphi_db4 + dphi );
 double db2_n_db5 = 1./(b5*B)*b4*sign_du/b34_hat*( ddphi_db5 - dphi/b5);
 double db2_n_du  = 1./(b5*B)*b4*sign_du/b34_hat*ddphi_du;

 // db3_n_db

 double db3_n_db3 = a_hat_dphi*dc_hat_dphi_db3 + da_hat_dphi_db3*c_hat_dphi;
 double db3_n_db4 = a_hat_dphi*dc_hat_dphi_db4 + da_hat_dphi_db4*c_hat_dphi;
 double db3_n_db5 = a_hat_dphi*dc_hat_dphi_db5 + da_hat_dphi_db5*c_hat_dphi;
 double db3_n_du  = a_hat_dphi*dc_hat_dphi_du  + da_hat_dphi_du *c_hat_dphi;

 // db4_n_db

 double db4_n_db3 = b4*da_hat_dphi_db3;
 double db4_n_db4 = b4*da_hat_dphi_db4 + a_hat_dphi;
 double db4_n_db5 = b4*da_hat_dphi_db5;
 double db4_n_du  = b4*da_hat_dphi_du;

 // db5_n_db

 // double db5_n_db5 = 1.;

 // db1_n_dc

 double db1_n_dc1 = db1_n_du * du_dc1 + db1_n_db1*db1_dc1 + 
                    db1_n_db3*db3_dc1 + db1_n_db4*db4_dc1;
 double db1_n_dc2 = 0.;
 double db1_n_dc3 = db1_n_db3*db3_dc3 + db1_n_db4*db4_dc3;
 double db1_n_dc4 = db1_n_db4*db4_dc4 + db1_n_db5*db5_dc4;
 double db1_n_dc5 = db1_n_db5*db5_dc5;
 

 // db2_n_dc

 double db2_n_dc1 = db2_n_du * du_dc1 + db2_n_db3*db3_dc1 + db2_n_db4*db4_dc1;
 double db2_n_dc2 = db2_n_db2 * db2_dc2;
 double db2_n_dc3 = db2_n_db3*db3_dc3 + db2_n_db4*db4_dc3;
 double db2_n_dc4 = db2_n_db4*db4_dc4 + db2_n_db5*db5_dc4;
 double db2_n_dc5 = db2_n_db5*db5_dc5;

 // db3_n_dc

 double db3_n_dc1 = db3_n_du * du_dc1 + db3_n_db3*db3_dc1 + db3_n_db4*db4_dc1;
 double db3_n_dc2 = 0.;
 double db3_n_dc3 = db3_n_db3*db3_dc3 + db3_n_db4*db4_dc3;
 double db3_n_dc4 = db3_n_db4*db4_dc4 + db3_n_db5*db5_dc4;
 double db3_n_dc5 = db3_n_db5*db5_dc5;

 // db4_n_dc

 double db4_n_dc1 = db4_n_du * du_dc1 + db4_n_db3*db3_dc1 + db4_n_db4*db4_dc1;
 double db4_n_dc2 = 0.;
 double db4_n_dc3 = db4_n_db3*db3_dc3 + db4_n_db4*db4_dc3;
 double db4_n_dc4 = db4_n_db4*db4_dc4 + db4_n_db5*db5_dc4;
 double db4_n_dc5 = db4_n_db5*db5_dc5;

 // db5_n_dc

 double db5_n_dc1 = 0.;
 double db5_n_dc2 = 0.;
 double db5_n_dc3 = 0.;
 double db5_n_dc4 = db5_dc4;
 double db5_n_dc5 = db5_dc5;

 TrackDerivative& deriv = *pderiv;

 deriv(IV,IPHI)       =db1_n_dc1;
 deriv(IV,IZ_CYL)     =db1_n_dc2;
 deriv(IV,IALF)       =db1_n_dc3;
 deriv(IV,ITLM)       =db1_n_dc4;
 deriv(IV,IQPT)       =db1_n_dc5;
 deriv(IZ,IPHI)       =db2_n_dc1;
 deriv(IZ,IZ_CYL)     =db2_n_dc2;
 deriv(IZ,IALF)       =db2_n_dc3;
 deriv(IZ,ITLM)       =db2_n_dc4;
 deriv(IZ,IQPT)       =db2_n_dc5;
 deriv(IDVDU,IPHI)    =db3_n_dc1;
 deriv(IDVDU,IZ_CYL)  =db3_n_dc2;
 deriv(IDVDU,IALF)    =db3_n_dc3;
 deriv(IDVDU,ITLM)    =db3_n_dc4;
 deriv(IDVDU,IQPT)    =db3_n_dc5;
 deriv(IDZDU,IPHI)    =db4_n_dc1;
 deriv(IDZDU,IZ_CYL)  =db4_n_dc2;
 deriv(IDZDU,IALF)    =db4_n_dc3;
 deriv(IDZDU,ITLM)    =db4_n_dc4;
 deriv(IDZDU,IQPT)    =db4_n_dc5;
 deriv(IQP_XY,IPHI)   =db5_n_dc1;
 deriv(IQP_XY,IZ_CYL) =db5_n_dc2;
 deriv(IQP_XY,IALF)   =db5_n_dc3;
 deriv(IQP_XY,ITLM)   =db5_n_dc4;
 deriv(IQP_XY,IQPT)   =db5_n_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 = "PropCylXY (I): ";
  string error_prefix = "PropCylXY test (E): ";

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

 PropCylXY propcylxy(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,     0.     };
 double qpt[]    ={ 0.01,  -0.01,   0.01,  -0.01,   -0.01,    0.01   };

 double u2b[]    ={ 6.,     6.,     6.,     6.,      6.,      6.     };
 double phic2b[] ={ PI/6.,  PI/5.,  PI/7.,  5/3*PI,  4/3*PI,  7/4*PI };
 double u2[]     ={ 15.,    15.,    15.,    15.,     15.,     15.    };
 double phic2[]  ={ PI/6.,  PI/5.,  PI/7.,  5/3*PI,  4/3*PI,  7/4*PI };

 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.);
  SurfXYPlane  sxyp2(u2[i],phic2[i]);
  SurfXYPlane  sxyp2b(u2b[i],phic2b[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 = propcylxy.vec_dir_prop(trv2f,sxyp2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "\n  forward: " << trv2f << endl;

  VTrack trv2f_my = trv1;
  pstat = vec_propcylxy(BFIELD,trv2f_my,sxyp2,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 = propcylxy.vec_dir_prop(trv2b,sxyp2b,Propagator::BACKWARD_MOVE);
  assert( pstat.backward() );  
  cout << "\n  backward: " << trv2b << endl;

  VTrack trv2b_my = trv1;
  pstat = vec_propcylxy(BFIELD,trv2b_my,sxyp2b,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.004, -0.004,  0.004, -0.004,  0.004, -0.004,  0.004 };
 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.);
  SurfXYPlane  sxyp2(u2[i],phic2[i]);
  SurfXYPlane  sxyp2b(u2b[i],phic2b[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 = propcylxy.err_dir_prop(trv2f,sxyp2,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "\n  forward: " << trv2f << endl;

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

  ETrack trv2b_my = trv1;
  pstat = vec_propcylxy(BFIELD,trv2b_my,sxyp2b,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 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;
 {
   PropCylXY prop0(ObjDoublePtr(new ObjDouble(0.0)));
   cout << prop0 << endl;
   assert( prop0.get_bfield() == 0. );

   double u=10.,phi=0.1;
   SurfacePtr psrf(new SurfCylinder(13.0));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfCylinder::IPHI) = phi+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 SurfXYPlane(u,phi));

   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 SurfXYPlane(13.0,phi+0.1);
   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 = 
     PropCylXY(ObjDoublePtr(new ObjDouble(0.0))).err_prop(tre,SurfXYPlane(15.,0.1));
   assert(pstat.success());
   cout<<tre<<endl;
   /*
   assert( fabs(tre.get_error(SurfXYPlane::IQP,SurfXYPlane::IQP)-1) < 1e-10);
   assert( fabs(tre.get_error(SurfXYPlane::IQP,SurfXYPlane::IV)) < 1e-10);
   assert( fabs(tre.get_error(SurfXYPlane::IQP,SurfXYPlane::IZ)) < 1e-10);
   assert( fabs(tre.get_error(SurfXYPlane::IQP,SurfXYPlane::IDVDU)) < 1e-10);
   assert( fabs(tre.get_error(SurfXYPlane::IQP,SurfXYPlane::IDZDU)) < 1e-10);
   */
 }
   
 //********************************************************************

  ObjTable::register_type<PropCylXY>();

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