// HitZPlane2_t.cpp

// Test component HitZPlane2.

#include "HitZPlane2.h"
#include <cassert>
#include <string>
#include <sstream>
#include "objstream/StdObjStream.hpp"
#include "objstream/ObjData.hpp"
#include "trfbase/ETrack.h"
#include "trfutil/trfstream.h"
#include <cmath>

#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;

// calculate hm from a track vector
HitVector calc_hm(TrackVector vec) {
  double x_track = vec(SurfZPlane::IX);
  double y_track = vec(SurfZPlane::IY);

  return HitVector( x_track, y_track );
}

// calculate dhm from error matrix
HitError calc_dhm(TrackError err) {
  double exx_track = err(SurfZPlane::IX,SurfZPlane::IX);
  double exy_track = err(SurfZPlane::IX,SurfZPlane::IY);
  double eyy_track = err(SurfZPlane::IY,SurfZPlane::IY);

  return HitError( exx_track, exy_track, eyy_track);
}

// compare HitError and error matrix of the cluster

bool hiteqsm(const HitError& lhs, const HitError& rhs) {
    if ( lhs.size() != rhs.size() || lhs.size()!=2 ) return false;
    return ( (lhs(0,0) == rhs(ClusZPlane2::IX,ClusZPlane2::IX)) &&
             (lhs(0,1) == rhs(ClusZPlane2::IX,ClusZPlane2::IY)) &&
             (lhs(1,1) == rhs(ClusZPlane2::IY,ClusZPlane2::IY)) );
} 

// compare HitVector and vector of the cluster

bool hiteqvec(const HitVector& lhs, const HitVector& rhs) {
    if ( lhs.size() != rhs.size() || lhs.size()!=2 ) return false;
    return ( (lhs(0) == rhs(ClusZPlane2::IX)) &&
             (lhs(1) == rhs(ClusZPlane2::IY)) );
} 


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

