// PropDCACyl_t.cpp

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

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

using namespace trf;
namespace {


void check_derivatives(const Propagator& prop,const VTrack& trv0,const Surface& srf,double prec=1.e-5) ;
void check_derivative(const Propagator& prop,const VTrack& trv0,const Surface& srf,int i,int j,double prec) ;
 
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 = "PropDCACyl (I): ";
  string error_prefix = "PropDCACyl test (E): ";

  cout << ok_prefix
       << "-------- Testing component PropDCACyl. --------" << 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<PropDCACyl>();

  cout << trf_format;
  cout << ok_prefix << "Test constructor." << endl;
  PropDCACyl 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) = 10.0;                                // r_signed
  tvec(1) =  2.0;                                // z
  tvec(2) =  3.0;                                // phi_direction
  tvec(3) =  4.0;                                // tlm
  tvec(4) =  5.0;                                // qpt
  SurfDCA sdca;
  VTrack trv( SurfacePtr(sdca.new_pure_surface()),tvec );
  SpacePoint spt = trv.space_point();

  cout << " *** Propagation to Cylinder of same r *** " << endl;
  SurfCylinder scys(10.0);
  VTrack trvsf = trv;
  pstat = prop.vec_dir_prop(trvsf,scys,Propagator::FORWARD);
  assert( pstat.success() );
  SpacePoint sptsf = trvsf.space_point();
  TrackVector tvec_scys = trvsf.get_vector();
  cout << " sptsf = " << endl;
  cout <<   sptsf << endl;
  cout << " tvec_scys = " << tvec_scys << endl;
  assert( is_equal(sptsf.rxy(),            tvec(SurfDCA::IRSIGNED)  ) );
  assert( is_equal(tvec_scys(SurfCylinder::IZ),  tvec(SurfDCA::IZ)  ) );
  assert( is_equal(tvec_scys(SurfCylinder::IPHI),spt.phi()          ) );
  assert( is_equal(tvec_scys(SurfCylinder::ITLM),tvec(SurfDCA::ITLM)) );
  assert( is_equal(tvec_scys(SurfCylinder::IQPT),tvec(SurfDCA::IQPT)) );

  SurfCylinder scyl(15.);

  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,scyl,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  after propagation: trv2f = " << trv2f << endl;
             spt2f = trv2f.space_point();
  cout << "  after propagation: spt2f = " << endl;
  cout << spt2f << endl;
  assert( spt2f.rxy() == 15.0 );

  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,scyl,Propagator::BACKWARD);
  assert( pstat.backward() );
  cout << "  after propagation: trv2b = " << trv2b << endl;
             spt2b = trv2b.space_point();
  cout << "  after propagation: spt2b = " << endl;
  cout << spt2b << endl;
  assert( spt2b.rxy() == 15.0 );


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

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

  //  PropStat pstat;

  //  TrackVector tvec;
  //  tvec(0) = 10.0;                                // r_signed
  //  tvec(1) =  2.0;                                // z
  //  tvec(2) =  3.0;                                // phi_direction
  //  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;
  //  SurfDCA sdca;
  ETrack etrv( SurfacePtr(sdca.new_pure_surface()),tvec,err );
  SpacePoint espt = etrv.space_point();

  //  SurfCylinder scyl(15.);

  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,scyl,Propagator::FORWARD);
  assert( pstat.forward() );
  cout << "  after propagation: etrv2f = " << etrv2f << endl;
             espt2f = etrv2f.space_point();
  cout << "  after propagation: espt2f = " << endl;
  cout << espt2f << endl;
  assert( espt2f.rxy() == 15.0 );

  //*******************************************************************
  // Test non 0.0 DCA
  #define NUM 10
  double rad[3] = {20.,2.0,sqrt(5.)};
  for( int j=0; j <3 ;++j)
  for(int i=0 ; i< NUM; ++i )
  {
    // double prec[NUM] = { 1e-5,1e-5, 1e-5};
    double xv[NUM]   = { 0.5 , 0., 2. , 2.,    2.,   2.,  2. ,2.,  2. ,2.};
    double yv[NUM]   = { 0.4 , 0., -1., -1.,  -1.,  -1., -1. ,-1., -1. ,-1.};
    double r[NUM]    = { 1. , 0., 0.1, -0.1, -0.1, 0.1 , 0. , 0., -0.1 , 0.1};
    double phid[NUM] = { 0.1, 0., 0.2, 0.2, 0.2 , 0.2 ,  0. , 0.,  0. , 0.};
    double z[NUM]    = {  2., 2., 2. , 2.,    2.,    2., 2. , 3., 2. , 3.};
    double tlm[NUM]  = { -4., 4., 4. , 4.,    4.,   -4., 4. , 2., 4. , 2.};
    double qpt[NUM]  = { 0.1, 0., 0.01, 0.01,-0.01,-0.01,-0.01 , 0.01,0.0 ,0.};
    SurfDCA srf(xv[i],yv[i]);
    TrackVector vec;
    TrackError err;
    for(int k=1;k<=5;++k)
      err(k-1,k-1)=k;
    vec(SurfDCA::IRSIGNED)= r[i];
    vec(SurfDCA::IZ)= z[i];
    vec(SurfDCA::IPHID)= phid[i];
    vec(SurfDCA::ITLM)= tlm[i];
    vec(SurfDCA::IQPT)=qpt[i];
    ETrack tre(SurfacePtr(srf.new_pure_surface()));
    tre.set_vector(vec);
    tre.set_error(err);
    tre.set_forward();
    SurfCylinder cyl(rad[j]);
    PropDCACyl propdca_cyl(ObjDoublePtr(new ObjDouble(-2.)));
    PropCylDCA propcyl_dca(ObjDoublePtr(new ObjDouble(-2.)));
    ETrack tre0(tre);
    ETrack treb(tre);
    PropStat pstat = propdca_cyl.err_dir_prop(tre,cyl,Propagator::FORWARD);
    assert( pstat.success());
    pstat = propdca_cyl.err_dir_prop(treb,cyl,Propagator::BACKWARD);
    assert( pstat.success());
    check_derivatives(propdca_cyl,tre0,cyl);
    ETrack tre1(tre);
    ETrack tre1b(treb);
    pstat = propcyl_dca.err_prop(tre,srf);
    assert( pstat.success());
    pstat = propcyl_dca.err_prop(treb,srf);
    assert( pstat.success());
    check_derivatives(propcyl_dca,tre1,srf);
    assert(about_equal(tre0,tre));
    check_derivatives(propcyl_dca,tre1b,srf);
    assert(about_equal(tre0,treb));
  }

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

  cout << ok_prefix << "Write object data." << endl;
  {
    ObjPtr pobj( new PropDCACyl(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( "PropDCACyl ",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 PropDCACyl bfield=24.6 ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const PropDCACyl* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() == PropDCACyl::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,double prec) {
  for(int i=0;i<4;++i)
    for(int j=0;j<4;++j)
      check_derivative(prop,trv0,srf,i,j,prec);
}

void check_derivative(const Propagator& prop,const VTrack& trv0,const Surface& srf,int i,int j,double prec) {

  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 dy1 = vecpl(i)-trv_0.get_vector(i);

  if( !is_zero(dy) ) {
    if( fabs((dy1-dy)/dy) > 0.1 )
      return;
  }
  else {
    if( fabs(dy1) > 1e-5 )
      return;
  }
  if( fabs(fabs(dy1)-fabs(dy)) > 0.01 )
    dy=dy1;

  double dydx = dy/dx;

  double didj = der(i,j);

  if( fabs(didj) > 1e-10 )
    assert( fabs((dydx - didj)/didj) < prec );
  else
    assert( fabs(dydx) < prec );
}
}