// AddFitZPlane_XY2_Z2_t.cpp

// Test AddFitZPlane_XY2_Z2.

#include "AddFitZPlane_XY2_Z2.h"
#include <iostream>
#include <string>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "objstream/ObjData.hpp"
#include "trffit/HTrack.h"
#include "trfzp/HitZPlane2.h"
#include "trfxyp/HitXYPlane2.h"
#include <cmath>
#include "trfutil/TRFMath.h"
#include "trfxyp/SurfXYPlane.h"
#include "trfzp/SurfZPlane.h"
#include "trfxyp/PropXYXY.h"
#include "trfzp/PropZZ.h"
#include "PropZXY.h"
#include "PropXYZ.h"
#include "trfbase/PropDispatch.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;
using namespace trf;
#endif

// Assign track parameter indices.

enum {
  IX     = SurfZPlane::IX,
  IY     = SurfZPlane::IY,
  IDXDZ  = SurfZPlane::IDXDZ,
  IDYDZ  = SurfZPlane::IDYDZ,
  IQP_Z  = SurfZPlane::IQP
};

enum {
  IV     = SurfXYPlane::IV,
  IZ     = SurfXYPlane::IZ,
  IDVDU  = SurfXYPlane::IDVDU,
  IDZDU  = SurfXYPlane::IDZDU,
  IQP_XY = SurfXYPlane::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 AddFitXYZTest : public AddFitter {
private:
  int _param;
  void ostr(ostream& stream) const {
    cout << "AddFitXYZTest with parameter " << _param;
  };
public:
  // Return the type name.
  static TypeName get_type_name() { return "AddFitXYZTest"; }

  // 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;};
public:
  AddFitXYZTest(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 AddFitXYZTest(data.get_int("param")) );
}

}

class AddFitXYZTest2 : public AddFitter {
public:
  // Return the type name.
  static TypeName get_type_name() { return "AddFitXYZTest2"; }

  // 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 << "AddFitXYZTest2 " ;
  };
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 AddFitXYZTest2 );
}

}

Propagator& prop_create(double bfield)
{
  Propagator* _propxyxy = new PropXYXY(ObjDoublePtr(new ObjDouble(-bfield)));
  Propagator* _propzz = new PropZZ(ObjDoublePtr(new ObjDouble(-bfield)));
  Propagator* _propxyz = new PropXYZ(ObjDoublePtr(new ObjDouble(-bfield)));
  Propagator* _propzxy = new PropZXY(ObjDoublePtr(new ObjDouble(-bfield)));
  PropDispatch* _propall = new PropDispatch;

  TrfObject::Type stypexy= SurfXYPlane::get_static_type();
  TrfObject::Type stypez= SurfZPlane::get_static_type();

  _propall->add_propagator(stypez,stypez,PropagatorPtr(_propzz));
  _propall->add_propagator(stypexy,stypexy,PropagatorPtr(_propxyxy));


  _propall->add_propagator(stypez,stypexy,PropagatorPtr(_propzxy));
  _propall->add_propagator(stypexy,stypez,PropagatorPtr(_propxyz));

  return *_propall;

}

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

