// PropCylDCA_t.cpp

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

using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::sqrt;
using std::ostringstream;
using std::istringstream;

using namespace trf;

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

// Return true if two ETrack's are close to one another.
// Note tests phrased so that NAN will fail.
namespace {


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) ;


bool about_equal(const ETrack& tre1, const ETrack& tre2) {
  bool equal = true;
  double maxdif = 1.e-6;
  // Check vector.
  TrackVector vdiff =
    tre1.get_surface()->vec_diff( tre1.get_vector(), tre2.get_vector() );
  for (int i=0; i<5; ++i) {
    double adif = abs( vdiff(i) );
    if ( ! (adif < maxdif) ) {
      cout << "vecdif(" << i << ") = " << adif << endl;
      equal = false;
    }
  }
  // Check errors.
  TrackError ediff = tre1.get_error() - tre2.get_error();
  TrackError eavg = tre1.get_error() + tre2.get_error();
  for (int i=0; i<5; ++i) {
    for (int j=0; j<=i; ++j) {
      double adif = abs( ediff(i,j) );
      double afrac = ediff(i,j)/sqrt(eavg(i,i)*eavg(j,j));
      if ( !(adif < maxdif) ) {
        cout << "errdif(" << i << "," << j << ") = " << adif
             << "   frac = " << afrac << endl;
        equal = false;
      }
    }
  }
  return equal;
}

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

