// AddFitCyl_PhiZ_PhiZ_t.cpp

// Test AddFitCyl_PhiZ_PhiZ.

#include "AddFitCyl_PhiZ_PhiZ.h"
#include <string>
#include <iostream>
#include <cassert>
#include <cmath>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfutil/trfstream.h"
#include "trfbase/PropDirectedTest.h"
#include "trfbase/PropStat.h"
#include "trfbase/TrackVector.h"
#include "trfbase/HitTest.h"
#include "trffit/HTrack.h"
#include "trffit/AddFitterPtr.h"
#include "SurfCylinder.h"
#include "HitCylPhi.h"
#include "HitCylPhiZ.h"
#include "AddFitCyl_Phi_Phi.h"
#include "AddFitterTest.h"

using std::cout;
using std::cerr;
using std::endl;
using std::string;
using std::ostringstream;
using std::istringstream;
#ifndef DEFECT_CMATH_NOT_STD
using std::fabs;
#endif

using namespace trf;

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

int main( ) {

  string component = "AddFitCyl_PhiZ_PhiZ";
  string ok_prefix = component + " (I): ";
  string error_prefix = component + " test (E): ";

  cout << ok_prefix
       << "-------- Testing component " + component
          + ". --------" << 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<AddFitTest>();
  ObjTable::register_type<AddFitTest2>();
  ObjTable::register_type<AddFitCyl_PhiZ_PhiZ>();

  cout << trf_format;
  cout << ok_prefix << "Test constructor." << endl;
  AddFitTest ftest(1);
  PropTest ptest;
  SurfacePtr pscy( new SurfCylinder(10.) );
  TrackError err;
  err(0,0) = 0.01;
  err(1,1) = 0.02;
  err(2,2) = 0.03;
  err(3,3) = 0.04;
  err(4,4) = 0.05;
  AddFitCyl_PhiZ_PhiZ fit(ObjDoublePtr(new ObjDouble(-2.0)),
			  ftest,ptest,pscy,err,-1000,1000);
  cout << fit << endl;
  assert( fit.get_fit_order() == AddFitCyl_PhiZ_PhiZ::UNCHANGED ); 

  AddFitCyl_PhiZ_PhiZ::FlagState flag= AddFitCyl_PhiZ_PhiZ::DR_ALPHA;

  AddFitCyl_PhiZ_PhiZ fit1(ObjDoublePtr(new ObjDouble(-2.0)),
					ftest,ptest,pscy,err,-1000.,1000.,flag, AddFitCyl_PhiZ_PhiZ::OUTWARD);
  assert( fit1.get_fit_order() == AddFitCyl_PhiZ_PhiZ::OUTWARD ); 
  AddFitCyl_PhiZ_PhiZ fit2(ObjDoublePtr(new ObjDouble(-2.0)),
					ftest,ptest,pscy,err,-1000.,1000.,flag, AddFitCyl_PhiZ_PhiZ::INWARD);
  assert( fit2.get_fit_order() == AddFitCyl_PhiZ_PhiZ::INWARD ); 

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

  cout << ok_prefix << "Build starting track and track." << endl;
  double r1 = 20.0;
  ETrack tre( SurfacePtr(new SurfCylinder(r1)) );
  tre.set_error(err);
  HTrack trh(tre);

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

  cout << ok_prefix << "Build 1st hit and track." << endl;
  double stereo1 = -0.01;
  double phi1 = 1.234;
  double z1 = 45.67;
  double phiz1 = phi1 + stereo1*z1;
  double dphiz1 = 0.00567;
  ClusterPtr pclus1 = new ClusCylPhiZ(r1,phiz1,dphiz1,stereo1);
  HitList hits1 = pclus1->predict(tre);
  HitPtr phit1 = hits1.front();
  cout << *phit1 << endl;
  cout << trh << endl;

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

  cout << ok_prefix << "Check 1st hit fails." << endl;
  int stat = fit.add_hit(trh,phit1);
  cout << stat << endl;
  assert( stat );
  assert( trh.get_hits().size() == 0 );

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

  cout << ok_prefix << "Add first hit with dummy fitter." << endl;
  stat = ftest.add_hit(trh,phit1);
  assert( stat == 0 );
  assert( trh.get_hits().size() == 1 );
  // Generate hits for the new track.
  HitPtr phit1f = pclus1->predict( trh.get_track() ).front();
  cout << *phit1f << endl;
  cout << "phiz1 = " << phiz1 << endl;
  cout << "Track phiz - hit phiz: " 
       << phit1f->predicted_vector()(0) - phiz1 << endl;
  assert( phit1f->predicted_vector()(0) == phiz1 );

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

  cout << ok_prefix << "Try to add 2nd hit of wrong type." << endl;
  HitPtr phitw( new HitTest(1) );
  stat = fit.add_hit(trh,phitw);
  cout << stat << endl;
  assert( stat );
  assert( trh.get_hits().size() == 1 );

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

  cout << ok_prefix << "Add second hit (1st phi)." << endl;
  double r2 = 10.0;
  SurfCylinder srf2(r2);
  double phi2 = 1.345;
  double dphi2 = 0.0246;
  ClusterPtr pclus2 = new ClusCylPhi(r2,phi2,dphi2);
  HitList hits2 = pclus2->predict(tre);
  HitPtr phit2 = hits2.front();
  cout << *phit2 << endl;
  PropStat pstat = trh.propagate(ptest,srf2);
  assert( pstat.success() );
  stat = ftest.add_hit(trh,phit2);
  cout << trh << endl;
  cout << stat << endl;
  assert( stat == 0 );
  assert( trh.get_hits().size() == 2 );
  cout << "Track phi - hit phi: " 
       << trh.get_track().get_vector()(0) - phi2 << endl;
  double close = 1.e-8;
  assert( fabs( trh.get_track().get_vector()(0) - phi2 ) < close );

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

  cout << ok_prefix << "Add third hit (2nd phiz)." << endl;
  double stereo3 = -0.02;
  double r3 = r2;
  SurfCylinder srf3(r3);
  double phi3 = phi2;
  double z3 = -23.45;
  double phiz3 = phi3 + stereo3*z3;
  double dphiz3 = 0.00456;
  ClusterPtr pclus3 = new ClusCylPhiZ(r3,phiz3,dphiz3,stereo3);
  HitList hits3 = pclus3->predict(tre);
  HitPtr phit3 = hits3.front();
  cout << *phit3 << endl;
  pstat = trh.propagate(ptest,srf3);
  assert( pstat.success() );
  cout << trh << endl;
  stat = fit.add_hit(trh,phit3);
  cout << trh << endl;
  cout << stat << endl;
  assert( stat == 0 );
  assert( trh.get_hits().size() == 3 );

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

  cout << ok_prefix << "Try to add third phiz hit." << endl;
  stat = fit.add_hit(trh,phit1);
  cout << stat << endl;
  assert( stat );
  assert( trh.get_hits().size() == 3 );

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

  cout << ok_prefix << "Write object data." << endl;
  AddFitterPtr pfit( new AddFitTest(1) );
  PropagatorPtr pprop( new PropTest );
  SurfacePtr psrf( new SurfCylinder(30.0) );
  TrackError terr;
  terr(0,0) = .0100;
  terr(0,1) = .0101;
  terr(1,1) = .0111;
  terr(0,2) = .0102;
  terr(1,2) = .0112;
  terr(2,2) = .0122;
  terr(0,3) = .0103;
  terr(1,3) = .0113;
  terr(2,3) = .0123;
  terr(3,3) = .0133;
  terr(0,4) = .0104;
  terr(1,4) = .0114;
  terr(2,4) = .0124;
  terr(3,4) = .0134;
  terr(4,4) = .0144;
  {
    AddFitCyl_PhiZ_PhiZ::FlagState flag = AddFitCyl_PhiZ_PhiZ::DR_ONLY;
    ObjPtr pobj( 
      new AddFitCyl_PhiZ_PhiZ(ObjDoublePtr(new ObjDouble(-2.2)),
			      *pfit,*pprop,psrf,terr, 20, 40, flag) );
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("myfitter",pfit);
    objstream.write_object("myprop",pprop);
    objstream.write_object("mysurf",psrf);
    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( "AddFitCyl_PhiZ_PhiZ ",pos)) !=
            string::npos );
    assert( (pos=mystream.str().find( "bfield",pos)) != string::npos );
    assert( (pos=mystream.str().find( "2.2 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "fitter",pos)) != string::npos );
    assert( (pos=mystream.str().find( "myfitter ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "propagator",pos)) != string::npos );
    assert( (pos=mystream.str().find( "myprop ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "surface",pos)) != string::npos );
    assert( (pos=mystream.str().find( "mysurf ",pos)) != string::npos );
#ifdef ObjData_supports_lists
    assert( (pos=mystream.str().find( "error_matrix",pos)) != string::npos );
    assert( (pos=mystream.str().find( ".01",pos)) != string::npos );
    assert( (pos=mystream.str().find( ".0101 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( ".0111 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( ".0102 ",pos)) != string::npos );
#endif
    assert( (pos=mystream.str().find( "z0min",pos)) != string::npos );
    assert( (pos=mystream.str().find( "z0max",pos)) != string::npos );
    assert( (pos=mystream.str().find( "flag",pos)) != string::npos );
    assert( (pos=mystream.str().find( "\"DR_ONLY\"",pos)) != string::npos );
  }

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

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ obj2 AddFitCyl_PhiZ_PhiZ";
    istring += "\n  bfield=2.1";
    istring += "\n  fitter=*myfitter";
    istring += "\n  propagator=*myprop";
    istring += "\n  surface=@mysurf";
#ifdef ObjData_supports_lists
    istring += "\n  error_matrix=double(";
    istring += "\n    .0100";
    istring += "\n    .0101 .0111";
    istring += "\n    .0102 .0112 .0122";
    istring += "\n    .0103 .0113 .0123 .0133";
    istring += "\n    .0104 .0114 .0124 .0134 .0144";
    istring += "\n  )";
#endif
    istring += "\n  z0max=200.0";
    istring += "\n  flag=\"DR_ALPHA\"";
    istring += "\n]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const AddFitCyl_PhiZ_PhiZ* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() == AddFitCyl_PhiZ_PhiZ::get_static_type() );
      assert( pobj->get_bfield() == 2.1 );
      assert( &pobj->get_fitter() == pfit );
      assert( &pobj->get_propagator() == pprop );
      assert( pobj->get_surface() == psrf );
#ifdef ObjData_supports_lists
      assert( pobj->get_error() == terr );
#endif
      assert( pobj->get_z0min() == -200);
      assert( pobj->get_z0max() == 200);
      assert( pobj->get_flag() == AddFitCyl_PhiZ_PhiZ::DR_ALPHA );
      assert( pobj->get_fit_order() == AddFitCyl_PhiZ_PhiZ::UNCHANGED );
    }
  }

  {
    string istring = "[ obj3 AddFitCyl_PhiZ_PhiZ";
    istring += "\n  bfield=2.1";
    istring += "\n  fitter=*myfitter";
    istring += "\n  propagator=*myprop";
    istring += "\n  surface=@mysurf";
#ifdef ObjData_supports_lists
    istring += "\n  error_matrix=double(";
    istring += "\n    .0100";
    istring += "\n    .0101 .0111";
    istring += "\n    .0102 .0112 .0122";
    istring += "\n    .0103 .0113 .0123 .0133";
    istring += "\n    .0104 .0114 .0124 .0134 .0144";
    istring += "\n  )";
#endif
    istring += "\n  z0max=200.0";
    istring += "\n  flag=\"DR_ALPHA\"";
    istring += "\n  fit_order=\"INWARD\"";
    istring += "\n]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj3" );
      const AddFitCyl_PhiZ_PhiZ* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() == AddFitCyl_PhiZ_PhiZ::get_static_type() );
      assert( pobj->get_bfield() == 2.1 );
      assert( &pobj->get_fitter() == pfit );
      assert( &pobj->get_propagator() == pprop );
      assert( pobj->get_surface() == psrf );
#ifdef ObjData_supports_lists
      assert( pobj->get_error() == terr );
#endif
      assert( pobj->get_z0min() == -200);
      assert( pobj->get_z0max() == 200);
      assert( pobj->get_flag() == AddFitCyl_PhiZ_PhiZ::DR_ALPHA );
      assert( pobj->get_fit_order() == AddFitCyl_PhiZ_PhiZ::INWARD );
    }
  }

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

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

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

}