// AddFitZPlane_Z2_Z2_t.cpp

// Test AddFitZPlane_Z2_Z2.

#include "AddFitZPlane_Z2_Z2.h"
#include <iostream>
#include <string>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trffit/HTrack.h"
#include "HitZPlane2.h"
#include "trfbase/HitTest.h"
#include <cmath>
#include "trfutil/TRFMath.h"
#include "SurfZPlane.h"
#include "PropZZ.h"
#include "trfutil/trfstream.h"

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

using namespace trf;

// Assign track parameter indices.
enum {
  IX    = SurfZPlane::IX,
  IY    = SurfZPlane::IY,
  IDXDZ = SurfZPlane::IDXDZ,
  IDYDZ = SurfZPlane::IDYDZ,
  IQP   = SurfZPlane::IQP
};

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

// Dummy AddFitter subclass.
// It sets the first two track parameters equal to the first two hit parameters
// for the first hit and does nothing for any other hits.
namespace {

// Creator.

ObjPtr create(const ObjData& data) ;
ObjPtr create1(const ObjData& data) ;

}

class AddFitZTest : public AddFitter {
private:
  int _param;
  void ostr(ostream& stream) const {
    stream<<begin_object << "AddFitTest with parameter " << _param << end_object;
  };
public:
  // Return the type name.
  static TypeName get_type_name() { return "AddFitZTest"; }

  // Return the creator.
  static ObjCreator get_creator() {return create;};

  // Return the type.
  static Type get_static_type() { return get_creator(); }

  // Return the type.
  Type get_type() const { return get_static_type(); }

  // Write the object data.
  ObjData write_data() const { 
    ObjData data( get_type_name() );
    data.add_double( "param", _param );
    return data;};

  AddFitZTest(int param) : _param(param) { };
  int fit(HTrack& trh) const { return _param; };
  int add_hit(HTrack& trh, const HitPtr& phit) const { 
    if ( trh.get_hits().size() ) return 0;
    ETrack tre = trh.get_track();
    TrackVector vec = tre.get_vector();
    vec(IX) = phit->measured_vector()(0);
    vec(IY) = phit->measured_vector()(1);
    tre.set_vector(vec);
    trh.set_fit(tre,0.0);
    trh.add_hit(phit);
    return 0;
  };
};

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
 return ObjPtr( new AddFitZTest(data.get_int("param")) );
}

}

class AddFitZTest2 : public AddFitter {

public:

  // Return the type name.
  static TypeName get_type_name() { return "AddFitZTest2"; }

  // Return the creator.
  static ObjCreator get_creator() {return create1;};

  // Return the type.
  static Type get_static_type() { return get_creator(); }

  // Return the type.
  Type get_type() const { return get_static_type(); }

  // Write the object data.
  ObjData write_data() const {return ObjData( get_type_name() );};

private:

  void ostr(ostream& stream) const {
    cout << "AddFitZTest2 with parameter " ;
  };

public:

  int add_hit(HTrack& trh, const HitPtr& phit) const {
    trh.add_hit(phit);
    return 0;
  };
 int add_hit_fit(ETrack& tre, double& chisq, const HitPtr& phit) const
  {
   return 0;
  };
};

namespace {

// Creator.

ObjPtr create1(const ObjData& data) {
 return ObjPtr( new AddFitZTest2 );
}

}

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

