// FullFitKalman_t.cpp

#include "FullFitKalman.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "objstream/StdObjStream.hpp"
#include "trfutil/trfstream.h"
#include "trfbase/PropStat.h"
#include "trfbase/PropDirected.h"
#include "trfbase/SurfTest.h"
#include "trfbase/ETrack.h"
#include "HitTestFit1.h"
#include "HTrack.h"
#include "FullFitterPtr.h"

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

using namespace trf;

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

ObjPtr create_PropTest(const ObjData& data);

class PropTest : public PropDirected {
public:  // static methods
  // Return the type name.
  static TypeName get_type_name() { return "PropTest"; }
  // Return the creator.
  static ObjCreator get_creator() { return create_PropTest; }
  // Return the type.
  static Type get_static_type() { return get_creator(); }
private:
  void ostr(ostream& stream) const { stream << "PropTest"; };
public:
  // Return the type.
  Type get_type() const { return get_static_type(); };
  // Return object data.
  ObjData write_data() const {
    return ObjData("PropTest");
  }
  PropStat
  vec_dir_prop(VTrack& trv,const Surface& srf, PropDir dir,
               TrackDerivative* pder =0) const {
    PropStat pstat;
    pstat.set_forward();
    return pstat;
  };
  PropStat
  err_dir_prop(ETrack& tre,const Surface& srf, PropDir dir,
               TrackDerivative* pder =0) const {
    PropStat pstat;
    pstat.set_forward();
    return pstat;
  };
  Propagator* new_propagator() const { return new PropTest(); };
};

// Creator.

ObjPtr create_PropTest(const ObjData& data) {
  assert( data.get_object_type() == "PropTest" );
  if ( data.get_object_type() != "PropTest" ) return ObjPtr(0);
  return ObjPtr( new PropTest );
}

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

int main( ) {

  string component = "FullFitKalman";
  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 starting surface and track.
  ObjTable::register_type<PropTest>();
  ObjTable::register_type<HitTestFit1>();
  ObjTable::register_type<ClusTestFit1>();
  ObjTable::register_type<FullFitKalman>();
  cout << trf_format;
  SurfTest stest(1);
  double tvec = 2.1;
  double terr = 0.014;
  TrackVector vec;
  TrackError err;
  vec(0) = tvec;
  err(0,0) = terr;
  err(1,1) = 0.000001;
  err(2,2) = 0.000001;
  err(3,3) = 0.000001;
  err(4,4) = 0.000001;
  ETrack tre( SurfacePtr(stest.new_pure_surface()), vec, err );
  WAvg avg;
  avg.add_pair(tvec,terr);

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

  cout << ok_prefix << "Create clusters and generate prediction."
       << endl;
  ClusterList clusters;
  int nmsmt = 5;
  double msmt[] = { 2.00, 2.33, 1.94, 2.22, 1.87 };
  double emsmt[] = { 0.01, 0.015, 0.016, 0.023, 0.012 };
  int i;
  for ( i=0; i<nmsmt; ++i ) {
    clusters.push_back( new ClusTestFit1(stest,msmt[i],emsmt[i]) );
    avg.add_pair(msmt[i],emsmt[i]);
  };
  cout << avg << endl;

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

  // Create hits.
  HitList hits;
  ClusterList::iterator iclu;
  for( iclu=clusters.begin(); iclu!=clusters.end(); ++iclu ) {
    HitList newhits = (*iclu)->predict(tre);
    hits.push_back( newhits.back() );
  }

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

  // Put hit on an HTrack.
  HTrack trh(tre);
  HitList::iterator ihit;
  for ( ihit=hits.begin(); ihit!=hits.end(); ++ihit ) {
    trh.add_hit(*ihit);
  }

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

  cout << ok_prefix << "Construct fitter." << endl;
  PropagatorPtr pprop(new PropTest);
  FullFitKalman fitk(pprop);
  cout << fitk << endl;

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

  cout << ok_prefix << "Fit with all hits." << endl;
  assert( ! trh.is_fit() );
  fitk.fit(trh);
  cout << trh << endl;
  double small = 1.e-10;
  assert( trh.is_fit() );
  assert( fabs( trh.get_track().get_vector()(0) - avg.get_average() )
                  < small );
  assert( fabs( trh.get_track().get_error()(0,0) - avg.get_error() )
                  < small );
  cout << fabs( trh.get_chi_square() - avg.get_chi_square() ) << endl;
  assert( fabs( trh.get_chi_square() - avg.get_chi_square() )
                  < small );

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

  cout << ok_prefix << "Write object data." << endl;
  {
    FullFitterPtr pfitk( new FullFitKalman(pprop) );
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("prop",pprop);
    objstream.write_object("fitk",pfitk);
    cout << mystream.str() << endl;
    assert( ObjTable::has_object_name("fitk") );
  }

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

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "\n[ fitk2 FullFitKalman prop=@prop ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      assert( obstr.read_object() == "fitk2" );
      FullFitterPtr pfit;
      ObjTable::get_object("fitk2",pfit);
      assert( pfit != 0 );
      assert( pfit->get_type() == FullFitKalman::get_static_type() );
      Ptr<const FullFitKalman,LocalSharedDeletePolicy> pfitk;
      pfitk.assign_with_dynamic_cast(pfit);
      assert( pfitk != 0 );
      assert( pfitk->get_propagator() == pprop );
    }
  }

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

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

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

}