// AddFitKalman_t.cpp

// Test AddFitKalman.

#include "AddFitKalman.h"
#include <string>
#include <iostream>
#include <cassert>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "trfutil/trfstream.h"
#include "trfbase/SurfTest.h"
#include "HTrack.h"
#include "HitTestFit1.h"
#include "AddFitterPtr.h"

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

using namespace trf;

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

int main( ) {

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

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

  cout << trf_format;
  ObjTable::register_type<HitTestFit1>();
  ObjTable::register_type<ClusTestFit1>();
  ObjTable::register_type<AddFitKalman>();
  cout << ok_prefix << "Test constructor." << endl;
  AddFitKalman fitk;
  cout << fitk << endl;
  assert( fitk.get_type() == AddFitKalman::get_static_type() );
  cout << fitk.get_type_name() << endl;
  assert( fitk.get_type_name() == "AddFitKalman" );

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

  cout << ok_prefix << "Test fitting 1 hit." << endl;
  double small = 1.e-12;
  WAvg avg;
  SurfTest srf(10);
  TrackVector vec;
  TrackError err;
  vec(0) = 2.9;
  err(0,0) = 0.04;
  err(1,1) = 0.0001;
  err(2,2) = 0.0001;
  err(3,3) = 0.0001;
  err(4,4) = 0.0001;
  avg.add_pair(vec(0),err(0,0));
  ETrack tre( SurfacePtr(srf.new_pure_surface()), vec, err );
  HTrack trh(tre);
  ClusterPtr pclu1( new ClusTestFit1(srf,3.0,0.01) );
  avg.add_pair(3.0,0.01);
  HitList hits = pclu1->predict(tre);
  cout << trh << endl;
  cout << *hits.front() << endl;
  assert( fitk.add_hit(trh,hits.front()) == 0 );
  cout << trh << endl;
  cout << avg << endl;
  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 );
  assert( fabs( trh.get_chi_square() - avg.get_chi_square() )
                < small );
  assert( trh.get_hits().size() == 1 );
 
  //********************************************************************

  cout << ok_prefix << "Test fitting 2 hits." << endl;
  ClusterPtr pclu2( new ClusTestFit1(srf,3.3,0.02) );
  avg.add_pair(3.3,0.02);
  hits = pclu2->predict(trh.get_track());
  assert( fitk.add_hit( trh, hits.front() ) == 0 );
  cout << trh << endl;
  cout << avg << endl;
  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 );
  assert( fabs( trh.get_chi_square() - avg.get_chi_square() )
                < small );
  assert( trh.get_hits().size() == 2 );

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

  cout << ok_prefix << "Write object data." << endl;
  {
    AddFitterPtr pfit( new AddFitKalman );
    ostringstream mystream;
    StdObjStream objstream(mystream);
    objstream.write_object("fit1",pfit);
    cout << mystream.str() << endl;
    assert( ObjTable::has_object_name("fit1") );
  }

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

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ fit2 AddFitKalman debug=true ]";
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "fit2" );
      AddFitterPtr pfit;
      assert( pfit == 0 );
      ObjTable::get_object("fit2",pfit);
      assert( pfit != 0 );
      assert( pfit->get_type() == AddFitKalman::get_static_type() );
    }
  }

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

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

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

}