int main( ) {

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

  enum {
  IX = ClusZPlane2::IX,
  IY = ClusZPlane2::IY
  };

  cout << ok_prefix << "Test cluster constructors." << endl;
  McIdList mcids;
  mcids.push_back(1);
  mcids.push_back(2);

  double zpos1 = 10.0;
  double x1 = 1.0;
  double y1 = 1.1;
  HitError dhm1(2);
  dhm1(ClusZPlane2::IX,ClusZPlane2::IX) = 0.02;
  dhm1(ClusZPlane2::IX,ClusZPlane2::IY) = 0.01;
  dhm1(ClusZPlane2::IY,ClusZPlane2::IY) = 0.03;
  SurfZPlane szp1(zpos1);
  ClusZPlane2& hcp1 = *new ClusZPlane2(zpos1,x1,y1,dhm1(ClusZPlane2::IX,ClusZPlane2::IX),dhm1(ClusZPlane2::IY,ClusZPlane2::IY),dhm1(ClusZPlane2::IX,ClusZPlane2::IY));
  ClusZPlane2& hcp1a = *new ClusZPlane2(zpos1,x1,y1,dhm1(ClusZPlane2::IX,ClusZPlane2::IX),dhm1(ClusZPlane2::IY,ClusZPlane2::IY),dhm1(ClusZPlane2::IX,ClusZPlane2::IY),mcids);

  assert( hcp1 == hcp1a);
  assert(hcp1.get_x() == x1 );
  assert(hcp1.get_dhm() == dhm1 );
  assert(hcp1.get_hm() == HitVector(x1,y1) );
  assert(hcp1.get_y() == y1 );
  assert(hcp1.get_dx2() == 0.02 );
  assert(hcp1.get_dy2() == 0.03 );
  assert(hcp1.get_dxdy() == 0.01 );
  assert(hcp1a.get_x() == x1 );
  assert(hcp1a.get_y() == y1 );
  assert(hcp1a.get_dx2() == 0.02 );
  assert(hcp1a.get_dy2() == 0.03 );
  assert(hcp1a.get_dxdy() == 0.01 );
  assert(hcp1.get_mc_ids().size()==0);
  assert(hcp1a.get_mc_ids().size()==2);

  ClusterPtr phcp1 = &hcp1;
  cout << hcp1 << endl;
  ClusZPlane2 hcp1c = hcp1;
  cout << hcp1c << endl;
  double zpos2 = 20.0;
  double x2 = 2.0;
  double y2 = 2.2;
  HitVector hm2(2);
  hm2(ClusZPlane2::IX) = x2;
  hm2(ClusZPlane2::IY) = y2;
  HitError dhm2(2);
  dhm2(ClusZPlane2::IX,ClusZPlane2::IX) = 0.022;
  dhm2(ClusZPlane2::IX,ClusZPlane2::IY) = 0.012;
  dhm2(ClusZPlane2::IY,ClusZPlane2::IY) = 0.032;
  ClusZPlane2& hcp2 = *new ClusZPlane2(zpos2,x2,y2,dhm2);
  ClusZPlane2& hcp2a = *new ClusZPlane2(zpos2,x2,y2,dhm2,mcids);
  ClusterPtr phcp2 = &hcp2;

  assert(hcp2==hcp2a);
  assert(hcp2.get_mc_ids().size()==0);
  assert(hcp2a.get_mc_ids().size()==2);
  assert(hcp2.get_x() == x2 );
  assert(hcp2.get_y() == y2 );
  assert(hcp2.get_dx2() == 0.022 );
  assert(hcp2.get_dy2() == 0.032 );
  assert(hcp2.get_dxdy() == 0.012 );
  assert(hcp2a.get_x() == x2 );
  assert(hcp2a.get_y() == y2 );
  assert(hcp2a.get_dx2() == 0.022 );
  assert(hcp2a.get_dy2() == 0.032 );
  assert(hcp2a.get_dxdy() == 0.012 );

  HitVector hm1(2);
  hm1(ClusZPlane2::IX) = x1;
  hm1(ClusZPlane2::IY) = y1;
  ClusZPlane2 hcp3(zpos1,hm1,dhm1);
  ClusZPlane2 hcp3a(zpos1,hm1,dhm1,mcids);
  ClusZPlane2 hcp3b(hcp3a);
  ClusZPlane2 hcp3c(hcp3);

  assert(hcp3.get_x() == x1 );
  assert(hcp3.get_y() == y1 );
  assert(hcp3.get_dx2() == 0.02 );
  assert(hcp3.get_dy2() == 0.03 );
  assert(hcp3.get_dxdy() == 0.01 );
  assert(hcp3a.get_x() == x1 );
  assert(hcp3a.get_y() == y1 );
  assert(hcp3a.get_dx2() == 0.02 );
  assert(hcp3a.get_dy2() == 0.03 );
  assert(hcp3a.get_dxdy() == 0.01 );

  // test two diferent constructors
  assert(hcp3b==hcp3a);
  assert(hcp3==hcp3c);
  assert(hcp3.get_mc_ids().size()==0);
  assert(hcp3a.get_mc_ids().size()==2);
  assert(hcp3c.get_mc_ids().size()==0);
  assert(hcp3b.get_mc_ids().size()==2);

  assert( hcp3 == hcp1 );
  //********************************************************************

  cout << ok_prefix << "Test cluster type ID." << endl;
  cout << hcp1.get_type() << endl;
  cout << hcp2.get_type() << endl;
  assert( hcp1.get_type() != 0 );
  assert( hcp1.get_type() == hcp2.get_type() );

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

  cout << ok_prefix << "Test cluster accessors." << endl;
  cout << hcp1 << endl;
  cout << hcp1.get_surface() << endl;
  cout << hcp1.get_hm() << endl;
  cout << hcp1.get_dhm() << endl;
  assert( hcp1.get_surface() == szp1 );
  assert( hcp1.get_hm() == hm1 );
  assert( hcp1.get_dhm() == dhm1 );

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

  cout << ok_prefix << "Generate a hit." << endl;
  TrackVector vec;
  TrackError err;
  HitDerivative der_expect(2);
  der_expect(0,0) = 1.;
  der_expect(1,1) = 1.;
  vec(SurfZPlane::IX) = 1.1;
  vec(SurfZPlane::IY) = 40.0;
  err(SurfZPlane::IX,SurfZPlane::IX) = 0.04;
  err(SurfZPlane::IX,SurfZPlane::IY) = 0.03;
  err(SurfZPlane::IY,SurfZPlane::IY) = 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() == 2);
  cout << pre11.measured_vector() << endl;
  assert( ((const HitZPlane2&)pre11).get_x() == pre11.predicted_vector()(IX) );
  assert( ((const HitZPlane2&)pre11).get_y() == pre11.predicted_vector()(IY) );
  assert( ((const HitZPlane2&)pre11).get_dx2() == pre11.predicted_error()(IX,IX) );
  assert( ((const HitZPlane2&)pre11).get_dy2() == pre11.predicted_error()(IY,IY) );
  assert( ((const HitZPlane2&)pre11).get_dxdy() == pre11.predicted_error()(IX,IY) );  assert( hiteqvec(pre11.measured_vector(), hm1) );
  cout << pre11.measured_error() << endl;
  assert( hiteqsm(pre11.measured_error(), dhm1) );
  cout << pre11.predicted_vector() << endl;
  assert( pre11.predicted_vector() == calc_hm(vec) );
  cout << pre11.predicted_error() << endl;
  assert( pre11.predicted_error() == calc_dhm(err) );
  cout << pre11.dhit_dtrack() << endl;
  assert( pre11.dhit_dtrack() == der_expect );
  cout << pre11.difference_vector() << endl;
  assert( fabs(pre11.difference_vector()(0) - 
               calc_hm(vec)(0) + hm1(0)) < maxdiff );
  assert( fabs(pre11.difference_vector()(1) - 
               calc_hm(vec)(1) + hm1(1)) < maxdiff );
  //********************************************************************

  cout << ok_prefix << "Generate hit for a different track."
       << endl;
  vec(SurfZPlane::IX) = 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() == 2);
  cout << pre12.measured_vector() << endl;
  assert( hiteqvec(pre12.measured_vector(), hm1) );
  cout << pre12.measured_error() << endl;
  assert( hiteqsm(pre12.measured_error(), dhm1) );
  cout << pre12.predicted_vector() << endl;
  assert( pre12.predicted_vector() == calc_hm(vec) );
  cout << pre12.predicted_error() << endl;
  assert( pre12.predicted_error() == calc_dhm(err) );
  cout << pre12.dhit_dtrack() << endl;
  assert( pre12.dhit_dtrack() == der_expect );
  cout << pre12.difference_vector() << endl;
  assert( fabs(pre12.difference_vector()(0) - 
          calc_hm(vec)(0) + hm1(0)) < maxdiff );
  assert( fabs(pre12.difference_vector()(1) - 
          calc_hm(vec)(1) + hm1(1)) < 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() == 2);
  cout << pre22.measured_vector() << endl;
  assert( hiteqvec(pre22.measured_vector(), hm2) );
  cout << pre22.measured_error() << endl;
  assert( hiteqsm(pre22.measured_error(), dhm2) );
  cout << pre22.predicted_vector() << endl;
  assert( pre22.predicted_vector() == calc_hm(vec) );
  cout << pre22.predicted_error() << endl;
  assert( pre22.predicted_error() == calc_dhm(err) );
  cout << pre22.dhit_dtrack() << endl;
  assert( pre22.dhit_dtrack() == der_expect );
  cout << pre22.difference_vector() << endl;
  assert( fabs(pre22.difference_vector()(0) - 
               calc_hm(vec)(0) + hm2(0)) < maxdiff );
  assert( fabs(pre22.difference_vector()(1) - 
               calc_hm(vec)(1) + hm2(1)) < 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() == 2);
  cout << pre11.measured_vector() << endl;
  assert( hiteqvec(pre11.measured_vector(), hm1) );
  cout << pre11.measured_error() << endl;
  assert( hiteqsm(pre11.measured_error(), dhm1) );
  cout << pre11.predicted_vector() << endl;
  assert( pre11.predicted_vector() == calc_hm(vec) );
  cout << pre11.predicted_error() << endl;
  assert( pre11.predicted_error() == calc_dhm(err) );
  cout << pre11.dhit_dtrack() << endl;
  assert( pre11.dhit_dtrack() == der_expect );
  cout << pre11.difference_vector() << endl;
  assert( fabs(pre11.difference_vector()(0) - 
               calc_hm(vec)(0) + hm1(0)) < maxdiff );
  assert( fabs(pre11.difference_vector()(1) - 
               calc_hm(vec)(1) + hm1(1)) < maxdiff );
  //********************************************************************

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

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

  ObjTable::register_type<ClusZPlane2>();
  
  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);
  { 
    HitError err(2);
    err(ClusZPlane2::IX,ClusZPlane2::IX) = 0.01;
    err(ClusZPlane2::IX,ClusZPlane2::IY) = 0.0001;
    err(ClusZPlane2::IY,ClusZPlane2::IY) = 0.02;

    ObjPtr pobj( new ClusZPlane2(12.3, 0.1,0.9,err, 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( "ClusZPlane2 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "z",pos)) != string::npos );
    assert( (pos=mystream.str().find( "12.3 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "x",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.1 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "y",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.9 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "dxdy",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.0001 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "dx2",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.01 ",pos)) != string::npos );
    assert( (pos=mystream.str().find( "dy2",pos)) != string::npos );
    assert( (pos=mystream.str().find( "0.02 ",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 ClusZPlane2 z=24.4 x=0.1 y=0.9 dx2=1.3 dy2=0.3 dxdy=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 ClusZPlane2* pobj;
      ObjTable::get_object(name,pobj);
      assert( pobj != 0 );
      cout << *pobj << endl;
      assert( pobj->get_type() == ClusZPlane2::get_static_type() );
      assert( ((const SurfZPlane&)(pobj->get_surface())).get_z() == 24.4 );
      assert( pobj->get_hm()(ClusZPlane2::IX) == 0.1 );
      assert( pobj->get_hm()(ClusZPlane2::IY) == 0.9 );
      assert( pobj->get_dhm()(ClusZPlane2::IX,ClusZPlane2::IX) == 1.3 );
      assert( pobj->get_dhm()(ClusZPlane2::IY,ClusZPlane2::IY) == 0.3 );
      assert( pobj->get_dhm()(ClusZPlane2::IX,ClusZPlane2::IY) == 0.1 );
#ifdef ObjData_supports_lists
      assert( pobj->get_mc_ids() == mcids_h );
#endif
    }
  }

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

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

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

}