//---------------------------------------------------------
// File and Version Information :
//      Implementation for CprWireCollection class
//
// Description:
//      It makes a collection of hit wires in the CPR 
//      around the seed wire. 
//        
// Environment:
//      Software developed for the CDF Detector.
//
// Author List:
//      04/26/2000  Tania Moulik : created 
//
// Revision History :
//      06/04/2000  Adding seed based clustering
//      07/14/2000  Adding CES based clustering
//----------------------------------------------------------
//-------------
// C Headers --
//-------------
#include <assert.h>

//---------------
// C++ Headers --
//---------------
#include <iostream>
#include <vector>





//-------------------------------
// Collaborating Class Headers --
//-------------------------------

#include "Calor/CprWireCollectionMaker.hh"
#include "RawDataBanks/CPRD_StorableBank.hh"
#include "CalorObjects/CPRQColl.hh"
#include "BaBar/Cdf.hh"
#include "AbsEnv/AbsEnv.hh"
#include "StripChamberGeometry/CprWedgeCode.hh"
#include "StripChamberGeometry/CprIntersection.hh"
#include "StripChamberGeometry/SimpleCprPathFinder.hh"
#include "ErrorLogger_i/gERRLOG.hh"
//----------------
// Constructors --
//----------------

CprWireCollectionMaker::CprWireCollectionMaker() : _seedthrs(CPR_SEED_THRS)
{  
}

//--------------
// Destructor --
//--------------

CprWireCollectionMaker::~CprWireCollectionMaker( )
{
}

//--------------
// Operations --
//--------------


void CprWireCollectionMaker::initJob(bool useCPRQ){  
  _useCPRQ = useCPRQ;
}


void CprWireCollectionMaker::initRun(bool verbose){

  // load calibration info for this run
  _dbInitialized = false; 
  bool result = _calib.initializeDB();
  if(result) _dbInitialized = true;

  if ( !gblEnv->monteFlag() ) { // if real data, check if info was loaded OK
    if ( !_dbInitialized ) {
      ERRLOG(ELabort,"Calor:CprWireCollectionMaker: ") <<  
        "Error initialising DB!" << endmsg;
    } else {
      bool result = _calib.loadCalibConsts();
      if(!result)  ERRLOG(ELabort,"Calor:CprWireCollectionMaker: ") << 
                     "Error fetching calibration constants" << endmsg;      
    }    
  } else { // if MonteCarlo, load appropriate constants:
    _calib.loadMCConsts();
  }

  if (verbose ) _calib.print(std::cout);
}


