// PropXYXYBV_t.cc

// Test PropXYXYBV.

#include "PropXYXYBV.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"

#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 v propagation

double check_dv(VTrack trv1, VTrack trv2)
{
 double v1    = trv1.get_vector()(SurfXYPlane::IV);
 double v2    = trv2.get_vector()(SurfXYPlane::IV);
 double dvdu  = trv1.get_vector()(SurfXYPlane::IDVDU);
 int sign_du = (trv1.is_forward())?1:-1;
 return (v2-v1)*dvdu*sign_du;
}
//**********************************************************************
int main( ) {

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

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

  //********************************************************************
  // Check that if planes are not parallel the propagatiopn fails
  {
  cout<< ok_prefix << "Check that only tracks on right surfaces are being propagated " << endl;

  PropStat pstat;

  SurfXYPlane sxyp1(10.,0.);
  SurfXYPlane sxyp2(12.,0.);
  SurfXYPlane sxyp1np(10.,0.1);
  SurfXYPlane sxyp2np(12.,0.1);

  TrackVector vec1; 

  vec1(SurfXYPlane::IV)    = -2.;     // v
  vec1(SurfXYPlane::IZ)    = 3.;     // z
  vec1(SurfXYPlane::IDVDU) = 1.5;  // dv/du
  vec1(SurfXYPlane::IDZDU) = 2.3;  // dz/du
  vec1(SurfXYPlane::IQP)   = 0.05;    //  q/p

  VTrack trv1(SurfacePtr(sxyp1.new_pure_surface()),vec1);
  trv1.set_forward();
  
  pstat = prop.vec_dir_prop(trv1,sxyp2,Propagator::FORWARD);
  assert( pstat.success() );  

  trv1.set_vector(vec1);
  trv1.set_surface(SurfacePtr(sxyp1.new_pure_surface()));
  trv1.set_forward();

  pstat = prop.vec_dir_prop(trv1,sxyp2np,Propagator::FORWARD);
  assert( !pstat.success() ); 
 
  trv1.set_vector(vec1);
  trv1.set_surface(SurfacePtr(sxyp1np.new_pure_surface()));
  trv1.set_forward();

  pstat = prop.vec_dir_prop(trv1,sxyp2,Propagator::FORWARD);
  assert( !pstat.success() );  
 
  trv1.set_vector(vec1);
  trv1.set_surface(SurfacePtr(sxyp1np.new_pure_surface()));
  trv1.set_forward();

  pstat = prop.vec_dir_prop(trv1,sxyp2np,Propagator::FORWARD);
  assert( !pstat.success() );  
 

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

  // 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[]  ={12.,      12.,     12.,     12.,     12.,     12.};
 double phi1[]={  0.,    0.,   0. ,   0.,   0.,   0.};
 double phi2[]={  0.,    0.,   0.,    0.,   0.,   0.};
 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);
  cout << pstat << endl;
  assert( pstat.forward() );  
  cout << "  forward: " << trv2f << endl;
  assert(check_dv(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_dv(trv1,trv2b)<=0.);

  VTrack trv2fb = trv2f;
  pstat = prop.vec_dir_prop(trv2fb,sxyp1,Propagator::BACKWARD);
  cout << pstat << endl;
  assert( pstat.backward() );
  cout << " f return: " << trv2fb << endl;
  assert(check_dv(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_dv(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 || diffb < maxdiff );
  //  assert( diffb < maxdiff );
 }

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

 // Repeat the above with errors.
 cout << ok_prefix << "Check reversibility with errors." << endl;
 double evv[] =   {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double evz[] =   {  0.01,  -0.01,   0.01,  -0.01,   0.01,  -0.01  };
 double ezz[] =   {  0.25,   0.25,   0.25,   0.25,   0.25,   0.25, };
 double evdv[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezdv[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvdv[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };
 double evdz[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvdz[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezdz[] =  {  0.04,  -0.04,   0.04,  -0.04,   0.04,  -0.04  };
 double edzdz[] = {  0.02,   0.02,   0.02,   0.02,   0.02,   0.02  };
 double evqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double ezqp[] =  {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edvqp[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double edzqp[] = {  0.004, -0.004,  0.004, -0.004,  0.004, -0.004 };
 double eqpqp[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01  };

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

  
  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 << "vec diffs: " << difff << ' ' << diffb << endl;
  cout << "err diffs: " << edifff << ' ' << ediffb << endl;

  assert( (difff < maxdiff && edifff < maxdiff) || 
          (diffb < maxdiff && ediffb < maxdiff)    );
 
 }

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

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

 PropStat pstat;
 SurfXYPlane sxyp1(2.,0.);
 SurfXYPlane sxyp2(3.,0.);

 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 cloning." << endl;
 assert( prop.new_propagator() != 0);

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

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

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

  ObjTable::register_type<PropXYXYBV>();

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

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

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

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