// HitXYPlane2.cpp

#include "HitXYPlane2.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() == "ClusXYPlane2" );
  double dist = data.get_double("dist");
  double phi = data.get_double("phi");
  double v = data.get_double("v");
  double z = data.get_double("z");
  double dv2 = data.get_double("dv2");
  double dz2 = data.get_double("dz2");
  double dvdz = data.get_double("dvdz");
#ifdef ObjData_supports_lists
  McIdList mcids;
  data.get_int_list("mcids",mcids);
#endif

  return ObjPtr( new ClusXYPlane2(dist,phi,v,z,dv2,dz2,dvdz,mcids) );
}

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

}

//**********************************************************************
// ClusXYPlane2
//**********************************************************************

// output stream

void ClusXYPlane2::ostr(ostream& stream) const {

  stream << begin_object ;
  stream << " vz " << _sxyp
	 << ": vz =  [" << _v << " , " << _z << " ] +/- " << 
    "[ " << _dv2 << " , " << _dz2 <<" , " << _dvdz << " ] ";
  stream << end_object;
}

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

// equality

bool ClusXYPlane2::equal(const Cluster& clus) const {
  assert( this->get_type() == clus.get_type() );
  const ClusXYPlane2& ccp = (const ClusXYPlane2&) clus;
  return ( _v == ccp._v ) && 
         ( _z == ccp._z ) &&
         ( _dv2 == ccp._dv2 ) && 
         ( _dz2 == ccp._dz2 ) && 
         ( _dvdz == ccp._dvdz ) && 
         ( _sxyp == ccp._sxyp );
}

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

// constructor

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         const HitVector& hm,
                         const HitError& dhm )
: _sxyp(dist,phi) {

  _v = hm(IV);
  _z= hm(IZ);
  _dv2 = dhm(IV,IV);
  _dz2 = dhm(IZ,IZ);
  _dvdz = dhm(IV,IZ);
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);

}

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

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         double v, double z,
                         const HitError& dhm )
: _sxyp(dist,phi) {
  _v = v;
  _z=  z;
  _dv2 = dhm(IV,IV);
  _dz2 = dhm(IZ,IZ);
  _dvdz = dhm(IV,IZ);
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);

}

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

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         double v, double z,
                         double dv2,double dz2,double dvdz )
: _sxyp(dist,phi) {
  _v = v;
  _z=  z;
  _dv2 = dv2;
  _dz2 = dz2;
  _dvdz = dvdz;
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);

}

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

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         const HitVector& hm,
                         const HitError& dhm,const McIdList& mcids  )
:_sxyp(dist,phi),McCluster(mcids)  {

  _v = hm(IV);
  _z= hm(IZ);
  _dv2 = dhm(IV,IV);
  _dz2 = dhm(IZ,IZ);
  _dvdz = dhm(IV,IZ);
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );
  
  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);

}

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

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         double v, double z,
                         const HitError& dhm,const McIdList& mcids  )
: _sxyp(dist,phi),McCluster(mcids)  {
  _v = v;
  _z=  z;
  _dv2 = dhm(IV,IV);
  _dz2 = dhm(IZ,IZ);
  _dvdz = dhm(IV,IZ);
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);
}

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

ClusXYPlane2::ClusXYPlane2(double dist, double phi, 
                         double v, double z,
			 double dv2,double dz2,double dvdz,
                         const McIdList& mcids  )
: _sxyp(dist,phi),McCluster(mcids)  {
 
  _v = v;
  _z=  z;
  _dv2 = dv2;
  _dz2 = dz2;
  _dvdz = dvdz;
  
  assert( _dv2 >= 0.0 && _dz2 >=0. );

  // check that determinant of _dhm is positive
  assert( _dv2*_dz2 - _dvdz*_dvdz >= 0.0);  
}


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

// copy constructor

ClusXYPlane2::ClusXYPlane2(const ClusXYPlane2& rhs)
:  _sxyp(rhs._sxyp), _v(rhs._v),_z(rhs._z),
   _dv2(rhs._dv2),_dz2(rhs._dz2),_dvdz(rhs._dvdz), McCluster(rhs) {  
 }

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