void CprWireCollectionMaker::initCollection(AbsEvent* anEvent){

  // Zeroise used wire array and wires
  for (int i=0; i<2; i++){
    for (int j=0; j<24; j++){
      for (int k=0; k<CPR_NUM_WIRE; k++){
	int wireId = k + CPR_NUM_WIRE * j + CPR_NUM_WIRE*CPR_NUM_MOD * i;
	_wireused[wireId] = 0; 
	_wires[i][j][k] = 0.;
      }
    }
  }
  // Clear contents of _seedwire;
  _seedWire.clear();

  _seed=-100;

  // set the seed Threshold to a default if 0;
  // seedthrs initiaised in constructor.
  // if (_seedthrs == 0) {
  //    _seedthrs = CPR_SEED_THRS;
  //  }
  
  if (_useCPRQ) {
    // Get the CPRQ Collection
    EventRecord::ConstIterator cprq(anEvent,"CPRQColl");
    CPRQColl_ch cprqColl(cprq);
    if (cprq.is_valid()) {
      for (CPRQColl::const_iterator cprqIter = cprqColl->contents().begin(); 
	   cprqIter != cprqColl->contents().end(); ++cprqIter) {
	int wireNo = cprqIter->wireNo();
	int module = cprqIter->module();
	int i_ew = cprqIter->i_ew();      
	int energy = cprqIter->energy();

	float fl_charge = float(energy -_calib.pedCpr())*_calib.detCpr();

	if (fl_charge>=0.0) _wires[i_ew][module][wireNo] = fl_charge;
	// Fill seed wire collection for seed based clustering
	//	cout << "Seed Thrs " << _seedthrs << endl;
	if (fl_charge>_seedthrs) {
	  CprWire seedwire;
	  seedwire.setWireNo(wireNo);
	  seedwire.setModule(module);
	  seedwire.setSide(i_ew);
	  seedwire.setPulseHeight(fl_charge);	  
	  _seedWire.push_back( seedwire );
	}
      }
    }
    else {
      ERRLOG(ELerror,"CprWireCollectionMaker: ") << "Error Pad Object CPRQColl not found" << endmsg;
    }
  }

  //Get the CPRD bank in an event (there should be only one)
  else {
    EventRecord::ConstIterator bankIter(anEvent,"CPRD_StorableBank");
    if(bankIter.is_valid() )    {
      ConstHandle<CPRD_StorableBank> cprdBank(bankIter);
    
      for (CPRD_StorableBank::ConstGrandBankIter data_iter(cprdBank);
	   data_iter.is_valid(); data_iter++)  {
	int blockind = data_iter.block_index();
	int wireNo = cprdBank->get_wire(data_iter);
	int module = cprdBank->get_module(data_iter);
	int energy = cprdBank->get_data(data_iter);
	int i_ew = cprdBank->get_we(data_iter);
	
	// Replace this with DB entries   MR
	//float fl_charge = float(energy-CPR_PED)*CPR_ADC_TO_CHARG;
	
	float fl_charge = float(energy -_calib.pedCpr())*_calib.detCpr();
	// MR
	
	if (fl_charge>0.0) {
	  _wires[i_ew][module][wireNo] = fl_charge;
	  // Fill seed wire collection for seed based clustering
	  if (fl_charge>_seedthrs) {
	    CprWire seedwire;
	    seedwire.setWireNo(wireNo);
	    seedwire.setModule(module);
	    seedwire.setSide(i_ew);
	    seedwire.setPulseHeight(fl_charge);	  
	    _seedWire.push_back( seedwire );
	  }
	}
	
      }
    }
    else {
      ERRLOG(ELerror,"CprWireCollectionMaker: ") << "Error CPRD RawDataBank not found" << endmsg;
    }
  }
}

int CprWireCollectionMaker::extrapolate_track(const CdfTrack_clnk & theTrack){
  int success = 0;
  _xlocal     = -999.0;
  _zlocal     = -999.0;
  _eta_global = -999.0;
  _phi_global = -999.0;
  _barrel     = -999.0;
  _phiwedge   = -999.0;

  SimpleCprPathFinder finder;
  const CprIntersection *intersect = finder.newIntersection(theTrack->getTrajectory());
  if(intersect) {
    // Get the global position to determine the eta and phi (barrel and
    // module) hitten
    HepPoint3D globalPos = intersect->globalPosition();
    float z_int=globalPos.z();

    float theta_global = globalPos.theta();
    _eta_global = - log ( tan ( theta_global / 2 ) );
    _barrel=1;
    if (_eta_global<0){
      _barrel=0;
    } 
    
    _phi_global = globalPos.phi();
    if (_phi_global<0) {
      while (_phi_global<0) _phi_global+= 2*M_PI;
    }
    else if (_phi_global>2*M_PI) {
      while (_phi_global>2*M_PI) _phi_global-= 2*M_PI;
    }

    _phiwedge = int(_phi_global * 24 / (2 * M_PI) );
    
    // Get the local X and Z intersections.
    _xlocal = intersect->localX();
    _zlocal = intersect->localZ();
    //    if (_barrel) _xlocal = -_xlocal;
    success = 1;

    delete intersect ;  // memory leak. if not done
  }  
  return(success);
}

//---------------------------------------------------------------------
//
// get_track_based_wires: Find all wires connected with a Cdf track.
// The seed wire corresponds to the intersection between the track and
// the CPR.
//
//---------------------------------------------------------------------

