/******************************************************************************
 * Calor Algorithm classes definition file                                    *
 *                                                                            *
 * Author: Bob Wagner, Argonne CDF Group                                      *
 *         Phone 630-252-6321 (Argonne)  630-840-8436 (Fermilab)              *
 *                                                                            *
 * Description:  Algorithms for manipulation of objects provided by           *
 *               calorimetry                                                  *
 *                                                                            *
 * Revision History:                                                          *
 *    23-Jan-2001   Bob Wagner   Initial creation                             *
 *    23-Mar-2001   Bob Wagner   Add TowerKeyFromTrack and                    *
 *                               ExtrapolateToShowerMax                       *
 *    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.         *
 *    10-Feb-2004   Bob Wagner   In TowerKeyFromTrack::getTower method, change*
 *                               creation of TowerKey for bad tracks from     *
 *                               passing max size_t argument to explicitly    *
 *                               passing 60 for ieta and iphi.  This avoids   *
 *                               possible problem of conversion from size_t to*
 *                               UChar_t.  TowerKey uses UChar_t.             *
 *    12-Jul-2004   Bob Wagner   Change length of constant B-field used in    *
 *                               track extrapolation from +/- 150 cm to       *
 *                               +/- 200 cm.  This goes along with studies    *
 *                               done by Brian Mohr on accuracy of            *
 *                               SimpleExtrapolatedTrack in the plug.         *
 *    16-Oct-2004   Bob Wagner   Add ExtrapolateToPreShower                   *
 *                                                                            *
 *****************************************************************************/

//--------------------------------------
// The Calor Algorithms Header        --
//--------------------------------------
#include "Calor/calor_alg.hh"

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

//-------------------------------
// Collaborating Class Headers --
//-------------------------------
#include "AbsEnv/AbsEnv.hh"
#include "Edm/EventRecord.hh"
#include "CLHEP/Vector/ThreeVector.h"
#include "CalorGeometry/HWCalorDetectorNode.hh"
#include "CalorGeometry/CalParameters.hh"
#include "CalorGeometry/PesAlignment.hh"
#include "CalorGeometry/TowerGeometry.hh"
#include "Calor/PhysicsTowerDataMaker.hh"
#include "Calor/CalorPredicates.hh"
#include "CalorObjects/PhysicsTower.hh"
#include "CalorObjects/PhysicsTowerParams.hh"
#include "GeometryBase/CdfDetector.hh"
#include "ElectronObjects/SimpleExtrapolatedTrack.hh"
#include "TrackingObjects/Tracks/CdfTrack.hh"


/*===========================================================================*\
 * CalTowerFromTowerKey methods                                              *
\*===========================================================================*/
calor_alg::CalTowerFromTowerKey::CalTowerFromTowerKey( void )
{
  AbsEvent* event(AbsEnv::instance()->theEvent());
  EventRecord::ConstIterator  iter( event, "CalData" );
  if (iter.is_valid()) _towers = iter;
}


/*===========================================================================*\
 * PhysicsTowerFromTowerKey methods                                          *
\*===========================================================================*/
calor_alg::PhysicsTowerFromTowerKey::PhysicsTowerFromTowerKey( void )
{
  AbsEvent* event(AbsEnv::instance()->theEvent());
  if (PhysicsTowerData::find(_towers, "DefaultPTD", event) ==
      PhysicsTowerData::ERROR) {
    PhysicsTowerParams params;
    _towers = PhysicsTowerDataMaker().create( event, params, "DefaultPTD" );
  }
}


const PhysicsTower*
calor_alg::PhysicsTowerFromTowerKey::operator()(const TowerKey& id) const
{
  return &(*(find_if(_towers->begin(),_towers->end(),TowerKey_eq(id))));
}


/*===========================================================================*\
 * TowerKeyFromTrack methods                                                 *
\*===========================================================================*/
calor_alg::TowerKeyFromTrack::TowerKeyFromTrack( void )
{
  _towerGeometry =
   (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());
}


calor_alg::TowerKeyFromTrack::TowerKeyFromTrack(const TowerKeyFromTrack& rhs) :
  _towerGeometry(rhs._towerGeometry) { }


TowerKey
calor_alg::TowerKeyFromTrack::operator()(const CdfTrack* aTrack) const
{
  SimpleExtrapolatedTrack  extTrack;
  if (calor_alg::ExtrapolateToShowerMax::extrapolate(aTrack,extTrack)) {
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = _towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t iphi = _towerGeometry->iPhi(ieta,extTrack.currentAzimuthSpacePt());
    TowerKey hitTower(ieta, iphi);
    return hitTower;
  } else {
    size_t badValue(std::numeric_limits<size_t>::max());
    TowerKey hitTower(badValue, badValue);
    return hitTower;
  }
}


