// HitZPlane2.cpp

#include "HitZPlane2.h"
#include "trfbase/ETrack.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"

using namespace trf;

//**********************************************************************
// Free functions.
//**********************************************************************

namespace {

// Creator.

ObjPtr create_cluster(const ObjData& data) {
  assert( data.get_object_type() == "ClusZPlane2" );
  double z = data.get_double("z");
  double x = data.get_double("x");
  double y = data.get_double("y");
  double dx2 = data.get_double("dx2");
  double dy2 = data.get_double("dy2");
  double dxdy = data.get_double("dxdy");
#ifdef ObjData_supports_lists
  McIdList mcids;
  data.get_int_list("mcids",mcids);
#endif
  
  return ObjPtr( new ClusZPlane2(z,x,y,dx2,dy2,dxdy,mcids) );
}

ObjPtr create_hit(const ObjData& data) {
  assert(false);
  abort();
  return ObjPtr(0);
}

}

//**********************************************************************
// ClusZPlane2
//**********************************************************************

// output stream

void ClusZPlane2::ostr(ostream& stream) const {
  stream << begin_object ;
  stream << " z " << _szp
	 << ": xy =  [" << _x << " , " << _y << " ] +/- " << 
    "[ " << _dx2 << " , " << _dy2 <<" , " << _dxdy << " ] ";
  stream << end_object;
}

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

// equality

bool ClusZPlane2::equal(const Cluster& clus) const {
  assert( this->get_type() == clus.get_type() );
  const ClusZPlane2& ccp = (const ClusZPlane2&) clus;
  return ( _x == ccp._x ) && 
         ( _y == ccp._y ) &&
         ( _dx2 == ccp._dx2 ) && 
         ( _dy2 == ccp._dy2 ) && 
         ( _dxdy == ccp._dxdy ) && 
         ( _szp == ccp._szp ); 
}

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

// constructor

ClusZPlane2::ClusZPlane2(double zpos, 
                         const HitVector& hm,
                         const HitError& dhm )
: _szp(zpos) {

  // check that determinant of _dhm is positive

  _x = hm(IX);
  _y = hm(IY);
  _dx2 = dhm(IX,IX);
  _dy2 = dhm(IY,IY);
  _dxdy = dhm(IX,IY);
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
  
}

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

ClusZPlane2::ClusZPlane2(double zpos, 
                         double x, double y,
                         const HitError& dhm )
: _szp(zpos) {

  _x = x;
  _y = y;
  _dx2 = dhm(IX,IX);
  _dy2 = dhm(IY,IY);
  _dxdy = dhm(IX,IY);
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
}

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

ClusZPlane2::ClusZPlane2(double zpos, 
                         double x, double y,
                         double dx2,double dy2,double dxdy )
: _szp(zpos) {

  _x = x;
  _y = y;
  _dx2 = dx2;
  _dy2 = dy2;
  _dxdy = dxdy;
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
}

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

ClusZPlane2::ClusZPlane2(double zpos, 
                         const HitVector& hm,
                         const HitError& dhm,const McIdList& mcids  )
: _szp(zpos),McCluster(mcids) {
  _x = hm(IX);
  _y = hm(IY);
  _dx2 = dhm(IX,IX);
  _dy2 = dhm(IY,IY);
  _dxdy = dhm(IX,IY);
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
}

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

ClusZPlane2::ClusZPlane2(double zpos, 
                         double x, double y,
                         const HitError& dhm,const McIdList& mcids  )
: _szp(zpos),McCluster(mcids) {
  _x = x;
  _y = y;
  _dx2 = dhm(IX,IX);
  _dy2 = dhm(IY,IY);
  _dxdy = dhm(IX,IY);
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
}

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

ClusZPlane2::ClusZPlane2(double zpos, 
                         double x, double y,
                         double dx2,double dy2,double dxdy,
			 const McIdList& mcids )
: _szp(zpos),McCluster(mcids) {

  _x = x;
  _y = y;
  _dx2 = dx2;
  _dy2 = dy2;
  _dxdy = dxdy;
  
  assert( _dx2 >= 0.0 && _dy2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dx2*_dy2 - _dxdy*_dxdy >= 0.0);
}

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

// copy constructor

ClusZPlane2::ClusZPlane2(const ClusZPlane2& rhs)
:  _szp(rhs._szp),_x(rhs._x),_y(rhs._y),
   _dx2(rhs._dx2),_dy2(rhs._dy2),_dxdy(rhs._dxdy),McCluster(rhs) {  
  
}

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

// destructor

ClusZPlane2::~ClusZPlane2( ) { }

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

