//-----------------------------------------------------------------------------
// June 20, 2001 A.Dominguez: initialization of the STNTUPLE track/Si
//                            intersection block.  This is after the
//                            similar code by Isi Dunietz
//-----------------------------------------------------------------------------
#include "AbsEnv/AbsEnv.hh"

#include "Stntuple/obj/TStnSiIsect.hh"

#include "Stntuple/obj/TStnEvent.hh"
#include "Stntuple/obj/TSiIsectBlock.hh"
#include "Stntuple/obj/TStnTrackBlock.hh"
#include "Stntuple/obj/TStnTrackLinkBlock.hh"
#include "Stntuple/mod/InitStntupleDataBlocks.hh"

#include "TrackingObjects/Tracks/CdfTrack.hh"
#include "TrackingObjects/Storable/CdfTrackColl.hh"
#include "GeometryBase/CdfDetector.hh"
#include "SiliconGeometry/AbsSiDetectorNode.hh"
#include "TrackingSI/PathFinder/SmarterSiPathFinder.hh"
#include "TrackingSI/PathFinder/SiWaferIntersection.hh"
#include "TrackingSI/PathFinder/SiWaferIntersectionSet.hh"
#include "TrackingUserHL/Utility/CorrectedHitLocations.hh"
#include "CLHEP/Geometry/Point3D.h"
#include <cmath>

#include "ErrorLogger_i/gERRLOG.hh"

//_____________________________________________________________________________
Int_t StntupleInitSiIsect(TStnSiIsect& isect, 
			  const SiWaferIntersection wi, 
			  const CdfTrack& t) {
  // initialize STNTUPLE track intersection starting from SiWaferIntersection
  
  // Local coordinate of intersection
  HepPoint3D loc(wi.getLocalCoordinate());
  // current halfladder touched by the expected trajectory
  const CdfHalfLadder * halfLad =  wi.getWafer();
  // sidigicode for current wafer
  SiDigiCode sidigicode(halfLad->getDetectorCode());

  // Get coordinates of intersection
  // corrected global position for bows and movements of individual wafers
  CorrectedHitLocations corrIsect(t, *halfLad);
  isect.fGlobal = TVector3(corrIsect.getTrkGlobal().x(),
			   corrIsect.getTrkGlobal().y(),
			   corrIsect.getTrkGlobal().z());
  isect.fDirection = TVector3(corrIsect.getTrkDir().x(),
			      corrIsect.getTrkDir().y(),
			      corrIsect.getTrkDir().z());
  float yloc = corrIsect.getTrkLocalPhi();
  float zloc = corrIsect.getTrkLocalZ();
  
  isect.fStripNumPhi = halfLad->toStripUnits(yloc,0);
  if (halfLad->getZStripSpecification()->getNumStrips()>0) {
    isect.fStripNumZ   = halfLad->toStripUnits(zloc,1);
  }
  else {
    isect.fStripNumZ = 0.0;
  }
  isect.fLocY=yloc;
  isect.fLocZ=zloc;

  isect.fDigiCode.fDigiCode = sidigicode.getKey();
  HepPoint3D glo = corrIsect.getTrkGlobal();
  if (halfLad->inActivePhiRegion(glo,0)) isect.fActiveRegion.SetBit(0);
  if (halfLad->inActivePhiRegion(glo,1)) isect.fActiveRegion.SetBit(1);
  if (halfLad->inActiveZRegion(glo,1))   isect.fActiveRegion.SetBit(2);
  if (halfLad->inActiveZRegion(glo,0))   isect.fActiveRegion.SetBit(3);
  isect.fCdfTrackId = t.id().value();

  return 0;
}

//_____________________________________________________________________________
// Now requires the TrackBlock to exist first, so we will fill it in the links
// function below.
Int_t StntupleInitSiIsectBlock(TStnDataBlock* Block, 
			       AbsEvent*      Event, 
			       int            Mode) {
  return 0;
}


// Fill both the intersection block and the link between tracks and intersection
// if requested.
Int_t StntupleSiIsectBlockLinks(TStnDataBlock* Block, 
				AbsEvent*      Event, 
				int            Mode) {
  int ev_number, rn_number;
					// check if block has already been 
					// initialized

  ev_number = AbsEnv::instance()->trigNumber();
  rn_number = AbsEnv::instance()->runNumber();

  if (Block->Initialized(ev_number,rn_number)) return 0;

  TSiIsectBlock* data = (TSiIsectBlock*) Block;
  data->Clear();

  // Initialize the path finder
  static AbsSiDetectorNode const *si(CdfDetector::instance()->getSiDetector());
  SiPathFinder *pathFinder = new SmarterSiPathFinder( si ); // DELETEME!

  TStnEvent* ev = Block->GetEvent();
  TStnTrackBlock* tdata = (TStnTrackBlock*)ev->GetDataBlock("TrackBlock");
  if (! tdata) return -1;

  TStnTrackLinkBlock* tlink;
  tlink = (TStnTrackLinkBlock* ) Block->GetEvent()->GetDataBlock("TrackLinkBlock");


  // Loop over tracks, and then their intersections
  for ( int i=0; i<tdata->NTracks(); i++ ){

    CdfTrack &cdf_track = *tdata->Track(i)->GetCdfTrack();
    SiWaferIntersectionSet * iSet(pathFinder->newIntersectionSet(cdf_track
								 ,0.0 // the full region of halflad
								 ,0.0 // the full region of halflad
								 ));
    if (iSet) {
      SiWaferIntersectionSet::ConstIterator w;
      for (w=iSet->begin();w!=iSet->end();++w) {

	TStnSiIsect* siIsect = data->NewSiIsect(); // new space for 1 TStnSiIsect in TClonesArray
	StntupleInitSiIsect( *siIsect, *w, cdf_track );

	if (tlink) { // fill link b/w track and isect at same time for speed
	  tlink->SiIsectLinkTrk()->Add(i,data->NSiIsects()-1);
	}
      }
    }
  } // Loop over tracks

  if (pathFinder) delete pathFinder;


  data->f_EventNumber = ev_number;
  data->f_RunNumber   = rn_number;

  return 0;
}