TowerKey
calor_alg::TowerKeyFromTrack::getTower(const CdfTrack& aTrack)
{
  SimpleExtrapolatedTrack  extTrack;
  return getTower(&aTrack, extTrack);
}


TowerKey
calor_alg::TowerKeyFromTrack::getTower(const CdfTrack* aTrack)
{
  SimpleExtrapolatedTrack  extTrack;
  return getTower(aTrack, extTrack);
}


TowerKey
calor_alg::TowerKeyFromTrack::getTower
(const CdfTrack* aTrack, SimpleExtrapolatedTrack& extTrack)
{

  const TowerGeometry* towerGeometry =
    (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());

  if (calor_alg::ExtrapolateToShowerMax::extrapolate(aTrack, extTrack)) {
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t iphi = towerGeometry->iPhi(ieta,extTrack.currentAzimuthSpacePt());
    TowerKey hitTower(ieta, iphi);
    return hitTower;
  } else {
    // Values of ieta = iphi = 60 are invalid and will cause TowerKey validflag
    // to be set false.
    TowerKey hitTower(60, 60);
    return hitTower;
  }
}


/*===========================================================================*\
 * ExtrapolateToShowerMax methods                                            *
\*===========================================================================*/
calor_alg::ExtrapolateToShowerMax::ExtrapolateToShowerMax( void )
{
  _towerGeometry =
   (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());
}


calor_alg::ExtrapolateToShowerMax::ExtrapolateToShowerMax
( const ExtrapolateToShowerMax& rhs ) :
  _towerGeometry(rhs._towerGeometry) { }


bool
calor_alg::ExtrapolateToShowerMax::operator()
  (const CdfTrack* aTrack, SimpleExtrapolatedTrack& extTrack) const
{

  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  // 12-July-2004  Bob Wagner
  // Change length parameter of B field from 150 cm to 200 cm
  extTrack.loadField( bfield, 149.1, 200.0);
  extTrack.loadTrack(aTrack->getAlpha());

  /*===========================================================================
   * Extrapolation Section
   *      Determine which calorimeter we will intercept and extrapolate to
   *      location of Shower Maximum in that calorimeter
   ==========================================================================*/
  double extrapRadius = TLYCES;
  double extrapZ = 0.5 * (TLZPESL0 + TLZPESL1);
  // 12-July-2004  Bob Wagner
  // Add alignment correction of PES to z coordinate for extrapolation
  // if in the plug.
  if (extTrack.cotTheta() < 0.0) {
    extrapZ = -extrapZ + PesAlignment::instance()->getGlobalZoffset(0);
  } else {
    extrapZ += PesAlignment::instance()->getGlobalZoffset(1);
  }

  // Try extrapolating to vicinty of CES plane.
  // If within Z boundary of CEM at this location, finish extrapolation to
  // CES plane.
  if (extTrack.extrapolateR(extrapRadius)  &&
      (fabs(extTrack.currentZ()) <= TLZCEM)) {

    // Track is within CEM region.  Finish extrapolation to CES plane
    // First we determine the phi index of the wedge containing the track
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = _towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t wedgeNo =
      _towerGeometry->iPhi(ieta, extTrack.currentAzimuthSpacePt());

    // Next we determine the distance of the extrapolated track space point
    // from the CES plane and the track length from extrapolated point to CES
    double azimuthCes = M_PI * (1 + 2 * wedgeNo) / TEPSEG[TETTYP[ieta]];
    double distanceCes = TLYCES -
      (extTrack.currentSpacePoint().x() * cos(azimuthCes)) -
      (extTrack.currentSpacePoint().y() * sin(azimuthCes));
    double trackLengthToCes =
      distanceCes / cos(extTrack.currentPhi() - azimuthCes);

    // Finally we calculate the updated extrapolation radius to get us to CES
    extrapRadius = sqrt((extrapRadius * extrapRadius) +
			(trackLengthToCes * trackLengthToCes) +
			(2.0 * extrapRadius * trackLengthToCes *
	cos(extTrack.currentPhi() - extTrack.currentAzimuthSpacePt())));
    return extTrack.extrapolateR(extrapRadius);

  } else if (extTrack.extrapolateZ(extrapZ)) {

    // Extrapolation done to Z of PES.
    // Check if within physical boundary of plug calorimeter
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = _towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    if (ieta >= MinEtaPEME  &&  ieta <= MaxEtaPEME) return true;
    else if (ieta >= MinEtaPEMW  &&  ieta <= MaxEtaPEMW) return true;
    else return false;
  }

  // Track doesn't extrapolate to calorimeter
  return false;
}