int main( ) {

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

  cout << " NEW TEST 3 !!!! " << endl;
  cout << ok_prefix
       << "-------- Testing component PropCylDCA. --------" << 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;
  }

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

  ObjTable::register_type<PropCylDCA>();

  cout << trf_format;
  cout << ok_prefix << "Test constructor." << endl;
  PropCylDCA prop(ObjDoublePtr(new ObjDouble(-2.0)));
  cout << prop << endl;

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

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

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

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

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

  cout << ok_prefix 
       << "Test propagation of a track - without error." << endl;

  PropStat pstat;

  TrackVector tvec;
  tvec(0) =  1.0;                                // phi
  tvec(1) =  2.0;                                // z
  tvec(2) =  3.0;                                // alpha
  tvec(3) =  4.0;                                // tlm
  tvec(4) =  5.0;                                // qpt
  SurfCylinder scyl(10.0);
  VTrack trv( SurfacePtr(scyl.new_pure_surface()),tvec );
  SpacePoint spt = trv.space_point();

  SurfDCA sdca;

  cout << " *** Forward propagation *** " << endl;
  VTrack trv2f = trv;
  cout << " before propagation: trv2f = " << trv2f << endl;
  SpacePoint spt2f = trv2f.space_point();
  cout << " before propagation: spt2f = " << endl;
  cout << spt2f << endl;
  assert( spt2f.rxy() == 10.0 );
  assert( fabs(spt2f.phi()-spt.phi()) < 1.e-4 );
  assert( spt2f.z()   == 2.0 );
  pstat = prop.vec_dir_prop(trv2f,sdca,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  after propagation: trv2f = " << trv2f << endl;
  spt2f = trv2f.space_point();
  cout << "  after propagation: spt2f = " << endl;
  cout << spt2f << endl;
  double phi  = spt2f.phi();
  // 1st way to calculate alpha
  TrackVector vec_dca = trv2f.get_vector();
  double phid = vec_dca(SurfDCA::IPHID);
  double alf  = phid - phi;
  cout << " phi  = " << phi  << endl;
  cout << " phid = " << phid << endl;
  cout << " alf  = " << alf  << endl;
  assert( fabs(fabs(alf)-PI2) < 1.e-4 );
  // 2nd way to calculate alpha
  SpacePath svec = trv2f.space_vector();
  double     dr_ds = svec.v_rxy();
  double r_dphi_ds = svec.v_phi();
  double alfa = atan2( r_dphi_ds, dr_ds );
  cout << " alfa  = " << alfa  << endl;
  assert( fabs(fabs(alfa)-PI2) < 1.e-4 );


  cout << " *** Backward propagation *** " << endl;
  VTrack trv2b = trv;
  cout << " before propagation: trv2b = " << trv2b << endl;
  SpacePoint spt2b = trv2b.space_point();
  cout << " before propagation: spt2b = " << endl;
  cout << spt2b << endl;
  assert( spt2b.rxy() == 10.0 );
  assert( fabs(spt2b.phi()-spt.phi()) < 1.e-4 );
  assert( spt2b.z()   == 2.0 );
  pstat = prop.vec_dir_prop(trv2b,sdca,Propagator::BACKWARD);
  //  assert( pstat.backward() );
  cout << "  after propagation: trv2b = " << trv2b << endl;
             spt2b = trv2b.space_point();
  cout << "  after propagation: spt2b = " << endl;
  cout << spt2b << endl;


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

  cout << ok_prefix 
       << "Test propagation of a track - with error." << endl;

  //  PropStat pstat;

  //  TrackVector tvec;
  //  tvec(0) =  1.0;                                // phi
  //  tvec(1) =  2.0;                                // z
  //  tvec(2) =  3.0;                                // alpha
  //  tvec(3) =  4.0;                                // tlm
  //  tvec(4) =  5.0;                                // qpt
  TrackError err;
  err(0,0) = 2.0;
  err(0,1) = 1.0;
  err(0,2) = 1.0;
  err(0,3) = 1.0;
  err(0,4) = 1.0;
  err(1,1) = 3.0;
  err(1,2) = 1.0;
  err(1,3) = 1.0;
  err(1,4) = 1.0;
  err(2,2) = 4.0;
  err(2,3) = 1.0;
  err(2,4) = 1.0;
  err(3,3) = 5.0;
  err(3,4) = 1.0;
  err(4,4) = 6.0;
  //  SurfCylinder scyl;
  ETrack etrv( SurfacePtr(scyl.new_pure_surface()),tvec,err );
  SpacePoint espt = etrv.space_point();

  //  SurfDCA sdca;

  cout << " *** Forward propagation *** " << endl;
  ETrack etrv2f = etrv;
  cout << " before propagation: etrv2f = " << etrv2f << endl;
  SpacePoint espt2f = etrv2f.space_point();
  cout << " before propagation: espt2f = " << endl;
  cout << espt2f << endl;
  assert( espt2f.rxy() == 10.0 );
  assert( fabs(espt2f.phi()-espt.phi()) < 1.e-4 );
  assert( espt2f.z()   == 2.0 );
  pstat = prop.err_dir_prop(etrv2f,sdca,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  after propagation: etrv2f = " << etrv2f << endl;
             espt2f = etrv2f.space_point();
  cout << "  after propagation: espt2f = " << endl;
  cout << espt2f << endl;


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

  cout << ok_prefix
       << "Test propagation from DCA to Cylinder and back - no error"
       << endl;

  TrackVector tvec_dca;
  tvec_dca(0) =  1.0;                                // r_signed
  tvec_dca(1) =  2.0;                                // z
  tvec_dca(2) =  3.0;                                // phi_direction
  tvec_dca(3) =  4.0;                                // tlm
  tvec_dca(4) =  5.0;                                // qpt

  VTrack trvft( SurfacePtr(sdca.new_pure_surface()),tvec_dca );
  cout << trvft << endl;

  PropDCACyl propf(ObjDoublePtr(new ObjDouble(-2.0)));
  pstat = propf.vec_dir_prop(trvft,scyl,Propagator::FORWARD);
  cout << " pstat.forward()  = " << pstat.forward()  << endl;
  cout << " pstat.backward() = " << pstat.backward() << endl;
  cout << " pstat.same()     = " << pstat.same()     << endl;
  assert( pstat.success() );
  SpacePoint sptft = trvft.space_point();
  assert( sptft.rxy() == 10.0 );
  cout << trvft << endl;

  PropCylDCA propt(ObjDoublePtr(new ObjDouble(-2.0)));
  assert( sdca.get_pure_type() == SurfDCA::get_static_type() );
  pstat = propt.vec_dir_prop(trvft,sdca,Propagator::BACKWARD);
  cout << " pstat.forward()  = " << pstat.forward()  << endl;
  cout << " pstat.backward() = " << pstat.backward() << endl;
  cout << " pstat.same()     = " << pstat.same()     << endl;
  assert( pstat.success() );
  SpacePoint sptt = trvft.space_point();
  cout << trvft << endl;
  SpacePath svecft = trvft.space_vector();
  double alf_ft = atan2( svecft.v_phi(), svecft.v_rxy() );
  assert( fabs(fabs(alf_ft)-PI2) < 1.e-4 );
  assert( fabs(trvft.get_vector(0)-tvec_dca(0)) < 1.e-4 );
  assert( fabs(trvft.get_vector(1)-tvec_dca(1)) < 1.e-4 );
  assert( fabs(trvft.get_vector(2)-tvec_dca(2)) < 1.e-4 );
  assert( fabs(trvft.get_vector(3)-tvec_dca(3)) < 1.e-4 );
  assert( fabs(trvft.get_vector(4)-tvec_dca(4)) < 1.e-4 );

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

  cout << ok_prefix 
       << "Test propagation from DCA to Cylinder and back - w/ error" 
       << endl;

  TrackError err_dca;
  err_dca(0,0) =  100.0;
  err_dca(0,1) =  2.0;
  err_dca(0,2) =  3.0;
  err_dca(0,3) =  4.0;
  err_dca(0,4) =  5.0;
  err_dca(1,1) =  600.0;
  err_dca(1,2) =  7.0;
  err_dca(1,3) =  8.0;
  err_dca(1,4) =  9.0;
  err_dca(2,2) = 1000.0;
  err_dca(2,3) = 11.0;
  err_dca(2,4) = 12.0;
  err_dca(3,3) = 1300.0;
  err_dca(3,4) = 14.0;
  err_dca(4,4) = 1500.0;

  ETrack tre( SurfacePtr(sdca.new_pure_surface()),tvec_dca,err_dca );
  cout << tre << endl;

  PropDCACyl prop1(ObjDoublePtr(new ObjDouble(-2.0)));
  pstat = prop1.err_dir_prop(tre,scyl,Propagator::FORWARD);
  assert( pstat.success() );
  SpacePoint spt1 = tre.space_point();
  assert( spt1.rxy() == 10.0 );
  cout << tre << endl;

  PropCylDCA prop2(ObjDoublePtr(new ObjDouble(-2.0)));
  pstat = prop2.err_dir_prop(tre,sdca,Propagator::BACKWARD);
  assert( pstat.success() );
  SpacePoint spt2 = tre.space_point();
  //  assert( spt2.rxy() == 1.0 );
  cout << "spt2.rxy() = " << spt2.rxy() << endl; 
  cout << tre << endl;

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

  // DLA test.
  cout << ok_prefix << "DLA test." << endl;
  {
    double bfield = 2.0;
    PropCylDCA prop1(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop2(ObjDoublePtr(new ObjDouble(-bfield)));
    double r0 = 1.0;
    SurfacePtr pscyl( new SurfCylinder(r0) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfCylinder::IPHI) = 0.10;
    vec(SurfCylinder::IZ)   = 12.00;
    vec(SurfCylinder::IALF) = 0.03;
    vec(SurfCylinder::ITLM) = 1.04;
    vec(SurfCylinder::IQPT) = 0.05;
    TrackError err;
    err(SurfCylinder::IPHI,SurfCylinder::IPHI) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IZ  ) = 0.0200;
    err(SurfCylinder::IALF,SurfCylinder::IALF) = 0.0003;
    err(SurfCylinder::ITLM,SurfCylinder::ITLM) = 0.0400;
    err(SurfCylinder::IQPT,SurfCylinder::IQPT) = 0.000005;
    err(SurfCylinder::IPHI,SurfCylinder::IZ  ) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IALF) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IALF) = 0.0005;
    err(SurfCylinder::IPHI,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IZ,  SurfCylinder::ITLM) = 0.0100;
    err(SurfCylinder::IALF,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IQPT) = 0.00001;
    err(SurfCylinder::IZ,  SurfCylinder::IQPT) = 0.0002;
    err(SurfCylinder::IALF,SurfCylinder::IQPT) = 0.00003;
    err(SurfCylinder::ITLM,SurfCylinder::IQPT) = 0.0003;
    ETrack tre1(pscyl,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to dca
    prop1.err_prop(tre2,*psdca);
    cout << tre2 << endl;
    // Propagate back to cylinder.
    prop2.err_dir_prop(tre2,*pscyl,Propagator::FORWARD);
    cout << tre2 << endl;
    bool stat = about_equal(tre1,tre2);
    if ( ! stat ) {
      {
        ETrack tre(tre1);
        prop1.err_dir_prop(tre,*psdca,Propagator::FORWARD);
        cout << "Forward DCA->cyl prop gives: " << endl;
        cout << tre << endl;
      }
      {
        ETrack tre(tre1);
        prop1.err_dir_prop(tre,*psdca,Propagator::BACKWARD);
        cout << "Backward DCA->cyl prop gives: " << endl;
        cout << tre << endl;
      }
      {
        ETrack tre(tre1);
        prop1.err_dir_prop(tre,*psdca,Propagator::NEAREST);
        cout << "Nearest DCA->cyl prop gives: " << endl;
        cout << tre << endl;
      }
      assert( stat );
    }
  }

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

  cout << endl;
  cout << ok_prefix << "DLA test 2." << endl;
  {
    double bfield = 2.0;
    PropCyl prop_cyl_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    PropCylDCA prop_cyl_dca(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop_dca_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    double r1 = 1.0;
    double r2 = 20.0;
    SurfacePtr pscyl1( new SurfCylinder(r1) );
    SurfacePtr pscyl2( new SurfCylinder(r2) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfCylinder::IPHI) = 0.10;
    vec(SurfCylinder::IZ)   = 12.00;
    vec(SurfCylinder::IALF) = 0.03;
    vec(SurfCylinder::ITLM) = 1.04;
    vec(SurfCylinder::IQPT) = 0.05;
    TrackError err;
    // Diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IPHI) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IZ  ) = 0.0200;
    err(SurfCylinder::IALF,SurfCylinder::IALF) = 0.0003;
    err(SurfCylinder::ITLM,SurfCylinder::ITLM) = 0.0400;
    err(SurfCylinder::IQPT,SurfCylinder::IQPT) = 0.000005;
    // Off-diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IZ  ) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IALF) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IALF) = 0.0005;
    err(SurfCylinder::IPHI,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IZ,  SurfCylinder::ITLM) = 0.0100;
    err(SurfCylinder::IALF,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IQPT) = 0.00001;
    err(SurfCylinder::IZ,  SurfCylinder::IQPT) = 0.0002;
    err(SurfCylinder::IALF,SurfCylinder::IQPT) = 0.00003;
    err(SurfCylinder::ITLM,SurfCylinder::IQPT) = 0.0003;
    ETrack tre1(pscyl1,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to outer cylinder.
    cout << "Propagate from cyl1 to cyl2" << endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre2,*pscyl2,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cyl
    ETrack tre22=tre2;
     cout << "Propagate from cyl2 to cyl1"<< endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre22,*pscyl1,Propagator::BACKWARD);
    assert( pstat.success() );
    cout<<tre22<<endl;
    // Propagate to DCA
    cout << "Propagate from cyl2 to DCA" << endl;
    pstat = prop_cyl_dca.err_dir_prop(tre2,*psdca,Propagator::BACKWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cylinder.
    cout << "Propagate from DCA to cyl1" << endl;
    pstat = prop_dca_cyl.err_dir_prop(tre2,*pscyl1,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    assert( about_equal(tre1,tre2) );
  }
  //******************************************************************

  cout << endl;
  cout << ok_prefix << "DLA test 3: alf = curv = 0." << endl;
  {
    double bfield = 2.0;
    PropCyl prop_cyl_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    PropCylDCA prop_cyl_dca(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop_dca_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    double r1 = 1.0;
    double r2 = 20.0;
    SurfacePtr pscyl1( new SurfCylinder(r1) );
    SurfacePtr pscyl2( new SurfCylinder(r2) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfCylinder::IPHI) = 0.10;
    vec(SurfCylinder::IZ)   = 12.00;
    vec(SurfCylinder::IALF) = 0.0;
    vec(SurfCylinder::ITLM) = 1.04;
    vec(SurfCylinder::IQPT) = 0.0;
    TrackError err;
    // Diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IPHI) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IZ  ) = 0.0200;
    err(SurfCylinder::IALF,SurfCylinder::IALF) = 0.0003;
    err(SurfCylinder::ITLM,SurfCylinder::ITLM) = 0.0400;
    err(SurfCylinder::IQPT,SurfCylinder::IQPT) = 0.000005;
    // Off-diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IZ  ) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IALF) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IALF) = 0.0005;
    err(SurfCylinder::IPHI,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IZ,  SurfCylinder::ITLM) = 0.0100;
    err(SurfCylinder::IALF,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IQPT) = 0.00001;
    err(SurfCylinder::IZ,  SurfCylinder::IQPT) = 0.0002;
    err(SurfCylinder::IALF,SurfCylinder::IQPT) = 0.00003;
    err(SurfCylinder::ITLM,SurfCylinder::IQPT) = 0.0003;
    ETrack tre1(pscyl1,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to outer cylinder.
    cout << "Propagate from cyl1 to cyl2" << endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre2,*pscyl2,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cyl
    ETrack tre22=tre2;
     cout << "Propagate from cyl2 to cyl1"<< endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre22,*pscyl1,Propagator::BACKWARD);
    assert( pstat.success() );
    cout<<tre22<<endl;
    // Propagate to DCA
    cout << "Propagate from cyl2 to DCA" << endl;
    pstat = prop_cyl_dca.err_dir_prop(tre2,*psdca,Propagator::BACKWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cylinder.
    cout << "Propagate from DCA to cyl1" << endl;
    check_derivatives(prop_dca_cyl,tre2,*pscyl1);
    pstat = prop_dca_cyl.err_dir_prop(tre2,*pscyl1,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    assert( about_equal(tre1,tre2) );
  }
  //******************************************************************

  cout << endl;
  cout << ok_prefix << "New test 4: r = 0 crv!=0" << endl;
  {
    double bfield = 2.0;
    PropCyl prop_cyl_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    PropCylDCA prop_cyl_dca(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop_dca_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    double r1 = 1.0;
    double r2 = 20.0;
    SurfacePtr pscyl1( new SurfCylinder(r1) );
    SurfacePtr pscyl2( new SurfCylinder(r2) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfDCA::IRSIGNED) = 0.0;
    vec(SurfDCA::IZ)   = 12.00;
    vec(SurfDCA::IPHID) = 0.3;
    vec(SurfDCA::ITLM) = 1.04;
    vec(SurfDCA::IQPT) = -0.001;
    TrackError err;
    // Diagonal elements.
    err(SurfDCA::IRSIGNED,SurfDCA::IRSIGNED) = 0.0001;
    err(SurfDCA::IZ,  SurfDCA::IZ  ) = 0.0200;
    err(SurfDCA::IPHID,SurfDCA::IPHID) = 0.0003;
    err(SurfDCA::ITLM,SurfDCA::ITLM) = 0.0400;
    err(SurfDCA::IQPT,SurfDCA::IQPT) = 0.000005;
    // Off-diagonal elements.
    err(SurfDCA::IRSIGNED,SurfDCA::IZ  ) = 0.0010;
    err(SurfDCA::IRSIGNED,SurfDCA::IPHID) = 0.0001;
    err(SurfDCA::IZ,  SurfDCA::IPHID) = 0.0005;
    err(SurfDCA::IRSIGNED,SurfDCA::ITLM) = 0.0010;
    err(SurfDCA::IZ,  SurfDCA::ITLM) = 0.0100;
    err(SurfDCA::IPHID,SurfDCA::ITLM) = 0.0010;
    err(SurfDCA::IRSIGNED,SurfDCA::IQPT) = 0.00001;
    err(SurfDCA::IZ,  SurfDCA::IQPT) = 0.0002;
    err(SurfDCA::IPHID,SurfDCA::IQPT) = 0.00003;
    err(SurfDCA::ITLM,SurfDCA::IQPT) = 0.0003;
    ETrack tre1(psdca,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to outer cylinder.
    cout << "Propagate from dca to cyl2" << endl;
    pstat = prop_dca_cyl.err_dir_prop(tre2,*pscyl2,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to dca
    ETrack tre22=tre2;
     cout << "Propagate from cyl2 to dca"<< endl;
    pstat = prop_cyl_dca.err_dir_prop(tre22,*psdca,Propagator::BACKWARD);
    assert( pstat.success() );
    cout<<tre22<<endl;
   
    assert( about_equal(tre1,tre22) );
  }
  //******************************************************************

  cout << endl;
  cout << ok_prefix << "SL test 3." << endl;
  {
    double bfield = 2.0;
    PropCyl prop_cyl_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    PropCylDCA prop_cyl_dca(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop_dca_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    double r1 = 1.0;
    double r2 = 20.0;
    SurfacePtr pscyl1( new SurfCylinder(r1) );
    SurfacePtr pscyl2( new SurfCylinder(r2) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfCylinder::IPHI) = 0.10;
    vec(SurfCylinder::IZ)   = 12.00;
    vec(SurfCylinder::IALF) = -0.03;
    vec(SurfCylinder::ITLM) = 1.04;
    vec(SurfCylinder::IQPT) = 0.05;
    TrackError err;
    // Diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IPHI) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IZ  ) = 0.0200;
    err(SurfCylinder::IALF,SurfCylinder::IALF) = 0.0003;
    err(SurfCylinder::ITLM,SurfCylinder::ITLM) = 0.0400;
    err(SurfCylinder::IQPT,SurfCylinder::IQPT) = 0.000005;
    // Off-diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IZ  ) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IALF) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IALF) = 0.0005;
    err(SurfCylinder::IPHI,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IZ,  SurfCylinder::ITLM) = 0.0100;
    err(SurfCylinder::IALF,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IQPT) = 0.00001;
    err(SurfCylinder::IZ,  SurfCylinder::IQPT) = 0.0002;
    err(SurfCylinder::IALF,SurfCylinder::IQPT) = 0.00003;
    err(SurfCylinder::ITLM,SurfCylinder::IQPT) = 0.0003;
    ETrack tre1(pscyl1,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to outer cylinder.
    cout << "Propagate from cyl1 to cyl2" << endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre2,*pscyl2,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cyl
    ETrack tre22=tre2;
     cout << "Propagate from cyl2 to cyl1"<< endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre22,*pscyl1,Propagator::BACKWARD);
    assert( pstat.success() );
    cout<<tre22<<endl;
    // Propagate to DCA
    cout << "Propagate from cyl2 to DCA" << endl;
    pstat = prop_cyl_dca.err_dir_prop(tre2,*psdca,Propagator::BACKWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cylinder.
    cout << "Propagate from DCA to cyl1" << endl;
    pstat = prop_dca_cyl.err_dir_prop(tre2,*pscyl1,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    assert( about_equal(tre1,tre2) );
  }
  //******************************************************************

  cout << endl;
  cout << ok_prefix << "SL test 4." << endl;
  {
    double bfield = 2.0;
    PropCyl prop_cyl_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    PropCylDCA prop_cyl_dca(ObjDoublePtr(new ObjDouble(-bfield)));
    PropDCACyl prop_dca_cyl(ObjDoublePtr(new ObjDouble(-bfield)));
    double r1 = 1.0;
    double r2 = 20.0;
    SurfacePtr pscyl1( new SurfCylinder(r1) );
    SurfacePtr pscyl2( new SurfCylinder(r2) );
    SurfacePtr psdca( new SurfDCA );
    TrackVector vec;
    vec(SurfCylinder::IPHI) = 0.10;
    vec(SurfCylinder::IZ)   = 12.00;
    vec(SurfCylinder::IALF) = 0.03;
    vec(SurfCylinder::ITLM) = 1.04;
    vec(SurfCylinder::IQPT) = -0.05;
    TrackError err;
    // Diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IPHI) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IZ  ) = 0.0200;
    err(SurfCylinder::IALF,SurfCylinder::IALF) = 0.0003;
    err(SurfCylinder::ITLM,SurfCylinder::ITLM) = 0.0400;
    err(SurfCylinder::IQPT,SurfCylinder::IQPT) = 0.000005;
    // Off-diagonal elements.
    err(SurfCylinder::IPHI,SurfCylinder::IZ  ) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IALF) = 0.0001;
    err(SurfCylinder::IZ,  SurfCylinder::IALF) = 0.0005;
    err(SurfCylinder::IPHI,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IZ,  SurfCylinder::ITLM) = 0.0100;
    err(SurfCylinder::IALF,SurfCylinder::ITLM) = 0.0010;
    err(SurfCylinder::IPHI,SurfCylinder::IQPT) = 0.00001;
    err(SurfCylinder::IZ,  SurfCylinder::IQPT) = 0.0002;
    err(SurfCylinder::IALF,SurfCylinder::IQPT) = 0.00003;
    err(SurfCylinder::ITLM,SurfCylinder::IQPT) = 0.0003;
    ETrack tre1(pscyl1,vec,err);
    ETrack tre2(tre1);
    cout << tre2 << endl;
    // Propagate to outer cylinder.
    cout << "Propagate from cyl1 to cyl2" << endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre2,*pscyl2,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cyl
    ETrack tre22=tre2;
     cout << "Propagate from cyl2 to cyl1"<< endl;
    pstat = prop_cyl_cyl.err_dir_prop(tre22,*pscyl1,Propagator::BACKWARD);
    assert( pstat.success() );
    cout<<tre22<<endl;
    // Propagate to DCA
    cout << "Propagate from cyl2 to DCA" << endl;
    pstat = prop_cyl_dca.err_dir_prop(tre2,*psdca,Propagator::BACKWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    // Propagate back to inner cylinder.
    cout << "Propagate from DCA to cyl1" << endl;
    pstat = prop_dca_cyl.err_dir_prop(tre2,*pscyl1,Propagator::FORWARD);
    assert( pstat.success() );
    cout << tre2 << endl;
    assert( about_equal(tre1,tre2) );
  }

  // Now test propagation to non (0,0) DCA.
  {
    SurfacePtr psrf(new SurfCylinder(10.));
    tvec(0) =  1.0;                                // phi
    tvec(1) =  2.0;                                // z
    tvec(2) =  0.1;                                // alpha
    tvec(3) =  4.0;                                // tlm
    tvec(4) =  0.2;                                // qpt
    SurfCylinder scyl(10.0);
    VTrack trv0( psrf,tvec );
    VTrack trv(trv0);

    PropCyl propcyl(ObjDoublePtr(new ObjDouble(-2.)));
    PropStat pstat = propcyl.vec_prop(trv,SurfCylinder(1.));
    assert( pstat.success() );

    double xv = trv.space_point().x();
    double yv = trv.space_point().y();

    trv=trv0;

    pstat = prop.vec_prop(trv,SurfDCA(xv,yv));
    assert( pstat.success() );

    cout<< trv <<'\n';
    assert( fabs(trv.get_vector(SurfDCA::IRSIGNED))< 1e-10 );

    check_derivatives(prop,trv0,SurfDCA(xv,yv));

    // check negative curvature

    tvec(4) =  -0.002;     
    trv0.set_vector(tvec);
    trv0.set_forward();
    
    trv=trv0;
    pstat = propcyl.vec_prop(trv,SurfCylinder(1.));
    assert( pstat.success() );
    
    xv = trv.space_point().x();
    yv = trv.space_point().y();

    trv=trv0;

    pstat = prop.vec_prop(trv,SurfDCA(xv,yv));
    assert( pstat.success() );

    cout<< trv <<'\n';
    assert( fabs(trv.get_vector(SurfDCA::IRSIGNED))< 1e-10 );

    check_derivatives(prop,trv0,SurfDCA(xv,yv));

   // check backward propagation 
    tvec(2) = PI-0.1;
    trv0.set_vector(tvec);
    trv0.set_forward();
    
    trv=trv0;
    pstat = propcyl.vec_prop(trv,SurfCylinder(1.));
    assert( pstat.success() );
    
    xv = trv.space_point().x();
    yv = trv.space_point().y();

    trv=trv0;

    pstat = prop.vec_prop(trv,SurfDCA(xv,yv));
    assert( pstat.success() );

    cout<< trv <<'\n';
    assert( fabs(trv.get_vector(SurfDCA::IRSIGNED))< 1e-10 );

    check_derivatives(prop,trv0,SurfDCA(xv,yv));

    
    // construct track at a given DCA

    tvec(0) = 1.;
    tvec(2) = PI2;
    tvec(4) = 0.001;
    trv0.set_vector(tvec);
    trv0.set_forward();

    trv=trv0;
    pstat = propcyl.vec_prop(trv,SurfCylinder(12.));
    assert( pstat.success() );

    xv = cos(1.);
    yv = sin(1.);

    trv0=trv;
    pstat = prop.vec_prop(trv,SurfDCA(xv,yv));
    assert( pstat.success() );
    
    cout<<trv<<'\n';
    assert( fabs(trv.get_vector(SurfDCA::IRSIGNED)-9.)< 1e-10 );

    check_derivatives(prop,trv0,SurfDCA(xv,yv));
    
  }  

  

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

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

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

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

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

}

namespace {

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-5 );
  else
    assert( fabs(dydx) < 1e-5 );
}
}