int main( ) {

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

  SurfXYPlane sxy(15.1,PI/5.);
  TrackError err;
  err(0,0)=100.;
  err(1,1)=100.;
  err(2,2)=100.;
  err(3,3)=100.;
  err(4,4)=1.;
  Propagator& prop=prop_create(2.);
  

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

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

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

  cout << ok_prefix << "Test constructor." << endl;
  AddFitXYZTest ftest(1);
  AddFitXYZTest2 ftest2;
  double bfield=2.;
  AddFitZPlane_XY2_Z2 fit(ObjDoublePtr(new ObjDouble(bfield)),AddFitterPtr(new AddFitXYZTest2),PropagatorPtr(prop.new_propagator()),SurfacePtr(sxy.new_surface()),err,0.0002);
  cout << fit << endl;

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

  cout << ok_prefix << "Build 1st hit and track." << endl;
  double dist1 = 0.11;
  double phi1  = PI/4.;
  SurfXYPlane srf1(dist1,phi1);
  TrackVector vec;
  vec.fill(0.);
  vec(4)=.001; 
  ETrack tre(SurfacePtr(srf1.new_pure_surface()));
  tre.set_vector(vec);
  tre.set_forward();

  HTrack trh(tre);
  double hitv1 = 0.01;
  double hitz1 = 12.;
  HitError dhm(2);
  dhm(ClusXYPlane2::IV,ClusXYPlane2::IV) = 0.022;
  dhm(ClusXYPlane2::IV,ClusXYPlane2::IZ) = 0.012;
  dhm(ClusXYPlane2::IZ,ClusXYPlane2::IZ) = 0.032;
  ClusterPtr pclus1 = new ClusXYPlane2(dist1,phi1,hitv1,hitz1,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()(IV) - hitv1 << endl;
  cout << "Track z - hit z: " 
       << trh.get_track().get_vector()(IZ) - hitz1 << endl;
  assert( trh.get_track().get_vector()(IV) == hitv1 );
  assert( trh.get_track().get_vector()(IZ) == hitz1 );

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

  cout << ok_prefix << "Try to add 2nd hit of wrong type." << endl;
  
  stat = fit.add_hit(trh,phit1);
  cout << stat << endl;
  assert( stat );
  assert( stat == AddFitZPlane_XY2_Z2::WRONG_TYPE );
  assert( trh.get_hits().size() == 1 );

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

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

  double hitx2 = 0.1;
  double hity2 = 0.1;
  dhm(ClusZPlane2::IX,ClusZPlane2::IX) = 0.022;
  dhm(ClusZPlane2::IX,ClusZPlane2::IY) = 0.012;
  dhm(ClusZPlane2::IY,ClusZPlane2::IY) = 0.032;
  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_XY2_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_Z);

  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 hitx1 = dist1*cos(phi1) - hitv1*sin(phi1);
  double hity1 = hitv1*cos(phi1) + dist1*sin(phi1);

  double dxdz2 = (hitx2 - hitx1)/(zpos2-hitz1);
  double dydz2 = (hity2 - hity1)/(zpos2-hitz1);

  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_Z)<<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( trh.get_track().get_vector()(IQP_Z) - qp_old == 0.);
  assert(trh.get_track().is_backward());
  //********************************************************************

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

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

  ObjTable::register_type<AddFitXYZTest>();
  ObjTable::register_type<AddFitXYZTest2>();
  ObjTable::register_type<AddFitZPlane_XY2_Z2>();
  ObjTable::register_type<SurfZPlane>();
  ObjTable::register_type<SurfXYPlane>();
  ObjTable::register_type<PropXYZ>();
  ObjTable::register_type<PropZXY>();
  ObjTable::register_type<ObjDouble>();

  cout << ok_prefix << "Write object data." << endl;
  AddFitterPtr pfit( new AddFitXYZTest(3) );
  SurfacePtr psrft(new SurfXYPlane(12.0,0.1));
  ObjDoublePtr pfield(new ObjDouble(7.7));
  ObjDoublePtr pptmin(new ObjDouble(5.2));
  PropagatorPtr propt(new PropXYZ(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_XY2_Z2(ObjDoublePtr(new ObjDouble(8.9)),pfit,propt,psrft,err,0.5,0.1,0.2) );
    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_XY2_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_XY2_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_XY2_Z2* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      assert( pobj->get_type() ==
              AddFitZPlane_XY2_Z2::get_static_type() );
      assert( (pobj->get_fitter()) == pfit );
      assert( (pobj->get_surface()) == psrft );
      assert( (pobj->get_propagator()) == propt );
      assert( fabs(pobj->get_vx() - 0.1) < 1.e-10);
      assert( fabs(pobj->get_vy() - 0.2) < 1.e-10);
      assert( fabs(pobj->get_bfield() - 7.7) < 1.e-10);
      assert( fabs(pobj->get_ptmin() - 5.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;

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