int main( ) {

  string component = "AddFitZPlane_Z2_Z2";
  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;
  }


  //**********************************************************************
  // Create propagator, surface and error matrix;
  cout << trf_format;

  SurfZPlane sz(15.);
  TrackError err;
  err(0,0)=100.;
  err(1,1)=100.;
  err(2,2)=100.;
  err(3,3)=100.;
  err(4,4)=1.;
  PropZZ prop(ObjDoublePtr(new ObjDouble(-2.)));
  

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

  cout << ok_prefix << "Enable tuple." << endl;
  assert( ! AddFitZPlane_Z2_Z2::tuple_is_enabled() );
  AddFitZPlane_Z2_Z2::enable_tuple();
  assert( AddFitZPlane_Z2_Z2::tuple_is_enabled() );
  AddFitZPlane_Z2_Z2::disable_tuple();
  assert( ! AddFitZPlane_Z2_Z2::tuple_is_enabled() );
  AddFitZPlane_Z2_Z2::enable_tuple();
  assert( AddFitZPlane_Z2_Z2::tuple_is_enabled() );

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

  cout << ok_prefix << "Test constructor." << endl;
  AddFitZTest ftest(1);
  AddFitZTest2 ftest2;
  double bfield = -2.0;
  AddFitZPlane_Z2_Z2 fit(ObjDoublePtr(new ObjDouble(bfield)),AddFitterPtr(new AddFitZTest2),PropagatorPtr(prop.new_propagator()),SurfacePtr(sz.new_surface()),err,0.002);
  cout << fit << endl;

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

  cout << ok_prefix << "Build 1st hit and track." << endl;
  double zpos1 = 10.;
  SurfZPlane srf1(zpos1);
  TrackVector vec;
  vec.fill(0.);
  vec(4)=0.001;
  ETrack tre(SurfacePtr(srf1.new_pure_surface()));
  tre.set_vector(vec);
  tre.set_forward();

  HTrack trh(tre);
  double hitx1 = 11.;
  double hity1 = 12.;
  HitError dhm(2);
  dhm(ClusZPlane2::IX,ClusZPlane2::IX) = 0.022;
  dhm(ClusZPlane2::IX,ClusZPlane2::IY) = 0.012;
  dhm(ClusZPlane2::IY,ClusZPlane2::IY) = 0.032;
  ClusterPtr pclus1 = new ClusZPlane2(zpos1,hitx1,hity1,dhm);
  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 v - hit v: " 
       << trh.get_track().get_vector()(IX) - hitx1 << endl;
  cout << "Track z - hit z: " 
       << trh.get_track().get_vector()(IY) - hity1 << endl;
  assert( is_equal( trh.get_track().get_vector()(IX), hitx1 ) );
  assert( is_equal( trh.get_track().get_vector()(IY), hity1 ) );

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

  cout << ok_prefix << "Try to add 2nd hit of wrong type." << endl;
  const SurfTest *psrf = (const SurfTest *)&srf1;
  ClusterPtr pclusw = new ClusterTest(*psrf,1);
  HitList hitsw = pclusw->predict(tre);
  HitPtr phitw = hitsw.front(); 

  //HitPtr phitw = new HitTest(1);
  cout << *phitw << endl;
  
  stat = fit.add_hit(trh,phitw);
  cout << stat << endl;
  assert( stat );
  assert( stat == AddFitZPlane_Z2_Z2::WRONG_TYPE );
  assert( trh.get_hits().size() == 1 );

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

  cout << ok_prefix << "Try to add 2nd hit of wrong surface type." << endl;
 
  double zpos2 = 5.;
  SurfZPlane srf2(zpos2);

  double hitx2 = 6.;
  double hity2 = 9.;
  ClusterPtr pclus2 = new ClusZPlane2(zpos2,hitx2,hity2,dhm);
  HitList hits2 = pclus2->predict(tre);
  HitPtr phit2 = hits2.front();
  cout << *phit2 << endl;
  tre = trh.get_track();
  cout << tre.get_surface() << endl;
  stat = fit.add_hit(trh,phit2);
  cout << stat << endl;
  assert( stat );
  assert( stat == AddFitZPlane_Z2_Z2::DIFFERENT_SURFACES);
  assert( trh.get_hits().size() == 1 );
  //********************************************************************

  cout << ok_prefix << "Add second hit." << endl;

  cout << *phit2 << endl;
  tre = trh.get_track();
  tre.set_surface(SurfacePtr(srf2.new_pure_surface()));
  trh.set_fit(tre,0.0);

  double qp_old = trh.get_track().get_vector()(IQP);

  stat = fit.add_hit(trh,phit2);
  cout << trh << endl;
  cout << stat << endl;
  assert( stat == 0 );
  assert( trh.get_hits().size() == 2 );

  cout << "Track v - hit v: " 
       << trh.get_track().get_vector()(IX) - hitx2 << endl;
  cout << "Track z - hit z: " 
       << trh.get_track().get_vector()(IY) - hity2 << endl;
  assert( fabs(trh.get_track().get_vector()(IX) - hitx2) <1.e-6 );
  assert( fabs(trh.get_track().get_vector()(IY) - hity2) <1.e-6 );

  // Check that dx/dz = delta_x/delta_z, dy/dz = delta_y/delta_z
  
  double dxdz2 = (hitx2 - hitx1)/(zpos2-zpos1);
  double dydz2 = (hity2 - hity1)/(zpos2-zpos1);

  cout << " dx/dz check is: "<<dxdz2<<" dy/dz check is: "<<dydz2<<endl;
  cout << " dx/dz track is: "<<trh.get_track().get_vector()(IDXDZ)
        <<" dy/dz track is: "<<trh.get_track().get_vector()(IDYDZ)
              <<endl;
  cout << " q/p check is: "<<qp_old;
  cout << " q/p track is: "<<trh.get_track().get_vector()(IQP)<<endl;
  double close = 1.e-7; 
  assert( fabs(trh.get_track().get_vector()(IDXDZ) - dxdz2)<close );
  assert( fabs(trh.get_track().get_vector()(IDYDZ) - dydz2)<close );
  assert( is_equal( trh.get_track().get_vector()(IQP), qp_old ) );
  assert(trh.get_track().is_forward());
  //********************************************************************

  cout << ok_prefix << "Try to add third hit." << endl;
  stat = fit.add_hit(trh,phit2);
  cout << stat << endl;
  assert( stat );
  assert( stat == AddFitZPlane_Z2_Z2::WRONG_HIT_COUNT );
  assert( trh.get_hits().size() == 2 );

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

  ObjTable::register_type<AddFitZTest>();
  ObjTable::register_type<AddFitZTest2>();
  ObjTable::register_type<AddFitZPlane_Z2_Z2>();
  ObjTable::register_type<SurfZPlane>();
  ObjTable::register_type<PropZZ>();
  ObjTable::register_type<ObjDouble>();

  cout << ok_prefix << "Write object data." << endl;
  AddFitterPtr pfit( new AddFitZTest(3) );
  SurfacePtr psrft(new SurfZPlane(12.0));
  PropagatorPtr propt(new PropZZ(ObjDoublePtr(new ObjDouble(-2.))));
  {
    err(0,0)=1;
    err(0,1)=2;err(1,1)=3;
    err(0,2)=4;err(1,2)=5;err(2,2)=6;
    err(0,3)=7;err(1,3)=8;err(2,3)=9;err(3,3)=10;
    err(0,4)=11;err(1,4)=12;err(2,4)=13;err(3,4)=14;err(4,4)=15;
    ObjPtr pobj( new AddFitZPlane_Z2_Z2(ObjDoublePtr(new ObjDouble(8.9)),pfit,propt,psrft,err,0.5,0.1,0.2) ) ;
    ObjDoublePtr pfield(new ObjDouble(7.7)); 
    ObjDoublePtr pptmin(new ObjDouble(0.6)); 
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("myfit",pfit);
    objstream.write_object("mysurf",psrft);
    objstream.write_object("myprop",propt);
    objstream.write_object("bfield",pfield);
    objstream.write_object("ptmin",pptmin);
    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("AddFitZPlane_Z2_Z2 ",pos))
            != string::npos );
    assert( (pos=mystream.str().find("bfield",pos)) != string::npos );
    assert( (pos=mystream.str().find("8.9 ",pos)) != string::npos );
    assert( (pos=mystream.str().find("ptmin",pos)) != string::npos );
    assert( (pos=mystream.str().find("0.5 ",pos)) != string::npos );
    assert( (pos=mystream.str().find("vx",pos)) != string::npos );
    assert( (pos=mystream.str().find("0.1 ",pos)) != string::npos );
    assert( (pos=mystream.str().find("vy",pos)) != string::npos );
    assert( (pos=mystream.str().find("0.2 ",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("fitter",pos)) != string::npos );
    assert( (pos=mystream.str().find("myfit ",pos)) != string::npos );
    assert( (pos=mystream.str().find("surface",pos)) != string::npos );
    assert( (pos=mystream.str().find("mysurf ",pos)) != string::npos );
    assert( (pos=mystream.str().find("error",pos)) != string::npos );
    assert( (pos=mystream.str().find("1",pos)) != string::npos );
    assert( (pos=mystream.str().find("2",pos)) != string::npos );
    assert( (pos=mystream.str().find("3",pos)) != string::npos );
    assert( (pos=mystream.str().find("4",pos)) != string::npos );
    assert( (pos=mystream.str().find("5",pos)) != string::npos );
    assert( (pos=mystream.str().find("6",pos)) != string::npos );
    assert( (pos=mystream.str().find("7",pos)) != string::npos );
    assert( (pos=mystream.str().find("8",pos)) != string::npos );
    assert( (pos=mystream.str().find("9",pos)) != string::npos );
    assert( (pos=mystream.str().find("10",pos)) != string::npos );
    assert( (pos=mystream.str().find("11",pos)) != string::npos );
    assert( (pos=mystream.str().find("12",pos)) != string::npos );
    assert( (pos=mystream.str().find("13",pos)) != string::npos );
    assert( (pos=mystream.str().find("14",pos)) != string::npos );
    assert( (pos=mystream.str().find("15",pos)) != string::npos );
  }

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

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ obj2 AddFitZPlane_Z2_Z2";
    istring += "\n  bfield=@bfield";
    istring += "\n  ptmin=@ptmin";
    istring += "\n  vx=0.1";
    istring += "\n  vy=0.2";
    istring += "\n  fitter=@myfit";
    istring += "\n  propagator=@myprop";
    istring += "\n  surface=@mysurf";
    istring += "\n  error=double( 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 11.0 12.0 13.0 14.0 15.0 )";
    istring += "\n]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const AddFitZPlane_Z2_Z2* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() ==
              AddFitZPlane_Z2_Z2::get_static_type() );
      assert( (pobj->get_fitter()) == pfit );
      assert( (pobj->get_surface()) == psrft );
      assert( (pobj->get_propagator()) == propt );
      assert( fabs(pobj->get_bfield() - 7.7) < 1.e-10);
      assert( fabs(pobj->get_ptmin() - 0.6) < 1.e-10);
      assert( fabs(pobj->get_vx() - 0.1) < 1.e-10);
      assert( fabs(pobj->get_vy() - 0.2) < 1.e-10);
      const TrackError& error=pobj->get_error();
      cout<<error<<endl;
      assert(error(0,0)==1);
      assert(error(0,1)==2&&error(1,1)==3);
      assert(error(0,2)==4&&error(1,2)==5&&error(2,2)==6);
      assert(error(0,3)==7&&error(1,3)==8&&error(2,3)==9&&error(3,3)==10);
      assert(error(0,4)==11&&error(1,4)==12&&error(2,4)==13&&error(3,4)==14&&error(4,4)==15);
    }
  }

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

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

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