// AddFitCyl_Phi_Phi_Phi_t.cpp

// Test AddFitCyl_Phi_Phi_Phi.

#include "AddFitCyl_Phi_Phi_Phi.h"
#include <iostream>
#include <string>
#include <cassert>
#include <cmath>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfutil/trfstream.h"
#include "trfutil/TRFMath.h"
#include "trfbase/HitTest.h"
#include "trffit/HTrack.h"
#include "trffit/AddFitterPtr.h"
#include "HitCylPhi.h"
#include "AddFitterTest.h"
#include "trfcyl/PropCyl.h"
#include "trfbase/PropagatorPtr.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;

// Assign track parameter indices.
enum {
  IPHI = SurfCylinder::IPHI,
  IZ   = SurfCylinder::IZ,
  IALF = SurfCylinder::IALF,
  ITLM = SurfCylinder::ITLM,
  IQPT = SurfCylinder::IQPT
};

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

int main( ) {

  string component = "AddFitCyl_Phi_Phi_Phi";
  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_Phi_Phi_Phi>();

  cout << trf_format;
  cout << ok_prefix << "Test constructor." << endl;
  AddFitTest ftest(1);
  AddFitTest2 ftest2;
  AddFitCyl_Phi_Phi_Phi fit(2.0,ftest2);
  assert( fit.get_fit_order() == AddFitCyl_Phi_Phi_Phi::UNCHANGED );

  TrackError testerr;
  testerr(0,0)=100.;testerr(4,4)=10.;
  PropagatorPtr pprop ( new PropCyl(ObjDoublePtr(new ObjDouble(-2.))));
  SurfacePtr testpsrf(new SurfCylinder(10.));
  AddFitCyl_Phi_Phi_Phi fit1(2.0,ftest2,&*pprop,testpsrf,testerr,AddFitCyl_Phi_Phi_Phi::OUTWARD);
  assert( fit1.get_error() == testerr );
  assert( &*pprop == fit1.get_propagator() );
  assert( fit1.get_surface() == testpsrf);
  assert( fit1.get_fit_order() == AddFitCyl_Phi_Phi_Phi::OUTWARD );
  AddFitCyl_Phi_Phi_Phi fit2(2.0,ftest2,&*pprop,testpsrf,testerr,AddFitCyl_Phi_Phi_Phi::INWARD);
  assert( fit2.get_error() == testerr );
  assert( &*pprop == fit2.get_propagator() );
  assert( fit2.get_surface() == testpsrf);
  assert( fit2.get_fit_order() == AddFitCyl_Phi_Phi_Phi::INWARD );
  AddFitCyl_Phi_Phi_Phi fit3(2.0,ftest2,&*pprop,testpsrf,testerr,AddFitCyl_Phi_Phi_Phi::UNCHANGED);
  assert( fit3.get_fit_order() == AddFitCyl_Phi_Phi_Phi::UNCHANGED );
  
  AddFitCyl_Phi_Phi_Phi fit4(2.0,ftest2);
  assert( fit4.get_error() != testerr );
  assert( 0 == fit4.get_propagator() );
  assert( fit4.get_surface() == 0 );
  assert( fit4.get_fit_order() == AddFitCyl_Phi_Phi_Phi::UNCHANGED );

  cout << fit << endl;

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

  cout << ok_prefix << "Build 1st hit and track." << endl;
  double r1 = 30.0;
  ETrack tre( SurfacePtr(new SurfCylinder(r1)) );
  HTrack trh(tre);
  double phi1 = 1.234;
  double dphi = 0.00567;
  ClusterPtr pclus1 = new ClusCylPhi(r1,phi1,dphi);
  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 );
  cout << "Track phi - hit phi: " 
       << trh.get_track().get_vector()(IPHI) - phi1 << endl;
  assert( trh.get_track().get_vector()(IPHI) == phi1 );

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

  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 << "Build 2nd hit and track." << endl;
  double r2 = 20.0;
  double phi2 = 1.235;
  ClusterPtr pclus2 = new ClusCylPhi(r2,phi2,dphi);
  HitList hits2 = pclus2->predict(tre);
  HitPtr phit2 = hits2.front();
  cout << *phit2 << endl;
  cout << trh << endl;

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

  cout << ok_prefix << "Check 2nd hit fails." << endl;
  stat = fit.add_hit(trh,phit2);
  cout << stat << endl;
  assert( stat );
  assert( trh.get_hits().size() == 1 );

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

  cout << ok_prefix << "Add 2nd hit with dummy fitter." << endl;
  stat = ftest.add_hit(trh,phit2);
  assert( stat == 0 );
  assert( trh.get_hits().size() == 2 );
  cout << "Track phi - hit phi: "
       << trh.get_track().get_vector()(IPHI) - phi1 << endl;
  assert( trh.get_track().get_vector()(IPHI) == phi2 );

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

  cout << ok_prefix << "Add third hit." << endl;
  double r3 = 10.0;
  double phi3 = 1.236;
  ClusterPtr pclus3 = new ClusCylPhi(r3,phi3,dphi);
  tre = trh.get_track();
  HitList hits3 = pclus3->predict(tre);
  HitPtr phit3 = hits3.front();
  cout << *phit3 << endl;
  stat = fit.add_hit(trh,phit3);
  cout << trh << endl;
  cout << stat << endl;
  assert( stat == 0 );
  assert( trh.get_hits().size() == 3 );
  cout << "Track phi - hit phi: " 
       << trh.get_track().get_vector()(0) - phi2 << endl;

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

  cout << ok_prefix << "Try to add 4th 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(3) );
  {
    ObjPtr pobj( new AddFitCyl_Phi_Phi_Phi(8.9,*pfit) );
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("myfit",pfit);
    objstream.write_object("mysurf",testpsrf);
    objstream.write_object("myprop",pprop);
    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_Phi_Phi_Phi ",pos))
            != string::npos );
    assert( (pos=mystream.str().find("nsigma",pos)) != string::npos );
    assert( (pos=mystream.str().find("8.9 ",pos)) != string::npos );
    assert( (pos=mystream.str().find("fitter",pos)) != string::npos );
    assert( (pos=mystream.str().find("myfit ",pos)) != string::npos );
  }

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

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ obj2 AddFitCyl_Phi_Phi_Phi";
    istring += "\n  nsigma=7.7";
    istring += "\n  fitter=*myfit";
    istring += "\n]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const AddFitCyl_Phi_Phi_Phi* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() ==
              AddFitCyl_Phi_Phi_Phi::get_static_type() );
      assert( pobj->get_nsigma() == 7.7 );
      assert( &pobj->get_fitter() == pfit );
    }
  }

  {
    string istring = "[ obj3 AddFitCyl_Phi_Phi_Phi";
    istring += "\n  nsigma=7.7";
    istring += "\n  fitter=*myfit";
    istring += "\n  surface=@mysurf";
    istring += "\n  propagator=*myprop";
    istring += "\n  error_matrix=double( 100.0 0.0   0.0  0.0   0.0   0.0 0.0   0.0   0.0   0.0  0.0   0.0   0.0   0.0  10.0 )";
    istring += "\n  fit_order=\"OUTWARD\"";
    istring += "\n]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj3" );
      const AddFitCyl_Phi_Phi_Phi* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() ==
              AddFitCyl_Phi_Phi_Phi::get_static_type() );
      assert( pobj->get_nsigma() == 7.7 );
      assert( &pobj->get_fitter() == pfit );
      assert( pobj->get_propagator() == &*pprop );
      assert( pobj->get_surface() == testpsrf );
      assert( pobj->get_error() == testerr );
      
    }
  }


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

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

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

}