// ClusFindXYPlane2.cpp

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

using namespace trf;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "ClusFindXYPlane2" );
  double dist = data.get_double("dist");
  double phi = data.get_double("phi");
  double diff = data.get_double("max_chsq_diff");
  return ObjPtr( new ClusFindXYPlane2( dist,phi,diff) );
}

}


//**********************************************************************
// Helper functions.
//**********************************************************************

// Calculate the chi-square difference between and cluster and a track.
// Only v components are taken into account for calculated returned chisq_diff
// Real chisquare are returned in double *chisq variable
// It is assumed the cluster generates exactly one prediction and that
// this prediction has two dimension.

double chsq_diffxyp2(const ClusterPtr& pclu, const ETrack& tre ) {

  const ClusXYPlane2& clu = (const ClusXYPlane2&)(*pclu);
  const TrackVector& vec = tre.get_vector();
  const TrackError& err = tre.get_error();

  double diff0 = clu.get_v() - vec(SurfXYPlane::IV ); 
  double diff1 = clu.get_z() - vec(SurfXYPlane::IZ ); 

  if ( diff0 == 0. && diff1 == 0. ) return 0.;

  double e11 = err(SurfXYPlane::IV,SurfXYPlane::IV);
  double e22 = err(SurfXYPlane::IZ,SurfXYPlane::IZ);
  double e12 = err(SurfXYPlane::IV,SurfXYPlane::IZ);
  double a = e11 + clu.get_dv2();
  double b = e12 + clu.get_dvdz();
  double c = e22 + clu.get_dz2();
  double det = a*c-b*b;
 
  if ( det  == 0. ) return 1.e15;

  double chisq = (c*diff0*diff0 + a*diff1*diff1 - 2*b*diff0*diff1)/det; 

  if ( chisq < 0 ) return 1.e15;
  
  return chisq;


}

//**********************************************************************
// Methods.
//**********************************************************************

// ouput stream

void ClusFindXYPlane2::ostr(ostream& stream) const {
 
  stream << begin_object ;
  stream << "2D Cluster finder for " << _sxyp << "." << new_line;
  stream << "Maximum chi-square difference is " 
         << _max_chsq_diff << ".";
  stream << end_object;

}

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

// constructor

ClusFindXYPlane2::
ClusFindXYPlane2(double dist, double phi, double max_chsq_diff)
: _sxyp(dist,phi), _max_chsq_diff(max_chsq_diff) { }

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

// destructor

ClusFindXYPlane2::~ClusFindXYPlane2() { }

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

// add a cluster

int ClusFindXYPlane2::add_cluster(const ClusterPtr& pclu) {

  // Extract the cluster position depending on type
  if ( pclu->get_type() == ClusXYPlane2::get_static_type() ) {
    const ClusXYPlane2& clu = (const ClusXYPlane2&) *pclu;
    // assert( clu.get_surface() == _sxyp );
  }  else {
    assert(false);
    return 1;
  }

  // Check there is no other cluster at this position.
  const ClusterList& clusters = _box.get_clusters();
  ClusterList::const_iterator iclu ;
  for ( iclu= clusters.begin() ;  iclu!= clusters.end(); ++iclu )
    assert( (*iclu) != pclu );

  // Store the cluster.
  _box.add_cluster(&*pclu);

  return 0;

}

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

const ClusterContainer& ClusFindXYPlane2::get_cluster_container(
			 const ETrack& tre, const CutRecord* pcut_record,
                         std::map< ClusterPtr, CutRecord*>* precs) const {

  if( _max_chsq_diff >= 0. ) {
    fill_container(tre);
    return _outbox;
  }
 
  return _box;
}

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

// drop all clusters

void ClusFindXYPlane2::drop_clusters() {

  _box.drop_clusters();

}

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

// Return all clusters.
ClusterList ClusFindXYPlane2::get_clusters() const {

  return _box.get_clusters();

}


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

// Return the clusters near a track.
//

ClusterList ClusFindXYPlane2::get_clusters(const ETrack& tre) const {
  if ( _max_chsq_diff < 0. ) return _box.get_clusters();
  fill_container(tre);
  return _outbox.get_clusters();
}

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

// Return the clusters near a track.
//

void ClusFindXYPlane2::fill_container(const ETrack& tre) const {

  // Create the list of clusters.
  _outbox.drop_clusters();
  
  // Check the track surface.
  if ( *tre.get_surface() != _sxyp ) {
    assert(false);
    return;
  }
  
  const ClusterList& _clusters = _box.get_clusters();

  // If there are no clusters, return the empty list.
  if ( ! _clusters.size() ) return;
    
  ClusterList::const_iterator iclu;
  for(iclu = _clusters.begin() ; iclu != _clusters.end(); ++iclu ) {
    const ClusterPtr& pclu = (*iclu);
    if ( chsq_diffxyp2(pclu,tre) <= _max_chsq_diff ) _outbox.add_cluster( &*pclu );
  }
  

}

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

// Return the creator.

ObjCreator ClusFindXYPlane2::get_creator() {
  return create;
}

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

// Write the object data.

ObjData ClusFindXYPlane2::write_data() const {
  ObjData data( get_type_name() );
  data.add_double("dist", _sxyp.get_parameter(SurfXYPlane::DISTNORM) );
  data.add_double("phi", _sxyp.get_parameter(SurfXYPlane::NORMPHI) );
  data.add_double( "max_chsq_diff", get_max_chsq_diff() );
  return data;
}

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