bool
calor_alg::ExtrapolateToShowerMax::extrapolate
(const CdfTrack* aTrack, SimpleExtrapolatedTrack& extTrack)
{

  const TowerGeometry* towerGeometry =
    (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());
  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  extTrack.loadField( bfield, 149.1, 150.0);
  extTrack.loadTrack(aTrack->getAlpha());

  /*===========================================================================
   * Extrapolation Section
   *      Determine which calorimeter we will intercept and extrapolate to
   *      location of Shower Maximum in that calorimeter
   ==========================================================================*/
  double extrapRadius = TLYCES;
  double extrapZ = 0.5 * (TLZPESL0 + TLZPESL1);
  if (extTrack.cotTheta() < 0.0) {
    extrapZ = -extrapZ + PesAlignment::instance()->getGlobalZoffset(0);
  } else {
    extrapZ += PesAlignment::instance()->getGlobalZoffset(1);
  }

  // Try extrapolating to vicinty of CES plane.
  // If within Z boundary of CEM at this location, finish extrapolation to
  // CES plane.
  if (extTrack.extrapolateR(extrapRadius)  &&
      (fabs(extTrack.currentZ()) <= TLZCEM)) {

    // Track is within CEM region.  Finish extrapolation to CES plane
    // First we determine the phi index of the wedge containing the track
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t wedgeNo =
      towerGeometry->iPhi(ieta, extTrack.currentAzimuthSpacePt());

    // Next we determine the distance of the extrapolated track space point
    // from the CES plane and the track length from extrapolated point to CES
    double azimuthCes = M_PI * (1 + 2 * wedgeNo) / TEPSEG[TETTYP[ieta]];
    double distanceCes = TLYCES -
      (extTrack.currentSpacePoint().x() * cos(azimuthCes)) -
      (extTrack.currentSpacePoint().y() * sin(azimuthCes));
    double trackLengthToCes =
      distanceCes / cos(extTrack.currentPhi() - azimuthCes);

    // Finally we calculate the updated extrapolation radius to get us to CES
    extrapRadius = sqrt((extrapRadius * extrapRadius) +
			(trackLengthToCes * trackLengthToCes) +
			(2.0 * extrapRadius * trackLengthToCes *
	cos(extTrack.currentPhi() - extTrack.currentAzimuthSpacePt())));
    return extTrack.extrapolateR(extrapRadius);

  } else if (extTrack.extrapolateZ(extrapZ)) {

    // Extrapolation done to Z of PES.
    // Check if within physical boundary of plug calorimeter
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    if (ieta >= MinEtaPEME  &&  ieta <= MaxEtaPEME) return true;
    else if (ieta >= MinEtaPEMW  &&  ieta <= MaxEtaPEMW) return true;
    else return false;
  }

  // Track doesn't extrapolate to calorimeter
  // return unextrapolated track
  return false;
}


/*===========================================================================*\
 * ExtrapolateToPreShower methods                                            *
\*===========================================================================*/
calor_alg::ExtrapolateToPreShower::ExtrapolateToPreShower( void )
{
  _towerGeometry =
   (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());
}


calor_alg::ExtrapolateToPreShower::ExtrapolateToPreShower
( const ExtrapolateToPreShower& rhs ) :
  _towerGeometry(rhs._towerGeometry) { }


