// PropXYXY_t.cc

// Test PropXYXY.

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

//**********************************************************************
//Checker of correct z propagation
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) ;
}

double check_dz(VTrack trv1, VTrack trv2)
{
 double z1    = trv1.get_vector()(SurfXYPlane::IZ);
 double z2    = trv2.get_vector()(SurfXYPlane::IZ);
 double dzdu  = trv1.get_vector()(SurfXYPlane::IDZDU);
 int sign_du = (trv1.is_forward())?1:-1;
 return (z2-z1)*dzdu*sign_du;
}
//**********************************************************************
int main( ) {

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

  cout << ok_prefix
       << "-------- Testing component PropXYXY. --------" << 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;
  PropXYXY 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 u1[]  ={10.,      10.,     10.,     10.,     10.,     10.};
 double u2[]  ={10.,      10.,     10.,     10.,     10.,     10.};
 double phi1[]={  PI/2.,    PI/2.,   PI/2.,   PI/2.,   PI/2.,   PI/2.};
 double phi2[]={5*PI/6.,  5*PI/6., 5*PI/6., 5*PI/6., 5*PI/6., 5*PI/6.};
 int sign_du[]={  1,       -1,      -1,       1,       1,       1     };
 double v[]   ={ -2.,       2.,     40.,     40.,     -2.,     -2.    };
 double z[]   ={  3.,       3.,      3.,      3.,      3.,      3.    };
 double dvdu[]={-1.5,     -1.5,     1.5,     1.5,    -1.5,      0.    };
 double dzdu[]={ 2.3,     -2.3,    -2.3,     2.3,     0.,       2.3   };
 double qp[]  ={ 0.05,    -0.05,    0.05,   -0.05,    0.05,     0.05  };

 double maxdiff = 1.e-9;
 int ntrk = 6;
 int i;
 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;
  PropStat pstat;
  SurfXYPlane sxyp1(u1[i],phi1[i]);
  SurfXYPlane sxyp2(u2[i],phi2[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,sxyp2,Propagator::FORWARD_MOVE);
  cout << pstat << endl;
  assert( pstat.forward() );  
  cout << "  forward: " << trv2f << endl;
  assert(check_dz(trv1,trv2f)>=0.);

  VTrack trv2b = trv1;
  pstat = prop.vec_dir_prop(trv2b,sxyp2,Propagator::BACKWARD);
  cout << pstat << endl;
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;
  assert(check_dz(trv1,trv2b)<=0.);

  VTrack trv2fb = trv2f;
  pstat = prop.vec_dir_prop(trv2fb,sxyp1,Propagator::BACKWARD_MOVE);
  cout << pstat << endl;
  assert( pstat.backward() );
  cout << " f return: " << trv2fb << endl;
  assert(check_dz(trv2f,trv2fb)<=0.);

  VTrack trv2bf = trv2b;
  pstat = prop.vec_dir_prop(trv2bf,sxyp1,Propagator::FORWARD);
  cout << pstat << endl;
  assert( pstat.forward() );
  cout << " b return: " << trv2bf << endl;
    assert(check_dz(trv2b,trv2bf)>=0.);

  double difff = 
    sxyp1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
  double diffb = 
    sxyp1.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 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.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04, };
 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  };

 maxdiff = 1.e-6;

 for ( i=0; i<ntrk; ++i ) {
  cout << "********** Propagate track " << i << ". **********" << endl;
  PropStat pstat;
  SurfXYPlane sxyp1(u1[i],phi1[i]);
  SurfXYPlane sxyp2(u2[i],phi2[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,sxyp2,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  forward: " << trv2f << endl;
  ETrack trv2b = trv1;
  pstat = prop.err_dir_prop(trv2b,sxyp2,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " backward: " << trv2b << endl;
  ETrack trv2fb = trv2f;
  pstat = prop.err_dir_prop(trv2fb,sxyp1,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << " f return: " << trv2fb << endl;
  ETrack trv2bf = trv2b;
  pstat = prop.err_dir_prop(trv2bf,sxyp1,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << " b return: " << trv2bf << endl;
  double difff = 
    sxyp1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
  double diffb = 
    sxyp1.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;
 SurfXYPlane sxyp1(2.,PI/3.);
 SurfXYPlane sxyp2(3.,PI/3.);

 TrackVector vec1; 

 vec1(SurfXYPlane::IV)    = 1.;     // v
 vec1(SurfXYPlane::IZ)    = 1.;     // z
 vec1(SurfXYPlane::IDVDU) = 1.;     // dv/du
 vec1(SurfXYPlane::IDZDU) = 1.;     // dz/du
 vec1(SurfXYPlane::IQP)   = 0.01;   //  q/p

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

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

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

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

 cout << ok_prefix << "Test XXX_MOVE and Same Surface Propagation." << endl;

 VTrack trvt0(SurfacePtr(sxyp1.new_pure_surface()),vec1);
 trvt0.set_forward();
 VTrack trvt1(trvt0);
 PropStat tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::NEAREST);
 assert ( tst.success() && trvt1 == trvt0 );
 tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::FORWARD);
 assert ( tst.success() && trvt1 == trvt0 );
 tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::BACKWARD);
 assert ( tst.success() && trvt1 == trvt0 );

 trvt1=trvt0;
 tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::NEAREST_MOVE);
 assert ( tst.success() && trvt1 != trvt0 );
 trvt1=trvt0;
 tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::FORWARD_MOVE);
 assert ( tst.success() && trvt1 != trvt0 );
 trvt1=trvt0;
 tst = prop.vec_dir_prop(trvt1,sxyp1,Propagator::BACKWARD_MOVE);
 assert ( tst.success() && trvt1 != trvt0 );


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

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

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

   double u=10.,phi=0.;
   SurfacePtr psrf(new SurfXYPlane(u,phi));
   VTrack trv0(psrf);
   TrackVector vec;
   vec(SurfXYPlane::IV) = 2.;
   vec(SurfXYPlane::IZ) = 10.;
   vec(SurfXYPlane::IDVDU) = 4.;
   vec(SurfXYPlane::IDZDU) = 2.;
   trv0.set_vector(vec);
   trv0.set_forward();
   u=4.;
   SurfacePtr psrf_to(new SurfXYPlane(u,phi));
   
   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(SurfXYPlane::IDVDU) == trv0.get_vector(SurfXYPlane::IDVDU) );
   assert( trv.get_vector(SurfXYPlane::IDZDU) == trv0.get_vector(SurfXYPlane::IDZDU) );
   assert(trv.get_surface()->pure_equal(*psrf_to));

   check_zero_propagation(trv0,trv,pstat);
   
   psrf_to = new SurfXYPlane(4.,PI/16.);
   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 SurfXYPlane(14.,PI/4.);
   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 SurfXYPlane(14.,PI/2.);
   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 SurfXYPlane(14.,PI);
   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 SurfXYPlane(14.,PI*5./4.);
   trv = trv0;
   trv.set_surface(SurfacePtr(new SurfXYPlane(14.,PI*7./4.)));
   trv.set_forward();
   VTrack tmp = trv;
   VTrack der = trv;
   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);
   
 }

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

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

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

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

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

  ObjTable::register_type<PropXYXY>();

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

  }

  //********************************************************************
      /*
    {
      cout<<"===========================================\n";
      PropXYXY p1(ObjDoublePtr(new ObjDouble(-2.)));
      PropXYXY p2(ObjDoublePtr(new ObjDouble(2.)));
      SurfXYPlane xy1(5.,0.);
      SurfXYPlane xy2(3.,0.);
      ETrack tre0(SurfacePtr(xy2.new_surface()));
      TrackVector vec;TrackError err;
      vec(0)=0.;
      vec(1)=0.;
      vec(2)=0.0;
      vec(3)=0.1;
      vec(4)=-0.1;
      err(0,0)=err(1,1)=err(2,2)=err(3,3)=err(4,4)=0.1;
      tre0.set_vector(vec);
      tre0.set_error(err);
      tre0.set_backward();
      ETrack tre1(tre0),tre2(tre0);
      PropStat pstat=p1.err_dir_prop(tre1,xy1,Propagator::BACKWARD);
      assert(pstat.success());
      pstat=p2.err_dir_prop(tre2,xy1,Propagator::BACKWARD);
      assert(pstat.success());
      cerr<<tre1<<'\n'<<tre2<<'\n';
      cout<<"===========================================\n";
      cout.flush();
    }

    */

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

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