const vector<CprWire> CprWireCollectionMaker::get_track_based_wires(CprWirePar* MyPar){

  double wireThrs = MyPar->wireThrs();
  double seedThrs = MyPar->seedThrs();
  bool printWires = MyPar->printWires();
  int numberWires = MyPar->numberWires();

  vector<CprWire> WireVec;  

  // Chimney Module    
//   if (_phiwedge==5 && _barrel==1 && fabs(_zlocal) > ZHI0_CPR) {
//     if (printWires) {
//       std::cout << "In chimney Module" << std::endl;
//     }
//     return WireVec;
//   }

//   if (fabs(_zlocal) < ZLO0_CPR 
//       || (fabs(_zlocal) > ZHI0_CPR && fabs(_zlocal) < ZLO1_CPR)
//       || fabs(_zlocal) > ZHI1_CPR
//       || fabs(_xlocal) > CPR_WIRE_OFFSET) {
//     if (printWires) {
//       std::cout << "Track not within CPR chamber X,Z : " << _xlocal << " " << _zlocal << std::endl;
//     }
//     return WireVec;
//   }

  if (fabs(_xlocal) > CPR_WIRE_OFFSET) {
    if (printWires) {
      std::cout << "Track not within CPR chamber X,Z : " << _xlocal << " " << _zlocal << std::endl;
    }
    return WireVec;
  }

  float xloc = _xlocal;
  if (_barrel) xloc = -_xlocal;

  // Get the Wire # hit
  int wire_hit = int ((CPR_WIRE_OFFSET - xloc)/CPR_WIRE_PITCH); // output 0 - 15

  int min_wire = 0;
  int max_wire = 15;
  if (fabs(_zlocal) > CPR_ZCRACK ) {
    min_wire = 16;
    max_wire = 31;
  }    

  if (fabs(_zlocal) > CPR_ZCRACK ) {
    wire_hit = 31 - wire_hit ;
  }
  
  if (wire_hit<0 || wire_hit>31){
    if (printWires) {
      ERRLOG(ELerror,"CprWireCollectionMaker: ") << "Error in wire Hit no." << endmsg;
    }
    return WireVec;
  }

  int s1=(numberWires-1)/2; // # of wires on either side of hit wire
  
  int start_hit = max(min_wire, wire_hit-s1);
  int end_hit = min(max_wire, wire_hit+s1);

  if (printWires){
    std::cout << "******************Wires***************************" << std::endl;
    std::cout << "Wire Hit " << wire_hit << " Start Hit  " << start_hit 
	 << " End Hit  " << end_hit << std::endl ; 
  }


  // Set the seedWire
  _seed = wire_hit;

  float maxcharge = 0.0;
  for (int i=start_hit; i<=end_hit; i++){
    if (_wires[_barrel][_phiwedge][i] > maxcharge) maxcharge = _wires[_barrel][_phiwedge][i] ;
    CprWire wire;
    wire.setWireNo(i);
    wire.setModule(_phiwedge);
    wire.setSide(_barrel);
    wire.setPulseHeight(_wires[_barrel][_phiwedge][i]);
    WireVec.push_back( wire );
  }

  if (printWires){
    for (vector <CprWire>::const_iterator  wire=WireVec.begin(); wire!=WireVec.end(); ++wire) {
      std::cout << 
        " barrel      " << wire->getSide() <<
        " module      " << wire->getModule() << 
        " wire number " << wire->getWireNo() << 
        " wire energy " << wire->getPulseHeight() << std::endl;
    }
  }
  
  // If wireThrs is 0.0 then we need to check if we found at least 1 wire
  // above seed threshold in this collection
  if (maxcharge < seedThrs) {
    // Return an empty WireVec
    if (printWires) cout << "No wires above seedThrs in this TrackBased Collection " << endl;
    WireVec.clear();
  }

  if (printWires) {
    cout << "***************************************************" << endl;
  }

  return WireVec;
}


//---------------------------------------------------------------------
//
// get_ces_based_wires: Find all wires connected with the hit_wire
//
//---------------------------------------------------------------------

