// PropZZ_t.cc

// Test PropZZ.

#include "PropZZ.h"
#include "trfbase/PropStat.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "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;
#endif

using namespace trf;

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

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

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

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

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

  // Here we propagate some tracks both forward and backward and then
  // each back to the original track.  We check that the returned
  // track parameters match those of the original track.
 cout << ok_prefix << "Check reversibility." << endl;

 double z1[]  ={10.,      10.,     10.,     10.,     10.,     10.};
 double z2[]  ={20.,     -20.,     20.,     20.,    -20.,    -20.};
 double z3[]  ={30.,     -30.,     30.,     30.,    -30.,    -30.};
 int sign_dz[]={  1,       -1,      1,       1,       -1,      -1     };
 double x[]   ={ -2.,       2.,     40.,     40.,     -2.,     -2.    };
 double y[]   ={  3.,       3.,      3.,      3.,      3.,      3.    };
 double dxdz[]={-1.5,     -2.3,     0.,      1.5,     0.,       0.    };
 double dydz[]={ 2.3,     -1.5,    -2.3,     0.,      2.3,      0.   };
 double qp[]  ={ 0.05,    -0.05,    0.05,   -0.05,   -0.05,     0.05  };

 double maxdiff = 1.e-12;
 int ntrk = 6;
 int i;
 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;
  PropStat pstat;
  SurfZPlane szp1(z1[i]);
  SurfZPlane szp2(z2[i]);
  SurfZPlane szp3(z3[i]);
  TrackVector vec1; 
  vec1(SurfZPlane::IX)    = x[i];     // x
  vec1(SurfZPlane::IY)    = y[i];     // y
  vec1(SurfZPlane::IDXDZ) = dxdz[i];  // dx/dz
  vec1(SurfZPlane::IDYDZ) = dydz[i];  // dy/dz
  vec1(SurfZPlane::IQP)   = qp[i];    //  q/p
  VTrack trv1(SurfacePtr(szp2.new_pure_surface()),vec1);
  (sign_dz[i]==1)?trv1.set_forward():trv1.set_backward();
  cout << " starting: " << trv1 << endl;
  VTrack trv2f = trv1;
  pstat = prop.vec_dir_prop(trv2f,szp3,Propagator::FORWARD);
  assert( pstat.forward() );  
  cout << "  forward: " << trv2f << endl;
  VTrack trv2b = trv1;
  pstat = prop.vec_dir_prop(trv2b,szp1,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;
  VTrack trv2fb = trv2f;
  pstat = prop.vec_dir_prop(trv2fb,szp2,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " f return: " << trv2fb << endl;
  VTrack trv2bf = trv2b;
  pstat = prop.vec_dir_prop(trv2bf,szp2,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << " b return: " << trv2bf << endl;
  double difff = 
    szp1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
  double diffb = 
    szp1.vec_diff(trv2bf.get_vector(),trv1.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 exx[] =   {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double exy[] =   {  0.01,  -0.01,   0.01,  -0.01,   0.01,  -0.01  };
 double eyy[] =   {  0.25,   0.25,   0.25,   0.25,   0.25,   0.25, };
 double exdx[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double eydx[] =  {  0.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04, };
 double edxdx[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double exdy[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edxdy[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double eydy[] =  {  0.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04  };
 double edydy[] = {  0.02,   0.02,   0.02,   0.02,   0.02,   0.02  };
 double exqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double eyqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edxqp[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edyqp[] = {  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;
  SurfZPlane szp1(z1[i]);
  SurfZPlane szp2(z2[i]);
  SurfZPlane szp3(z3[i]);
  TrackVector vec1;
  vec1(SurfZPlane::IX)    = x[i];     // x
  vec1(SurfZPlane::IY)    = y[i];     // y
  vec1(SurfZPlane::IDXDZ) = dxdz[i];  // dx/dz
  vec1(SurfZPlane::IDYDZ) = dydz[i];  // dy/dz
  vec1(SurfZPlane::IQP)   = qp[i];    //  q/p

  TrackError err1;
  err1(SurfZPlane::IX,SurfZPlane::IX)       = exx[i];
  err1(SurfZPlane::IX,SurfZPlane::IY)       = exy[i];
  err1(SurfZPlane::IY,SurfZPlane::IY)       = eyy[i];
  err1(SurfZPlane::IX,SurfZPlane::IDXDZ)    = exdx[i];
  err1(SurfZPlane::IY,SurfZPlane::IDXDZ)    = eydx[i];
  err1(SurfZPlane::IDXDZ,SurfZPlane::IDXDZ) = edxdx[i];
  err1(SurfZPlane::IX,SurfZPlane::IDYDZ)    = exdy[i];
  err1(SurfZPlane::IY,SurfZPlane::IDYDZ)    = eydy[i];
  err1(SurfZPlane::IDXDZ,SurfZPlane::IDYDZ) = edxdy[i];
  err1(SurfZPlane::IDYDZ,SurfZPlane::IDYDZ) = edydy[i];
  err1(SurfZPlane::IX,SurfZPlane::IQP)      = exqp[i];
  err1(SurfZPlane::IY,SurfZPlane::IQP)      = eyqp[i];
  err1(SurfZPlane::IDXDZ,SurfZPlane::IQP)   = edxqp[i];
  err1(SurfZPlane::IDYDZ,SurfZPlane::IQP)   = edyqp[i];
  err1(SurfZPlane::IQP,SurfZPlane::IQP)     = eqpqp[i];
  ETrack trv1(SurfacePtr(szp2.new_pure_surface()),vec1,err1);
  (sign_dz[i]==1)?trv1.set_forward():trv1.set_backward();
  cout << " starting: " << trv1 << endl;

  ETrack trv2f = trv1;
  pstat = prop.err_dir_prop(trv2f,szp3,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  forward: " << trv2f << endl;
  ETrack trv2b = trv1;
  pstat = prop.err_dir_prop(trv2b,szp1,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;
  ETrack trv2fb = trv2f;
  pstat = prop.err_dir_prop(trv2fb,szp2,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " f return: " << trv2fb << endl;
  ETrack trv2bf = trv2b;
  pstat = prop.err_dir_prop(trv2bf,szp2,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << " b return: " << trv2bf << endl;
  double difff = 
    szp1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
  double diffb = 
    szp1.vec_diff(trv2bf.get_vector(),trv1.get_vector()).amax();
  cout << "vec diffs: " << difff << ' ' << diffb << endl;
  assert( difff < maxdiff );
  assert( diffb < maxdiff );
  TrackError dfb = trv2fb.get_error() - trv1.get_error();
  TrackError dbf = trv2bf.get_error() - trv1.get_error();
  double edifff = dfb.amax();
  double ediffb = dbf.amax();
  cout << "err diffs: " << edifff << ' ' << ediffb << endl;
  assert( edifff < maxdiff );
  assert( ediffb < maxdiff );
 }

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

 cout << ok_prefix << "Test Nearest Propagation" << endl;

 PropStat pstat;

 SurfZPlane szp1(5.);
 SurfZPlane szp2(10.);

 TrackVector vec1; 
 vec1(SurfZPlane::IX)    = 1.;     // x
 vec1(SurfZPlane::IY)    = 1.;     // y
 vec1(SurfZPlane::IDXDZ) = 1.;     // dx/dz
 vec1(SurfZPlane::IDYDZ) = 1.;     // dy/dz
 vec1(SurfZPlane::IQP)   = 0.01;   //  q/p

 VTrack trv1(SurfacePtr(szp1.new_pure_surface()),vec1);
 trv1.set_forward();

 cout << " starting: " << trv1 << endl;
 VTrack trv2n = trv1;
 pstat = prop.vec_dir_prop(trv2n,szp2,Propagator::NEAREST_MOVE);
 assert( pstat.forward() );  
 cout << " nearest: " << trv2n << endl; 

 trv1.set_backward();
 
 cout << " starting: " << trv1 << endl;
 trv2n = trv1;
 pstat = prop.vec_dir_prop(trv2n,szp2,Propagator::NEAREST);
 assert( pstat.backward() );  
 cout << " nearest: " << trv2n << endl;

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

 cout << ok_prefix << "Test same surface" << endl;
 VTrack trvs0(SurfacePtr(szp1.new_pure_surface()),vec1);
 trvs0.set_forward();
 VTrack trvs(trvs0);
 PropStat tst = prop.vec_dir_prop(trvs,szp1,Propagator::NEAREST_MOVE);
 assert( !tst.success() );
 tst = prop.vec_dir_prop(trvs,szp1,Propagator::FORWARD_MOVE);
 assert( !tst.success() );
 tst = prop.vec_dir_prop(trvs,szp1,Propagator::BACKWARD_MOVE);
 assert( !tst.success() );
 tst = prop.vec_dir_prop(trvs,szp1,Propagator::NEAREST);
 assert( tst.success() );
 tst = prop.vec_dir_prop(trvs,szp1,Propagator::FORWARD);
 assert( tst.success() );
 tst = prop.vec_dir_prop(trvs,szp1,Propagator::BACKWARD);
 assert( tst.success() );
 assert ( trvs0 == trvs );

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

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

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

 cout << ok_prefix << "Test Zero B field propagation" << endl;

 {
   PropZZ prop0(ObjDoublePtr(new ObjDouble(0.)));
   cout << prop0 << endl;
   assert( prop0.get_bfield() == 0. );

   double z=10.;
   SurfacePtr psrf(new SurfZPlane(z));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfZPlane::IX) = 2.;
   vec(SurfZPlane::IY) = 10.;
   vec(SurfZPlane::IDXDZ) = 4.;
   vec(SurfZPlane::IDYDZ) = 2.;
   trv0.set_vector(vec);
   trv0.set_forward();
   z=4.;
   SurfacePtr psrf_to(new SurfZPlane(z));
   
   VTrack trv(trv0);
   PropStat pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::FORWARD);
   assert( !pstat.success() );
   
   trv = trv0;
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::BACKWARD);
   assert( pstat.success() );

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

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

   assert( pstat.backward() );
   assert( trv.get_vector(SurfZPlane::IDXDZ) == trv0.get_vector(SurfZPlane::IDXDZ) );
   assert( trv.get_vector(SurfZPlane::IDYDZ) == trv0.get_vector(SurfZPlane::IDYDZ) );
   assert(trv.get_surface()->pure_equal(*psrf_to));

   check_zero_propagation(trv0,trv,pstat);
   
   psrf_to = new SurfZPlane(6.);
   trv = trv0;
   trv.set_forward();
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );
   check_zero_propagation(trv0,trv,pstat);

   psrf_to = new SurfZPlane(14.);
   trv = trv0;
   trv.set_forward();
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );
   check_zero_propagation(trv0,trv,pstat);

   psrf_to = new SurfZPlane(-1.);
   trv = trv0;
   trv.set_forward();
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );
   check_zero_propagation(trv0,trv,pstat);

   psrf_to = new SurfZPlane(-14.);
   trv = trv0;
   trv.set_forward();
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );
   check_zero_propagation(trv0,trv,pstat);

   psrf_to = new SurfZPlane(14.);
   trv = trv0;
   trv.set_surface(SurfacePtr(new SurfZPlane(1.)));
   trv.set_backward();
   VTrack tmp = trv;
   VTrack der = trv;
   VTrack der1 = trv;
   der1.set_forward();
   pstat = prop0.vec_dir_prop(trv,*psrf_to,Propagator::NEAREST);
   assert( pstat.success() );
   check_zero_propagation(tmp,trv,pstat);
   check_derivatives(prop0,der,*psrf_to);
   check_derivatives(prop0,der1,*psrf_to);   
 }

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

  ObjTable::register_type<PropZZ>();

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

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

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

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

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