bool
calor_alg::ExtrapolateToPreShower::operator()
  (const CdfTrack* aTrack, SimpleExtrapolatedTrack& extTrack) const
{

  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  // 12-July-2004  Bob Wagner
  // Change length parameter of B field from 150 cm to 200 cm
  extTrack.loadField( bfield, 149.1, 200.0);
  extTrack.loadTrack(aTrack->getAlpha());

  /*===========================================================================
   * Extrapolation Section
   *      Determine which calorimeter we will intercept and extrapolate to
   *      location of PreShower in that calorimeter
   ==========================================================================*/
  double extrapRadius = TLYCPR;
  double extrapZ = TLZPEM;
  if (extTrack.cotTheta() < 0.0) extrapZ = -extrapZ;

  // Try extrapolating to vicinty of CPR plane.
  // If within Z boundary of CEM at this location, finish extrapolation to
  // CPR plane.
  if (extTrack.extrapolateR(extrapRadius)  &&
      (fabs(extTrack.currentZ()) <= TLZCEM)) {

    // Track is within CEM region.  Finish extrapolation to CPR plane
    // First we determine the phi index of the wedge containing the track
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = _towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t wedgeNo =
      _towerGeometry->iPhi(ieta, extTrack.currentAzimuthSpacePt());

    // Next we determine the distance of the extrapolated track space point
    // from the CPR plane and the track length from extrapolated point to CPR
    double azimuthCpr = M_PI * (1 + 2 * wedgeNo) / TEPSEG[TETTYP[ieta]];
    double distanceCpr = TLYCPR -
      (extTrack.currentSpacePoint().x() * cos(azimuthCpr)) -
      (extTrack.currentSpacePoint().y() * sin(azimuthCpr));
    double trackLengthToCpr =
      distanceCpr / cos(extTrack.currentPhi() - azimuthCpr);

    // Finally we calculate the updated extrapolation radius to get us to CPR
    extrapRadius = sqrt((extrapRadius * extrapRadius) +
			(trackLengthToCpr * trackLengthToCpr) +
			(2.0 * extrapRadius * trackLengthToCpr *
	cos(extTrack.currentPhi() - extTrack.currentAzimuthSpacePt())));
    return extTrack.extrapolateR(extrapRadius);

  } else if (extTrack.extrapolateZ(extrapZ)) {

    // Extrapolation done to Z of PPR
    // Check if within physical boundary of plug calorimeter
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = _towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    if (ieta >= MinEtaPEME  &&  ieta <= MaxEtaPEME) return true;
    else if (ieta >= MinEtaPEMW  &&  ieta <= MaxEtaPEMW) return true;
    else return false;
  }

  // Track doesn't extrapolate to calorimeter
  return false;
}


bool
calor_alg::ExtrapolateToPreShower::extrapolate
(const CdfTrack* aTrack, SimpleExtrapolatedTrack& extTrack)
{

  const TowerGeometry* towerGeometry =
    (CdfDetector::instance()->getCalorDetector()->getTowerGeometry());
  HepPoint3D cdf_origin(0.0, 0.0, 0.0);
  double bfield = CdfDetector::instance()->getBfield()->getField(cdf_origin).z();
  extTrack.loadField( bfield, 149.1, 150.0);
  extTrack.loadTrack(aTrack->getAlpha());

  /*===========================================================================
   * Extrapolation Section
   *      Determine which calorimeter we will intercept and extrapolate to
   *      location of Pre Shower in that calorimeter
   ==========================================================================*/
  double extrapRadius = TLYCPR;
  double extrapZ = TLZPEM;
  if (extTrack.cotTheta() < 0.0) extrapZ = -extrapZ;

  // Try extrapolating to vicinty of CPR plane.
  // If within Z boundary of CEM at this location, finish extrapolation to
  // CPR plane.
  if (extTrack.extrapolateR(extrapRadius)  &&
      (fabs(extTrack.currentZ()) <= TLZCEM)) {

    // Track is within CEM region.  Finish extrapolation to CPR plane
    // First we determine the phi index of the wedge containing the track
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    size_t wedgeNo =
      towerGeometry->iPhi(ieta, extTrack.currentAzimuthSpacePt());

    // Next we determine the distance of the extrapolated track space point
    // from the CPR plane and the track length from extrapolated point to CPR
    double azimuthCpr = M_PI * (1 + 2 * wedgeNo) / TEPSEG[TETTYP[ieta]];
    double distanceCpr = TLYCPR -
      (extTrack.currentSpacePoint().x() * cos(azimuthCpr)) -
      (extTrack.currentSpacePoint().y() * sin(azimuthCpr));
    double trackLengthToCpr =
      distanceCpr / cos(extTrack.currentPhi() - azimuthCpr);

    // Finally we calculate the updated extrapolation radius to get us to CPR
    extrapRadius = sqrt((extrapRadius * extrapRadius) +
			(trackLengthToCpr * trackLengthToCpr) +
			(2.0 * extrapRadius * trackLengthToCpr *
	cos(extTrack.currentPhi() - extTrack.currentAzimuthSpacePt())));
    return extTrack.extrapolateR(extrapRadius);

  } else if (extTrack.extrapolateZ(extrapZ)) {

    // Extrapolation done to Z of PPR.
    // Check if within physical boundary of plug calorimeter
    Hep3Vector spacePt = extTrack.currentSpacePoint();
    size_t ieta = towerGeometry->iEta(spacePt.x(), spacePt.y(), spacePt.z());
    if (ieta >= MinEtaPEME  &&  ieta <= MaxEtaPEME) return true;
    else if (ieta >= MinEtaPEMW  &&  ieta <= MaxEtaPEMW) return true;
    else return false;
  }

  // Track doesn't extrapolate to calorimeter
  // return unextrapolated track
  return false;
}


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