// HitXYPlane1_t.cc

// Test component HitXYPlane1.

#include "HitXYPlane1.h"
#include <cassert>
#include <string>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "objstream/ObjData.hpp"
#include "trfbase/ETrack.h"
#include <cmath>
#include "trfutil/TRFMath.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;

double wv = 0.001;
double wz = 0.345;

// calculate avz from a track vector
double calc_avz(TrackVector vec) {
  double v_track = vec(SurfXYPlane::IV);
  double z_track = vec(SurfXYPlane::IZ);

  return( wv*v_track + wz*z_track );
}

// calculate eaxy from error matrix
double calc_eavz(TrackError err) {
  double evv_track = err(SurfXYPlane::IV,SurfXYPlane::IV);
  double evz_track = err(SurfXYPlane::IV,SurfXYPlane::IZ);
  double ezz_track = err(SurfXYPlane::IZ,SurfXYPlane::IZ);

  return( evv_track*wv*wv + 2.*evz_track*wv*wz + ezz_track*wz*wz );
}


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

int main( ) {

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

  cout << ok_prefix << "Test cluster constructors." << endl;
  McIdList mcids;
  mcids.push_back(1);
  mcids.push_back(2);
  double dist1 = 10.0;
  double phi1 = PI/3.;
  double avz1 = 1.0;
  double davz1 = 0.01;
  SurfXYPlane szp1(dist1,phi1);
  ClusXYPlane1* pbare_hcp1 = new ClusXYPlane1(dist1,phi1,wv,wz,avz1,davz1);
  ClusXYPlane1 tst1(*pbare_hcp1);
  ClusXYPlane1* pbare_hcp1a = new ClusXYPlane1(dist1,phi1,wv,wz,avz1,davz1,mcids);
  ClusXYPlane1 tst1a(*pbare_hcp1a);
  ClusXYPlane1& hcp1 = *pbare_hcp1;
  ClusterPtr phcp1 = pbare_hcp1;
  cout << *phcp1 << endl;
  ClusXYPlane1 hcp1c = hcp1;
  cout << hcp1c << endl;
  double dist2 = 20.0;
  double phi2 = PI/4.;
  double avz2 = 2.0;
  double davz2 = 0.02;
  ClusXYPlane1* pbare_hcp2 = new ClusXYPlane1(dist2,phi2,wv,wz,avz2,davz2);
  ClusterPtr phcp2 = pbare_hcp2;

  assert(*pbare_hcp1==*pbare_hcp1a);
  assert(tst1==*pbare_hcp1);
  assert(*phcp2==*pbare_hcp2);
 
  assert(pbare_hcp1->get_mc_ids().size()==0 );
  assert(pbare_hcp1a->get_mc_ids().size()==2 );
  assert(tst1.get_mc_ids().size()==0 );
  assert(tst1a.get_mc_ids().size()==2 );
  delete pbare_hcp1a;

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

  cout << ok_prefix << "Test cluster type ID." << endl;
  cout << phcp1->get_type() << endl;
  cout << phcp2->get_type() << endl;
  assert( phcp1->get_type() != 0 );
  assert( phcp1->get_type() == phcp2->get_type() );

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

  cout << ok_prefix << "Test cluster accessors." << endl;
  cout << *phcp1 << endl;
  cout << phcp1->get_surface() << endl;
  cout << hcp1.get_avz() << endl;
  cout << hcp1.get_davz() << endl;
  cout << hcp1.get_wv() << endl;
  cout << hcp1.get_wz() << endl; 
  assert( hcp1.get_surface() == szp1 );
  assert( hcp1.get_avz() == avz1 );
  assert( hcp1.get_davz() == davz1 );
  assert( hcp1.get_wv() == wv );
  assert( hcp1.get_wz() == wz );

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

  cout << ok_prefix << "Generate a hit." << endl;
  TrackVector vec;
  TrackError err;
  HitDerivative der_expect(1);
  der_expect(0,0) = wv;
  der_expect(0,1) = wz;
  vec(SurfXYPlane::IV) = 1.1;
  vec(SurfXYPlane::IZ) = 40.0;
  err(SurfXYPlane::IV,SurfXYPlane::IV) = 0.04;
  err(SurfXYPlane::IV,SurfXYPlane::IZ) = 0.03;
  err(SurfXYPlane::IZ,SurfXYPlane::IZ) = 0.02;
  ETrack tre1(SurfacePtr(szp1.new_pure_surface()),vec,err);
  HitList tclus11 = phcp1->predict(tre1);
  assert( tclus11.size() == 1 );
  Hit& pre11 = *tclus11.front();
  double maxdiff = 1.e-12;
  cout << pre11.size() << endl;
  assert( pre11.size() == 1);
  cout << pre11.measured_vector() << endl;
  assert( is_equal( pre11.measured_vector()(0), avz1 ) );
  cout << pre11.measured_error() << endl;
  assert( is_equal( pre11.measured_error()(0,0), davz1*davz1 ) );
  cout << pre11.predicted_vector() << endl;
  assert( is_equal( pre11.predicted_vector()(0), calc_avz(vec) ) );
  cout << pre11.predicted_error() << endl;
  assert( is_equal( pre11.predicted_error()(0,0), calc_eavz(err) ) );
  cout << pre11.dhit_dtrack() << endl;
  assert( is_equal( pre11.dhit_dtrack(), der_expect ) );
  cout << pre11.difference_vector() << endl;
  assert( fabs(pre11.difference_vector()(0) - 
               calc_avz(vec) + avz1) < maxdiff );

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

  cout << ok_prefix << "Generate hit for a different track."
       << endl;
  vec(SurfXYPlane::IV) = 1.2;
  ETrack tre2(SurfacePtr(szp1.new_pure_surface()),vec,err);
  HitList tclus12 = phcp1->predict(tre2);
  assert( tclus12.size() == 1 );
  Hit& pre12 = *tclus12.front();
  cout << pre12.size() << endl;
  assert( pre12.size() == 1);
  cout << pre12.measured_vector() << endl;
  assert( is_equal( pre12.measured_vector()(0), avz1 ) );
  cout << pre12.measured_error() << endl;
  assert( is_equal( pre12.measured_error()(0,0), davz1*davz1 ) );
  cout << pre12.predicted_vector() << endl;
  assert( is_equal( pre12.predicted_vector()(0), calc_avz(vec) ) );
  cout << pre12.predicted_error() << endl;
  assert( is_equal( pre12.predicted_error()(0,0), calc_eavz(err) ) );
  cout << pre12.dhit_dtrack() << endl;
  assert( is_equal( pre12.dhit_dtrack(), der_expect ) );
  cout << pre12.difference_vector() << endl;
  assert( fabs(pre12.difference_vector()(0) - 
          calc_avz(vec) + avz1) < maxdiff );
  // same cluster ==> same hit (even though track changes)
  assert( pre11 == pre11 );
  assert( ! ( pre11 != pre11 ) );
  assert( pre11 == pre12 );

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

  cout << ok_prefix << "Generate hit for a different cluster."
       << endl;
  HitList tclus22 = phcp2->predict(tre2);
  assert( tclus22.size() == 1 );
  Hit& pre22 = *tclus22.front();
  cout << pre22.size() << endl;
  assert( pre22.size() == 1);
  cout << pre22.measured_vector() << endl;
  assert( is_equal( pre22.measured_vector()(0), avz2 ) );
  cout << pre22.measured_error() << endl;
  assert( is_equal( pre22.measured_error()(0,0), davz2*davz2 ) );
  cout << pre22.predicted_vector() << endl;
  assert( is_equal( pre22.predicted_vector()(0), calc_avz(vec) ) );
  cout << pre22.predicted_error() << endl;
  assert( is_equal( pre22.predicted_error()(0,0), calc_eavz(err) ) );
  cout << pre22.dhit_dtrack() << endl;
  assert( is_equal( pre22.dhit_dtrack(), der_expect ) );
  cout << pre22.difference_vector() << endl;
  assert( fabs(pre22.difference_vector()(0) - 
               calc_avz(vec) + avz2) < maxdiff );
  // different cluster ==> different hit
  assert( pre22 != pre11 );
  assert( ! ( pre22 == pre11 ) );
  assert( pre22 != pre12 );

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

  cout << ok_prefix << "Update hit." << endl;
  cout << pre11 << endl;
  pre11.update(tre2);
  cout << pre11 << endl;
  assert( pre11.size() == 1);
  cout << pre11.measured_vector() << endl;
  assert( is_equal( pre11.measured_vector()(0), avz1 ) );
  cout << pre11.measured_error() << endl;
  assert( is_equal( pre11.measured_error()(0,0), davz1*davz1 ) );
  cout << pre11.predicted_vector() << endl;
  assert( is_equal( pre11.predicted_vector()(0), calc_avz(vec) ) );
  cout << pre11.predicted_error() << endl;
  assert( is_equal( pre11.predicted_error()(0,0), calc_eavz(err) ) );
  cout << pre11.dhit_dtrack() << endl;
  assert( is_equal( pre11.dhit_dtrack(), der_expect ) );
  cout << pre11.difference_vector() << endl;
  assert( fabs(pre11.difference_vector()(0) - 
               calc_avz(vec) + avz1) < maxdiff );

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

  cout << ok_prefix << "Check hit type and equality." << endl;
  cout << HitXYPlane1::get_static_type();
  cout << pre11.get_type() << endl;
  cout << pre12.get_type() << endl;
  assert( pre11.get_type() != 0 );
  assert( pre11.get_type() == HitXYPlane1::get_static_type() );
  assert( pre11.get_type() == pre12.get_type() );

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

  ObjTable::register_type<ClusXYPlane1>();
  
  cout << ok_prefix << "Write object data." << endl;
  McIdList mcids_h;
  mcids_h.push_back(2);
  mcids_h.push_back(4);
  mcids_h.push_back(8);
  {
    ObjPtr pobj( new ClusXYPlane1(12.3, 1.5, 0.1,0.9,1.2,0.15, mcids_h) );
    ostringstream mystream;
    StdObjStream objstream(mystream);
    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( "ClusXYPlane1 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "dist",pos)) != string::npos );
    assert( (pos=mystream.str().find( "12.3 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "phi",pos)) != string::npos );
    assert( (pos=mystream.str().find( "1.5 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "zweight",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.9 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "vweight",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.1 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "vz",pos)) != string::npos );
    assert( (pos=mystream.str().find( "1.2 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "dvz",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.15 ",pos)) != string::npos );
#ifdef ObjData_supports_lists
    assert( (pos=mystream.str().find( "mcids",pos)) != string::npos );
    assert( (pos=mystream.str().find( "2 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "4 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "8 ",pos)) != string::npos );
#endif
  }
  
  //********************************************************************

  cout << ok_prefix << "Read object data." << endl;
  {
    string istring = "[ obj2 ClusXYPlane1 dist=24.4 phi=2.8 vweight=0.1 zweight=0.9 vz=1.3 dvz=0.1";

    #ifdef ObjData_supports_lists
    istring += " mcids=int( 2 4 8 )";
    #endif

    istring += " ]";
    cout << istring << endl;
    istringstream isstrm(istring);
    {
      StdObjStream obstr(isstrm);
      string name = obstr.read_object();
      assert( name == "obj2" );
      const ClusXYPlane1* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      cout << *pobj << endl;
      assert( pobj->get_type() == ClusXYPlane1::get_static_type() );
      assert( ((const SurfXYPlane&)(pobj->get_surface())).get_parameter(SurfXYPlane::DISTNORM) == 24.4 );
      assert( ((const SurfXYPlane&)(pobj->get_surface())).get_parameter(SurfXYPlane::NORMPHI) == 2.8 );
      assert( pobj->get_avz() == 1.3 );
      assert( pobj->get_davz() == 0.1 );
      assert( pobj->get_wv() == 0.1 );
      assert( pobj->get_wz() == 0.9 );
#ifdef ObjData_supports_lists
      assert( pobj->get_mc_ids() == mcids_h );
#endif
    }
  }

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

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

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

}