// destructor

ClusXYPlane2::~ClusXYPlane2( ) { }

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

// generate hits from a track 

HitList ClusXYPlane2::_predict(const ETrack& tre) const {
  HitList hits;

  const TrackVector& vec = tre.get_vector();
  const TrackError& err = tre.get_error();
 
  hits.push_back( HitPtr(new 
		 HitXYPlane2( vec(SurfXYPlane::IV),vec(SurfXYPlane::IZ),
			      err(SurfXYPlane::IV,SurfXYPlane::IV),
			      err(SurfXYPlane::IZ,SurfXYPlane::IZ),
			      err(SurfXYPlane::IV,SurfXYPlane::IZ)
			      ) ));
  return hits;
}

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

// Return the creator.

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

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

// Write the object data.

ObjData ClusXYPlane2::write_data() const {
  ObjData data( get_type_name() );
  const SurfXYPlane& srf=(const SurfXYPlane&)(get_surface());
  data.add_double( "dist", srf.get_parameter( SurfXYPlane::DISTNORM) );
  data.add_double( "phi", srf.get_parameter( SurfXYPlane::NORMPHI) );
  data.add_double( "z", _z );
  data.add_double( "v", _v );
  data.add_double( "dvdz", _dvdz );
  data.add_double( "dv2", _dv2 );
  data.add_double( "dz2", _dz2 );
#ifdef ObjData_supports_lists
  data.add_int_list( "mcids", get_mc_ids() );
#endif
  return data;
}

//**********************************************************************
// HitXYPlane2
//**********************************************************************

// output stream

void HitXYPlane2::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 HitXYPlane2::equal(const Hit& hit) const {
  assert( this->get_type() == hit.get_type() );
  return get_cluster() == hit.get_cluster();
}

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

// constructor
  
HitXYPlane2::HitXYPlane2(double v, double z, double dv2, double dz2, double dvdz) : _v(v),_z(z),_dv2(dv2),_dz2(dz2),_dvdz(dvdz)
{ }

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

// copy constructor

HitXYPlane2::HitXYPlane2(const HitXYPlane2& lhs)
: Hit(lhs), _v(lhs._v),_z(lhs._z),_dv2(lhs._dv2),_dz2(lhs._dz2),_dvdz(lhs._dvdz) { }

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

// destructor

HitXYPlane2::~HitXYPlane2() { }

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

// return the measured hit vector

HitVector HitXYPlane2::measured_vector() const {
  const ClusXYPlane2& clu =  get_full_cluster();
  return HitVector( clu.get_v(), clu.get_z() );
}

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

// return the measured hit error

HitError HitXYPlane2::measured_error() const {
  const ClusXYPlane2& clu =  get_full_cluster();
  return HitError(clu.get_dv2(),clu.get_dvdz(), clu.get_dz2()   );
}

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

// return the predicted hit vector

HitVector HitXYPlane2::predicted_vector() const {
  return HitVector( _v, _z );
}

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

// return the predicted hit error

HitError HitXYPlane2::predicted_error() const {
  return HitError( _dv2,_dvdz,_dz2 );
}

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

// return the derivative

HitDerivative HitXYPlane2::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 HitXYPlane2::difference_vector() const {

  const ClusXYPlane2& clu =  get_full_cluster();
  

  double diff_v = _v - clu.get_v();
  double diff_z = _z - clu.get_z();
  return HitVector( diff_v, diff_z );
}

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

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

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

  const TrackVector& vec =   tre.get_vector(); 

  _v = vec(SurfXYPlane::IV);
  _z = vec(SurfXYPlane::IZ);
 
  const TrackError& err =  tre.get_error() ;

  _dv2 = err(SurfXYPlane::IV,SurfXYPlane::IV);
  _dvdz = err(SurfXYPlane::IV,SurfXYPlane::IZ);
  _dz2 = err(SurfXYPlane::IZ,SurfXYPlane::IZ);

}

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

// Return the creator.

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

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

// Write the object data.

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

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