const vector<CprWire> CprWireCollectionMaker::get_ces_based_wires
(CprWirePar* MyPar,vector<CesCluster>::const_iterator iters,vector<CesCluster>::const_iterator iterw,
 HepPoint3D primvtx) {

  double wireThrs = MyPar->wireThrs();
  double seedThrs = MyPar->seedThrs();
  bool printWires = MyPar->printWires();
  int numberWires = MyPar->numberWires();
  
  float zloc_str = iters->raw_z_strip();
  float xloc_str = iterw->raw_z_strip();
  _phiwedge = iters->module();
  _barrel = iters->side();
  _primvtx = primvtx;

  vector<CprWire> WireVec;  

  // get xlocal, zlocal
  int success = extrapolate_ces_to_cpr(xloc_str,zloc_str);
  if (success == 0) {
    if (printWires) {
      std::cout << "ces-based : Not within the CPR chambers" << std::endl;
    }
    return WireVec;
  }

  // Chimney Module    
  if (_phiwedge==29 && fabs(_zlocal) > ZHI0_CPR) {
    if (printWires) {
      std::cout << "In chimney Module" << std::endl;
    }
    return WireVec;
  }

  if (fabs(_zlocal) < ZLO0_CPR 
      || (fabs(_zlocal) > ZHI0_CPR && fabs(_zlocal) < ZLO1_CPR)
      || fabs(_zlocal) > ZHI1_CPR
      || fabs(_xlocal) > CPR_WIRE_OFFSET) {
    if (printWires) {
      std::cout << "ces-based : Not within the CPR chambers" << std::endl;
    }
    return WireVec;
  }

  float xloc = _xlocal;
  if (_barrel) xloc = -_xlocal;

  // Get the Wire # hit
  int wire_hit = int ((CPR_WIRE_OFFSET - xloc)/CPR_WIRE_PITCH); // output 0 - 15

  int min_wire = 0;
  int max_wire = 15;
  if (fabs(_zlocal) > CPR_ZCRACK ) {
      min_wire = 16;
      max_wire = 31;
  }    

  if (fabs(_zlocal) > CPR_ZCRACK ) {
    wire_hit = 31 - wire_hit ;
  }
  
  if (wire_hit<0 || wire_hit>31){
    if (printWires) {
      std::cout << " local x position exceeds boundaries - wrong hit_wire " << 
	_xlocal << "  " << wire_hit << std::endl;
    }
    return WireVec;
  }

  if (printWires){
    std::cout << "Initial Wire Hit " << wire_hit << std::endl ; 
  }
  // Look around within +- 2 wires to get the right position
  // (take most energetic wire as wire_hit)
  int start_hit = max(min_wire, wire_hit-2);
  int end_hit = min(max_wire, wire_hit+2);

  int wiremax = wire_hit;
  float emax = _wires[_barrel][_phiwedge][wiremax];
  for (int i=start_hit; i<=end_hit; i++){
    if (_wires[_barrel][_phiwedge][i] > emax) {
      wiremax = i;
      emax = _wires[_barrel][_phiwedge][i];
    }
  }

  // Redifine wire_hit,start_hit,end_hit
  int s1=(numberWires-1)/2; // # of wires on either side of hit wire
  wire_hit = wiremax;
  
  start_hit = max(min_wire, wire_hit-s1);
  end_hit = min(max_wire, wire_hit+s1);


  if (printWires){
    std::cout << "******************Wires***************************" << std::endl;
    std::cout << "Wire Hit " << wire_hit << " Start Hit  " << start_hit << " End Hit  " << end_hit << std::endl ; 
  }

  // Set seed wire No;
  _seed = wire_hit;
  
  float maxcharge = 0.0;
  for (int i=start_hit; i<=end_hit; i++){
    if (_wires[_barrel][_phiwedge][i] > maxcharge) maxcharge=_wires[_barrel][_phiwedge][i];
    CprWire wire;
    wire.setWireNo(i);
    wire.setModule(_phiwedge);
    wire.setSide(_barrel);
    wire.setPulseHeight(_wires[_barrel][_phiwedge][i]);
    WireVec.push_back( wire );
  }


  // At least 1 wire above seed threshold in this collection
  if (maxcharge < seedThrs) {
    // Return an empty WireVec
    if (printWires) cout << "No wires above seedThrs in this TrackBased Collection " << endl;
    WireVec.clear();
  }


  if (printWires){
    for (vector <CprWire>::const_iterator  wire=WireVec.begin(); wire!=WireVec.end(); ++wire) {
      std::cout << 
        " barrel      " << wire->getSide() <<
        " module      " << wire->getModule() << 
        " wire number " << wire->getWireNo() << 
        " wire energy " << wire->getPulseHeight() << std::endl;
    }
    std::cout << "***************************************************" << std::endl;
  }
  
  return WireVec;

}