// generate hits from a track 

HitList ClusZPlane2::_predict(const ETrack& tre) const {

  HitList hits;

  const TrackVector& vec = tre.get_vector();
  const TrackError& err = tre.get_error();
 
  hits.push_back( HitPtr(new 
		 HitZPlane2( vec(SurfZPlane::IX),vec(SurfZPlane::IY),
			      err(SurfZPlane::IX,SurfZPlane::IX),
			      err(SurfZPlane::IY,SurfZPlane::IY),
			      err(SurfZPlane::IX,SurfZPlane::IY)
			      ) ));

  return hits;
}
//**********************************************************************

// Return the creator.

ObjCreator ClusZPlane2::get_creator() {
  return create_cluster;
}

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

// Write the object data.

ObjData ClusZPlane2::write_data() const {
  ObjData data( get_type_name() );
  const SurfZPlane& srf=(const SurfZPlane&)(get_surface());
  data.add_double( "z", srf.get_z() );
  data.add_double( "x", _x );
  data.add_double( "y", _y );
  data.add_double( "dxdy", _dxdy );
  data.add_double( "dx2", _dx2 );
  data.add_double( "dy2", _dy2 );
#ifdef ObjData_supports_lists
  data.add_int_list( "mcids", get_mc_ids() );
#endif
  return data;
}


//**********************************************************************
// HitZPlane2
//**********************************************************************

// output stream

void HitZPlane2::ostr(ostream& stream) const {
  stream << begin_object ;
  if( _pclus )
    {
      stream << "hit from ";
      stream << *_pclus;
    }
  else
    {
      stream << "Hit with no parent clusters";
    }
  stream << end_object;
}

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

// equality
// Hits are equal if they have the same parent cluster.

bool HitZPlane2::equal(const Hit& hit) const {
  assert( this->get_type() == hit.get_type() );
  return get_cluster() == hit.get_cluster();
}

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

// constructor

HitZPlane2::HitZPlane2(double x, double y, double dx2, double dy2, double dxdy)
: _x(x),_y(y),_dx2(dx2),_dy2(dy2),_dxdy(dxdy)
{ }

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

// copy constructor

HitZPlane2::HitZPlane2(const HitZPlane2& lhs)
: Hit(lhs), _x(lhs._x),_y(lhs._y),_dx2(lhs._dx2),_dy2(lhs._dy2),_dxdy(lhs._dxdy) { }

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

// destructor

HitZPlane2::~HitZPlane2() { }

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

// return the measured hit vector

HitVector HitZPlane2::measured_vector() const {
  const ClusZPlane2& clu =  get_full_cluster();
  return HitVector( clu.get_x(), clu.get_y() );
}

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

// return the measured hit error

HitError HitZPlane2::measured_error() const {
  const ClusZPlane2& clu =  get_full_cluster();
  return HitError(clu.get_dx2(),clu.get_dxdy(), clu.get_dy2()   );
}

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

// return the predicted hit vector

HitVector HitZPlane2::predicted_vector() const {
  return HitVector( _x , _y );
}

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

// return the predicted hit error

HitError HitZPlane2::predicted_error() const {
  return HitError( _dx2,_dxdy, _dy2 );
}

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

// return the derivative

HitDerivative HitZPlane2::dhit_dtrack() const {
  static double values[] = { 1.0, 0.0, 0.0, 0.0, 0.0, 
                      0.0, 1.0, 0.0, 0.0, 0.0 };
  static HitDerivative deriv(2,values);
  return deriv;
}

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

// return the difference between prediction and measurement.

HitVector HitZPlane2::difference_vector() const {

  const ClusZPlane2& clu =  get_full_cluster();
  
  double diff_x = _x - clu.get_x();
  double diff_y = _y - clu.get_y();
  return HitVector( diff_x, diff_y );
}

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

// update the hit prediction (measurement and derivative do not change)

void HitZPlane2::update( const ETrack& tre) {

  const TrackVector& vec =   tre.get_vector(); 

  _x = vec(SurfZPlane::IX);
  _y = vec(SurfZPlane::IY);
 
  const TrackError& err =  tre.get_error() ;

  _dx2 = err(SurfZPlane::IX,SurfZPlane::IX);
  _dxdy = err(SurfZPlane::IX,SurfZPlane::IY);
  _dy2 = err(SurfZPlane::IY,SurfZPlane::IY);
}

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

// Return the creator.

ObjCreator HitZPlane2::get_creator() {
  return create_hit;
}

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

// Write the object data.

ObjData HitZPlane2::write_data() const {
  ObjData data( get_type_name() );
  return data;
}

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