/******************************************************************************
 * Ces Comparisons classes definition file                                    *
 *                                                                            *
 * Author: Bob Wagner, Argonne CDF Group                                      *
 *         Phone 630-252-6321 (Argonne)  630-840-8436 (Fermilab)              *
 *                                                                            *
 * Description:  Algorithmic function objects that do comparisons of CES      *
 *               quantities.  These are meant to be used with STL algorithms. *
 *                                                                            *
 * Revision History:                                                          *
 *    21-Nov-2000   Bob Wagner   Initial creation for IncrWireTrackDistance   *
 *                               IncrStripTrackDistance.                      *
 *    28-Nov-2000   Bob Wagner   Provide a common method for extrapolating to *
 *                               the CES using SimpleExtrapolatedTrack.       *
 *    29-Nov-2000   Bob Wagner   Fix error in trackStripDiff.  CesCluster     *
 *                               returns unsigned local z coord.  Need to add *
 *                               sign (West is -, East is +).                 *
 *    25-Jan 2002   Bob Wagner   Get value of central B field using the Bfield*
 *                               class in GeometryBase instead of the value   *
 *                               stored in                                    *
 *                             TrackingObjects/Utils/TempTrackingBFieldDefs.hh*
 *                               This is a more stable supported way.         *
 *                                                                            *
 *****************************************************************************/

//--------------------------------------
// The Ces Comparison Classes' Header --
//--------------------------------------
#include "Calor/ces_comp.hh"
#include "GeometryBase/CdfDetector.hh"

//---------------
// C++ Headers --
//---------------
#include <math.h>
#include <limits>

//-------------------------------
// Collaborating Class Headers --
//-------------------------------
#include "CalorGeometry/CalConstants.hh"
#include "CalorGeometry/CalParameters.hh"


//=============================================================================
// Static constants used in track extrapolation
//=============================================================================
const double ces_comp::IncrWireTrackDistance::_yToleranceCEM = 0.01;
const double ces_comp::IncrWireTrackDistance::_yFirstExtrapolationMin = 150.0;
const double ces_comp::IncrStripTrackDistance::_yToleranceCEM = 0.01;
const double ces_comp::IncrStripTrackDistance::_yFirstExtrapolationMin = 150.0;


/*===========================================================================*\
 * IncrWireTrackDistance methods                                             *
\*===========================================================================*/
//**************************************
// Custom constructor from track link **
//**************************************
ces_comp::IncrWireTrackDistance::IncrWireTrackDistance(
				       CdfEmObject::Track_link lTrack) {

  // Load initial track parameters for extrapolation
  _track.loadTrack( lTrack->getAlpha() );

  /*=======================================================================
   ***** WARNING: MAGNETIC FIELD LOADING USES FIELD VALUE AT B0      ******
   *****          BEAM CROSSING POINT, I.E. X=Y=Z=0.0.  THIS IS      ******
   *****          OBTAINED USING CLASS Bfield FOUND IN GeometryBase  ******
   *****          PACKAGE                                            ******
   *****          Z LENGTH OF CONSTANT FIELD IS ARBITRARILY SET      ******
   *****          IT SHOULD BE TUNED WITH A PROPER STUDY OF MATCHING ******
   *****          EXTRAPOLATED TRACK IN PLUG TO CLUSTER CENTROID     ******
   *======================================================================*/
  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  _track.loadField( bfield, 149.1, 150.0 );
}


//***********************************
// Comparison operator definitions **
//***********************************
bool
ces_comp::IncrWireTrackDistance::operator ()( const CesCluster& t1,
					      const CesCluster& t2 ) {

  // Make actual WIRE distance from track comparison only if both clusters are
  // WIRE views.
  // If one is WIRE and one a STRIP,
  // indicate WIRE view is always closer to track
  // If both are STRIP views, return false
  if (t1.view() != CesCluster::WIRE) return false;
  else if (t2.view() != CesCluster::WIRE) return true;
  else return (trackWireDiff(t1) < trackWireDiff(t2));
}


bool
ces_comp::IncrWireTrackDistance::operator ()(
		     const ValueVectorLink<CesClusterColl>& t1,
		     const ValueVectorLink<CesClusterColl>& t2 ) {

  // Make actual WIRE distance from track comparison only if both clusters are
  // WIRE views.
  // If one is WIRE and one a STRIP,
  // indicate WIRE view is always closer to track
  // If both are STRIP views, return false
  if (t1->view() != CesCluster::WIRE) return false;
  else if (t2->view() != CesCluster::WIRE) return true;
  else return (trackWireDiff(*t1) < trackWireDiff(*t2));
}