//---------------------------------------------------------------------
//
// get_seed_based_wires: Find all wires connected with the seed wire.
//
//---------------------------------------------------------------------

const vector<CprWire> CprWireCollectionMaker::get_seed_based_wires
(CprWirePar* MyPar, vector<CprWire>::const_iterator Seed){

  double seedThrs = MyPar->seedThrs();
  double wireThrs = MyPar->wireThrs();
  bool printWires = MyPar->printWires();
  int numberWires = MyPar->numberWires();
  
  vector <CprWire> WireVec;
  // First Check if the wire is used in another cluster

  int wire_hit=Seed->getWireNo();


  if ( wire_hit > 31 || wire_hit < 0) {
    ERRLOG(ELerror,"CprWireCollectionMaker: ") << "Error in wire Hit no." << endmsg;
    return WireVec;
  }

  int module=Seed->getModule();
  int barrel=Seed->getSide();

  int wireId = wire_hit + CPR_NUM_WIRE * module + CPR_NUM_WIRE*CPR_NUM_MOD * barrel;

  // Used Seed Wire
  if (_wireused[wireId]) return WireVec;

  // Set the seed wire
  if (_wires[barrel][module][wire_hit] > seedThrs){
    _seed = wire_hit;
  } else {
    return WireVec;
  }

  // Fill vector of wires
  int min_wire=0;
  int max_wire=0;

  if (wire_hit >= 0 && wire_hit < 16 ) {
    min_wire = 0;
    max_wire = 15;
  }
  else {
    min_wire = 16;
    max_wire = 31;
  }

  int s1 = (numberWires-1)/2; // No. of wires on each side

  int start_hit=max(min_wire, wire_hit-s1);
  int end_hit=min(max_wire, wire_hit+s1);
  
  if (printWires){
    std::cout << "******************Wires***************************" << std::endl;
    std::cout << "No. of wires included : " << (end_hit - start_hit)+1 << std::endl;
    std::cout << "Wire Hit " << wire_hit << " Start Hit  " << start_hit << " End Hit  " << end_hit << std::endl ; 
  }


  for (int i=start_hit; i<=end_hit; i++){
    CprWire wire;      
    wireId = i + CPR_NUM_WIRE * module + CPR_NUM_WIRE*CPR_NUM_MOD * barrel;
    if (_wireused[wireId]) continue;
    wire.setWireNo(i);
    wire.setModule(module);
    wire.setSide(barrel);
    wire.setPulseHeight(_wires[barrel][module][i]);
    WireVec.push_back( wire );
    _wireused[wireId] = 1;
  }

  if (printWires){
    for (vector <CprWire>::const_iterator  wire=WireVec.begin(); wire!=WireVec.end(); ++wire) {
      std::cout << 
        " barrel      " << wire->getSide() <<
        " module      " << wire->getModule() << 
        " wire number " << wire->getWireNo() << 
        " wire energy " << wire->getPulseHeight() << std::endl;
    }
    cout << "***************************************************" << endl;
  }

  return WireVec;
}

const vector<CprWire> CprWireCollectionMaker::get_all_seed_wires() {
  // sort in energy
  std::sort(_seedWire.begin(),_seedWire.end(), WireEless());
  return _seedWire;
}

