// PropCyl_t.cpp

// Test PropCyl.

#include "PropCyl.h"
#include <iostream>
#include <string>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfutil/trfstream.h"
#include "trfutil/TRFMath.h"
#include "trfbase/PropStat.h"
#include "trfbase/TrackVector.h"
#include "trfbase/VTrack.h"
#include "trfbase/ETrack.h"
#include "SurfCylinder.h"
#include "PropCylFieldConst.h"

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

using namespace trf;

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

int main( ) {

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

  cout << ok_prefix
       << "-------- Testing component PropCyl. --------" << 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<PropCylFieldConst>();
  ObjTable::register_type<PropCyl>();

  cout << trf_format;
  cout << ok_prefix << "Test constructor." << endl;
  PropCyl 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 r1[] =   { 10.0,  10.0,  10.0,  10.0, 10.0,  10.0, 10.0, 10.0 };
  double r2[] =   { 20.0,  20.0,  20.0,  20.0, 20.0,  20.0, 20.0, 20.0 };
  double phi1[] = {  0.0,   1.0,  -1.0,   2.0,  2.0,  -2.0, 3.0,   0.0 };
  double alf1[] = {  0.0,  .0001,-.0001,  0.9, -0.9,   0.9, 0.9,   3.0 };
  double crv[]  = {  0.0,   0.0,   0.0,   0.0,  0.0, -0.05, 0.01,  0.0 };
  double z[]  =   {  0.0,   0.0,   0.0,   0.0,  0.0,   0.0, 15.0,  0.0 };
  double tlm[]  = {  0.0,   0.0,   0.0,   0.0,  0.0,   0.0, 0.35,  0.0 };
  double fbdf[] = {  0.0,   0.0,   0.0,   0.0,  0.0,   0.0,   0.0, 0.0 };
  double bfdf[] = {  0.0,   0.0,   0.0,   0.0,  0.0,   0.0,   0.0, 0.0 };
  double maxdiff = 1.e-10;
  int ntrk = 8;
  int i;
  for ( i=0; i<ntrk; ++i ) {
    cout << "********** Propagate track " << i << ". **********" << endl;
    PropStat pstat;
    SurfCylinder scy1(r1[i]);
    SurfCylinder scy2(r2[i]);
    TrackVector vec1;
    vec1(0) = phi1[i]; // phi
    vec1(1) = z[i];    // z
    vec1(2) = alf1[i]; // alpha
    vec1(3) = tlm[i];  // tlam
    vec1(4) = crv[i];  // curv
    VTrack trv1(SurfacePtr(scy1.new_pure_surface()),vec1);
    cout << " starting: " << trv1 << endl;
    //
    // Propagate forward.
    VTrack trv2f = trv1;
    pstat = prop.vec_dir_prop(trv2f,scy2,Propagator::FORWARD);
    cout << "  forward: " << trv2f << endl;
    cout << pstat << endl;
    assert( pstat.forward() );
    //
    // Propagate backward.
    VTrack trv2b = trv1;
    pstat = prop.vec_dir_prop(trv2b,scy2,Propagator::BACKWARD);
    cout << " backward: " << trv2b << endl;
    cout << pstat << endl;
    assert( pstat.backward() );
    //
    // Propagate forward track backward.
    VTrack trv2fb = trv2f;
    pstat = prop.vec_dir_prop(trv2fb,scy1,Propagator::BACKWARD);
    cout << " f return: " << trv2fb << endl;
    cout << pstat << endl;
    assert( pstat.backward() );
    //
    // Propagate backward track forward.
    VTrack trv2bf = trv2b;
    pstat = prop.vec_dir_prop(trv2bf,scy1,Propagator::FORWARD);
    cout << " b return: " << trv2bf << endl;
    cout << pstat << endl;
    assert( pstat.forward() );
    // Check the return differences.
    // At least one of these should be zero.
    // Not both because we may cross the original surface on one of
    // the paths out.
    double difff = 
      scy1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
    double diffb = 
      scy1.vec_diff(trv2bf.get_vector(),trv1.get_vector()).amax();
    cout << "diffs: " << difff << ' ' << diffb << endl;
    assert( difff<maxdiff || diffb<maxdiff );
    //
    // Check no-move forward propagation to the same surface.
    VTrack trv1s = trv1;
    pstat = prop.vec_dir_prop(trv1s,scy1,Propagator::FORWARD);
    cout << " same f: " << trv1s << endl;
    cout << pstat << endl;
    assert( pstat.same() );
    assert( pstat.path_distance() == 0 );
    //
    // Check no-move backward propagation to the same surface.
    trv1s = trv1;
    pstat = prop.vec_dir_prop(trv1s,scy1,Propagator::BACKWARD);
    cout << " same f: " << trv1s << endl;
    cout << pstat << endl;
    assert( pstat.same() );
    assert( pstat.path_distance() == 0 );
    //
    // Check move propagation to the same surface.
    //
    // forward
    int successes = 0;
    trv1s = trv1;
    pstat = prop.vec_dir_prop(trv1s,scy1,Propagator::FORWARD_MOVE);
    cout << " forward move: " << trv1s << endl;
    cout << pstat << endl;
    if ( pstat.forward() ) ++successes;
    // backward
    trv1s = trv1;
    pstat = prop.vec_dir_prop(trv1s,scy1,Propagator::BACKWARD_MOVE);
    cout << " backward move: " << trv1s << endl;
    cout << pstat << endl;
    if ( pstat.backward() ) ++successes;
    // One of these should have succeeded.
    assert( successes == 1 );
    //
    // nearest
    trv1s = trv1;
    pstat = prop.vec_dir_prop(trv1s,scy1,Propagator::NEAREST_MOVE);
    cout << " nearest move: " << trv1s << endl;
    cout << pstat << endl;
    assert ( pstat.success() );
  }

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

  // Repeat the above with errors.
  cout << ok_prefix << "Check reversibility with errors." << endl;
  double epp[] = {  0.01,   0.01,   0.01,   0.01,   0.01,   0.01,  0.01, 0.01  };
  double epz[] = { 0.01,  -0.01,  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,  0.25,  0.25  };
  double epa[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double eza[] = { 0.04,  -0.04,  0.04,  -0.04,  0.04,  -0.04,  0.04,  0.04  };
  double eaa[] = { 0.01,   0.01,  0.01,   0.01,  0.01,   0.01,  0.01,  0.01  };
  double epl[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double eal[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double ezl[] = { 0.04,  -0.04,  0.04,  -0.04,  0.04,  -0.04,  0.04,  0.04  };
  double ell[] = { 0.02,   0.02,  0.02,   0.02,  0.02,   0.02,  0.02,  0.02  };
  double epc[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double ezc[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double eac[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double elc[] = { 0.004, -0.004, 0.004, -0.004, 0.004, -0.004, 0.004, 0.004 };
  double ecc[] = { 0.01,   0.01,  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;
    SurfCylinder scy1(r1[i]);
    SurfCylinder scy2(r2[i]);
    TrackVector vec1;
    vec1(0) = phi1[i]; // phi
    vec1(1) = z[i];    // z
    vec1(2) = alf1[i]; // alpha
    vec1(3) = tlm[i];  // tlam
    vec1(4) = crv[i];  // curv
    TrackError err1;
    err1(0,0) = epp[i];
    err1(0,1) = epz[i];
    err1(1,1) = ezz[i];
    err1(0,2) = epa[i];
    err1(1,2) = eza[i];
    err1(2,2) = eaa[i];
    err1(0,3) = epl[i];
    err1(1,3) = ezl[i];
    err1(2,3) = eal[i];
    err1(3,3) = ell[i];
    err1(0,4) = epc[i];
    err1(1,4) = ezc[i];
    err1(2,4) = eac[i];
    err1(3,4) = elc[i];
    err1(4,4) = ecc[i];
    ETrack trv1(SurfacePtr( scy1.new_pure_surface()), vec1, err1 );
    cout << " starting: " << trv1 << endl;
    ETrack trv2f = trv1;
    pstat = prop.err_dir_prop(trv2f,scy2,Propagator::FORWARD);
    assert( pstat.forward() );
    cout << "  forward: " << trv2f << endl;
    ETrack trv2b = trv1;
    pstat = prop.err_dir_prop(trv2b,scy2,Propagator::BACKWARD);
    assert( pstat.backward() );
    cout << " backward: " << trv2b << endl;
    ETrack trv2fb = trv2f;
    pstat = prop.err_dir_prop(trv2fb,scy1,Propagator::BACKWARD);
    assert( pstat.backward() );
    cout << " f return: " << trv2fb << endl;
    ETrack trv2bf = trv2b;
    pstat = prop.err_dir_prop(trv2bf,scy1,Propagator::FORWARD);
    assert( pstat.forward() );
    cout << " b return: " << trv2bf << endl;
    double difff = 
      scy1.vec_diff(trv2fb.get_vector(),trv1.get_vector()).amax();
    double diffb = 
      scy1.vec_diff(trv2bf.get_vector(),trv1.get_vector()).amax();
    cout << "vec diffs: " << difff << ' ' << diffb << endl;
    assert( difff<maxdiff || 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 || ediffb<maxdiff );
  }

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

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

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

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

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

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

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

  cout << ok_prefix << "Read object data with map." << endl;
  {
    string istring = "[ bmap PropCylFieldConst bfield=36.9 ]\n";
    istring += "[ obj3 PropCyl bfield_map=@bmap dir=\"forward\" ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      const PropCylField* pmap;
      {
        string name = obstr.read_object();
        assert( name == "bmap" );
        ObjTable::get_object(name,pmap);
      }
      const PropCyl* pobj;
      {
        string name = obstr.read_object();
        assert( name == "obj3" );
        ObjTable::get_object(name,pobj);
      }
      assert( pobj != 0 );
      assert( pobj->get_type() == PropCyl::get_static_type() );
      assert( is_equal( pobj->get_bfield(), 0.0 ) );
      assert( pobj->get_map() == pmap );
      assert( pobj->get_direction() == Propagator::FORWARD );
    }
  }

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

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

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

}