//***********************************************
// Extrapolated track/wire distance calculator **
//***********************************************
double
ces_comp::IncrWireTrackDistance::trackWireDiff( const CesCluster& cesClus ) {

  // Get rotation angle to coordinate system in which y axis passes through
  // center of CES containing cluster of interest.
  // CEM has constant phi segmentation, so we can choose any tower type
  // containing CEM to determine the segmentation.  Choose type = 0.
  size_t towerPhiSeg = TOWER_PHI_SEG[0];
  int wedgeNo = cesClus.module();
  double rotAngle = M_PI_2 - (M_PI/towerPhiSeg) * (2.0 * wedgeNo + 1.0 );
  HepVector initParam = _track.parameters();

  // Rotate to coordinate system for CesCluster
  // Save initial phi angle to restore initial parameters upon completion
  double initPhi = initParam[SimpleExtrapolatedTrack::PHI0];
  initParam[SimpleExtrapolatedTrack::PHI0] += rotAngle;
  while (initParam[SimpleExtrapolatedTrack::PHI0] > 2.0*M_PI)
    initParam[SimpleExtrapolatedTrack::PHI0] -= 2.0 * M_PI;
  while (initParam[SimpleExtrapolatedTrack::PHI0] < 0.0)
    initParam[SimpleExtrapolatedTrack::PHI0] += 2.0 * M_PI;
  _track.loadTrack( initParam );

  // Extrapolate track into wedge containing CesCluster
  // and calculate difference between track and CES local X coordinate
  // If track cannot be extrapolated to this wedge,
  // set difference to biggest double.
  double xDiff, currentY;
  double yDiff = 2.0 * _yToleranceCEM;
  double extrapRadius = TLYCES;
  if (!(_track.extrapolateR(extrapRadius))) {
    xDiff = std::numeric_limits<double>::max();
  } else {
    currentY = _track.currentSpacePoint().y();
    if (currentY < _yFirstExtrapolationMin) {
      xDiff = std::numeric_limits<double>::max();
    } else {
      yDiff = fabs(TLYCES - currentY);
      while (yDiff > _yToleranceCEM) {
	// Calculate y coordinate difference from radius of CES
	// and extrapolation radius to use on next iteration
	yDiff = TLYCES - _track.currentSpacePoint().y();
	extrapRadius += yDiff / sin(_track.currentPhi());
	if (_track.extrapolateR(extrapRadius)) {
	  yDiff = fabs(TLYCES - _track.currentSpacePoint().y());
	  xDiff = fabs(_track.currentSpacePoint().x()-cesClus.localCoord());
	} else {
	  xDiff = std::numeric_limits<double>::max();
	  break;
	}
      }
    }
  }

  initParam[SimpleExtrapolatedTrack::PHI0] = initPhi;
  _track.loadTrack( initParam );

  return xDiff;
}


/*===========================================================================*\
 * IncrStripTrackDistance methods                                             *
\*===========================================================================*/
//**************************************
// Custom constructor from track link **
//**************************************
ces_comp::IncrStripTrackDistance::IncrStripTrackDistance(
				       CdfEmObject::Track_link lTrack) {

  // Load initial track parameters for extrapolation
  _track.loadTrack( lTrack->getAlpha() );

  /*=======================================================================
   ***** WARNING: MAGNETIC FIELD LOADING USES FIELD VALUE AT B0      ******
   *****          BEAM CROSSING POINT, I.E. X=Y=Z=0.0.  THIS IS      ******
   *****          OBTAINED USING CLASS Bfield FOUND IN GeometryBase  ******
   *****          PACKAGE                                            ******
   *****          Z LENGTH OF CONSTANT FIELD IS ARBITRARILY SET      ******
   *****          IT SHOULD BE TUNED WITH A PROPER STUDY OF MATCHING ******
   *****          EXTRAPOLATED TRACK IN PLUG TO CLUSTER CENTROID     ******
   *======================================================================*/
  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  _track.loadField( bfield, 149.1, 150.0 );
}