//---------------------------------------------------------------------
//  get_Xlocal
//---------------------------------------------------------------------
double CprWireCollectionMaker::get_Xlocal(){
  return _xlocal;
}

//---------------------------------------------------------------------
//  get_Zlocal
//---------------------------------------------------------------------
double CprWireCollectionMaker::get_Zlocal(){
  return _zlocal;
}

//---------------------------------------------------------------------
//  Phi_global
//---------------------------------------------------------------------
double CprWireCollectionMaker::Phi_global(){
  return _phi_global;
}

//---------------------------------------------------------------------
//  Eta_global
//---------------------------------------------------------------------
double CprWireCollectionMaker::Eta_global(){
  return _eta_global;
}

//---------------------------------------------------------------------
//  Wedge
//---------------------------------------------------------------------
int CprWireCollectionMaker::get_Wedge(){
  return _phiwedge;
}

//---------------------------------------------------------------------
//  Side
//---------------------------------------------------------------------
int CprWireCollectionMaker::get_Side(){
  return _barrel;
}

void CprWireCollectionMaker::set_seed(double SeedThrs) {
  _seedthrs = SeedThrs;
}

int CprWireCollectionMaker::get_SeedWireNo(){
  return _seed;
}


int CprWireCollectionMaker::extrapolate_ces_to_cpr(float xcesp, float zcesp) {

  int success = 0;
  _xlocal=-999;
  _zlocal=-999;
  //  _eta_global = -999.0;
  //  _phi_global = -999.0;
  //  _barrel = -999.0;
  //  _phiwedge = -999.0;
  
  float x0=_primvtx.x();
  float y0=_primvtx.y();

  // phi module
  float phiwed = ((_phiwedge+0.5)/24.0)*(2*M_PI);

  float R_ces = 183.9;
  float ycesp = sqrt(R_ces*R_ces - xcesp*xcesp);
  // phi at ces. w.r.t to rotated x axis. 
  float phicesp = atan2(ycesp,xcesp);


  // the photon would go in a st. line originating at x0p,
  // hit the CES at an angle phi0p to the ces plane.
  // phi0p would change if its a helical track with finite curvature.
  float x0p = x0*sin(phiwed) - y0*cos(phiwed);
  float phi0p = phicesp + asin(x0p/R_ces);
  float slope = tan(phi0p);
  float cint = ycesp-slope*xcesp;  
  
  // xlocal at CPR satisfies a quadratic equation which is 
  // the intersection of a line at angle phi0p and a circle of radius rcpr.
  float b = 2*slope*cint;
  float a = (1+slope*slope);
  float R_cpr = 168.3;
  float c = cint*cint-R_cpr*R_cpr;
         
  float disc = b*b-4*a*c;
  float xcprp=0.0;
  if (disc>0) {
    float xcprp1 = (-b+sqrt(disc))/(2*a);
    float xcprp2 = (-b-sqrt(disc))/(2*a);
    if (fabs(xcprp1-xcesp)<fabs(xcprp2-xcesp)) {
      xcprp = xcprp1;
    }
    else {
      xcprp = xcprp2;
    }
  }
  else {
    return(success);
  }
  _xlocal = xcprp;


  //  z vertex
  float z0 = _primvtx.z();

  if (_barrel==0) {
    zcesp = -zcesp;
  }
  // A straight line in the s-z plane
  float tt1 = atan2(R_ces,(zcesp-z0));
  float dr = R_ces - R_cpr;
  _zlocal = fabs(zcesp-(dr/tan(tt1)));
  success = 1;

  // Some Printout for debugging
  if (getenv("PRINT_EXPCPR")) {
    std::cout << " Prim. vertex     : " <<  x0 << " " << y0 << " " << z0 << std::endl;
    std::cout << " CES position     : " <<  xcesp << " " << ycesp << " " << zcesp << std::endl;
    std::cout << " Phi,theta at CES : " <<  phi0p << " " << tt1 << std::endl;
    std::cout << " CPR position     : " << _xlocal << " " << _zlocal << std::endl;
  }
  return(success);
}