//***********************************
// Comparison operator definitions **
//***********************************
bool
ces_comp::IncrStripTrackDistance::operator ()( const CesCluster& t1,
					       const CesCluster& t2 ) {

  // Make actual STRIP distance from track comparison only if both clusters are
  // STRIP views.
  // If one is STRIP and one a WIRE,
  // indicate STRIP view is always closer to track
  // If both are WIRE views, return false
  if (t1.view() != CesCluster::STRIP) return false;
  else if (t2.view() != CesCluster::STRIP) return true;
  else return (trackStripDiff(t1) < trackStripDiff(t2));
}


bool
ces_comp::IncrStripTrackDistance::operator ()(
		     const ValueVectorLink<CesClusterColl>& t1,
		     const ValueVectorLink<CesClusterColl>& t2 ) {

  // Make actual STRIP distance from track comparison only if both clusters are
  // STRIP views.
  // If one is STRIP and one a WIRE,
  // indicate STRIP view is always closer to track
  // If both are WIRE views, return false
  if (t1->view() != CesCluster::STRIP) return false;
  else if (t2->view() != CesCluster::STRIP) return true;
  else return (trackStripDiff(*t1) < trackStripDiff(*t2));
}


//************************************************
// Extrapolated track/strip distance calculator **
//************************************************
double
ces_comp::IncrStripTrackDistance::trackStripDiff( const CesCluster& cesClus ) {

  // Get rotation angle to coordinate system in which y axis passes through
  // center of CES containing cluster of interest.
  // CEM has constant phi segmentation, so we can choose any tower type
  // containing CEM to determine the segmentation.  Choose type = 0.
  size_t towerPhiSeg = TOWER_PHI_SEG[0];
  int wedgeNo = cesClus.module();
  double rotAngle = M_PI_2 - (M_PI/towerPhiSeg) * (2.0 * wedgeNo + 1.0 );
  HepVector initParam = _track.parameters();

  // Rotate to coordinate system for CesCluster
  // Save initial phi angle to restore initial parameters upon completion
  double initPhi = initParam[SimpleExtrapolatedTrack::PHI0];
  initParam[SimpleExtrapolatedTrack::PHI0] += rotAngle;
  while (initParam[SimpleExtrapolatedTrack::PHI0] > 2.0*M_PI)
    initParam[SimpleExtrapolatedTrack::PHI0] -= 2.0 * M_PI;
  while (initParam[SimpleExtrapolatedTrack::PHI0] < 0.0)
    initParam[SimpleExtrapolatedTrack::PHI0] += 2.0 * M_PI;
  _track.loadTrack( initParam );

  // Extrapolate track into wedge containing CesCluster
  // and calculate difference between track and CES local Z coordinate
  // If track cannot be extrapolated to this wedge,
  // set difference to biggest double.
  double zDiff, currentY;
  double yDiff = 2.0 * _yToleranceCEM;
  double extrapRadius = TLYCES;
  if (!(_track.extrapolateR(extrapRadius))) {
    zDiff = std::numeric_limits<double>::max();
  } else {
    currentY = _track.currentSpacePoint().y();
    if (currentY < _yFirstExtrapolationMin) {
      zDiff = std::numeric_limits<double>::max();
    } else {
      yDiff = fabs(TLYCES - currentY);
      while (yDiff > _yToleranceCEM) {
	// Calculate y coordinate difference from radius of CES
	// and extrapolation radius to use on next iteration
	yDiff = TLYCES - _track.currentSpacePoint().y();
	extrapRadius += yDiff / sin(_track.currentPhi());
	if (_track.extrapolateR(extrapRadius)) {
	  yDiff = fabs(TLYCES - _track.currentSpacePoint().y());
	  // Must handle unsigned local z coordinate returned by CesCluster
	  if (1 == cesClus.side())
	    zDiff = fabs(_track.currentSpacePoint().z()-cesClus.localCoord());
	  else
	    zDiff = fabs(_track.currentSpacePoint().z()+cesClus.localCoord());
	} else {
	  zDiff = std::numeric_limits<double>::max();
	  break;
	}
      }
    }
  }

  initParam[SimpleExtrapolatedTrack::PHI0] = initPhi;
  _track.loadTrack( initParam );

  return zDiff;
}


/******************************************************************************
 *     ALL DONE                                                               *
 *****************************************************************************/