////////////////////////////////////////////////////////////////////////
// Package: CandTrackCam
//
// AlgTrackCam.cxx
//
// marshall@hep.phy.cam.ac.uk
////////////////////////////////////////////////////////////////////////

#include <cassert>
#include <cmath>

#include "HitCam.h"
#include "TrackCam.h"

#include "Algorithm/AlgConfig.h"
#include "Algorithm/AlgFactory.h"
#include "Algorithm/AlgHandle.h"

#include "Candidate/CandContext.h"

#include "CandData/CandRecord.h"
#include "CandData/CandHeader.h"

#include "CandTrackCam/AlgTrackCam.h"
#include "CandTrackCam/AlgTrackCam.h"
#include "CandTrackCam/CandTrackCamHandle.h"
#include "CandTrackCam/TrackCam.h"

#include "Calibrator/Calibrator.h"

#include "MessageService/MsgService.h"
#include "MinosObjectMap/MomNavigator.h"

#include "RecoBase/CandSliceHandle.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "RecoBase/AlgTrack.h"
#include "RecoBase/CandSliceHandle.h"
#include "RecoBase/PropagationVelocity.h"

#include "UgliGeometry/UgliGeomHandle.h"
#include "Plex/PlexPlaneId.h"
#include "Validity/VldContext.h"
#include "Conventions/Munits.h"

#include "DataUtil/GetMomFromRange.h"

ClassImp(AlgTrackCam)

CVSID("$Id: AlgTrackCam.cxx,v 1.14 2008/10/24 10:55:23 blake Exp $");


////////////////////////////////////////////////////////////////////////
AlgTrackCam::AlgTrackCam()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
AlgTrackCam::~AlgTrackCam()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
void AlgTrackCam::RunAlg(AlgConfig &ac, CandHandle &ch, CandContext &cx)
{
  // Standard set-up
  ////////////////////////////////
  assert(ch.InheritsFrom("CandTrackCamHandle"));
  CandTrackCamHandle &cth = dynamic_cast<CandTrackCamHandle &>(ch);

  assert(cx.GetDataIn());
  VldContext* vldc = (VldContext*)(cx.GetCandRecord()->GetVldContext());

  // refractive index
  const double SoL = Munits::c_light;
  double fFibreIndex = 1.77;
  fFibreIndex = SoL/PropagationVelocity::Velocity(vldc->GetSimFlag());

  // Unpack AlgConfig
  BeamFlag=true;
  if( ac.KeyExists("BeamFlag") ) {BeamFlag = ac.GetInt("BeamFlag");}
  PassTrack=true;

  // Unpack CandContext
  const TObjArray* arr = dynamic_cast<const TObjArray*>(cx.GetDataIn()); 
  TrackCam* trku = (TrackCam*)(arr->At(0));
  TrackCam* trkv = (TrackCam*)(arr->At(1));
  CandSliceHandle* slice = (CandSliceHandle*)(arr->At(2));
  if(slice) cth.SetCandSlice(slice);

  // Get Geometry
  ////////////////////////////////
  UgliGeomHandle ugh(*vldc);

  switch(vldc->GetDetector()){
  case Detector::kFar:
    ModuleType=1; break;
  case Detector::kNear:
    ModuleType=2; break;
  case Detector::kCalDet:
    ModuleType=3; break;
  default:
    ModuleType=-1; break;
  }
  ////////////////////////////////


  // Find the extent of the track in planes
  ////////////////////////////////
  int i,bpln,epln;
  if(trku->GetBegPlane()<trkv->GetBegPlane()) {bpln=trku->GetBegPlane(); begplane2=trku->GetBegPlane(); begplane1=trkv->GetBegPlane();}
  else {bpln=trkv->GetBegPlane(); begplane2=trkv->GetBegPlane(); begplane1=trku->GetBegPlane();}

  if(trku->GetEndPlane()>trkv->GetEndPlane()) {epln=trku->GetEndPlane(); endplane2=trku->GetEndPlane(); endplane1=trkv->GetEndPlane();} 
  else {epln=trkv->GetEndPlane(); endplane2=trkv->GetEndPlane(); endplane1=trku->GetEndPlane();}

  const int npln = 1+epln-bpln;
  ////////////////////////////////


  // Return if track is clearly very poor quality
  ////////////////////////////////
  if(npln<1 || ((ModuleType==1 || ModuleType==3) && begplane1>endplane1) || (ModuleType==2 && (begplane1-endplane1)>40)) {
    MSG("AlgTrackCam", Msg::kDebug) << "Not enough planes. Not Making track..." <<endl;

    for(int p=0; p<500; ++p) {fHitArr[p].clear();}
    fPlaneInfo.clear();
    return;
  }
  ////////////////////////////////
 

  // Fill the vector with an empty TrkPlaneInfo object for each plane in the range spanned by the track
  ////////////////////////////////
  TrkPlaneInfo tmpplane;
  tmpplane.plnvuw=-1; tmpplane.plnnum=0;
  tmpplane.U=0.0; tmpplane.V=0.0; tmpplane.Z=0.0; tmpplane.Q=0.0; 
  tmpplane.Qm=0.0; tmpplane.Qp=0.0; tmpplane.CTm=0.0; tmpplane.CTp=0.0; 
  tmpplane.Wm=0.0; tmpplane.Wp=0.0;
  for(i=0; i<npln; ++i) {fPlaneInfo.push_back(tmpplane);}
  ////////////////////////////////


  // Calculate Track Co-ordinates
  // Add daughterlinks here
  /////////////////////////////////////
  MaxPlane=-20; MinPlane=500;
  ExtractHitProperties(trku, bpln, cth);
  ExtractHitProperties(trkv, bpln, cth);

  if(PassTrack==false) {
    vector<CandStripHandle*> Daughters;

    TIter TrackStripItr = cth.GetDaughterIterator();
    while(CandStripHandle* TrackStrip = dynamic_cast<CandStripHandle*>(TrackStripItr()))
      {Daughters.push_back(TrackStrip);}

    for(unsigned int i=0; i<Daughters.size(); ++i) {cth.RemoveDaughter(Daughters[i]);}
    Daughters.clear();

    for(int i=0; i<500; ++i) {fHitArr[i].clear();}
    fPlaneInfo.clear();
    return;
  }
  /////////////////////////////////////


  // Finalise charge-weighting of quantities in fPlaneInfo (used below)
  /////////////////////////////////////
  int nplanes=0,nstrips=0;
  for(i=0;i<npln;i++) {
    if(fPlaneInfo[i].plnvuw>-1 && fPlaneInfo[i].Q>0.0) {
      fPlaneInfo[i].U/=fPlaneInfo[i].Q; fPlaneInfo[i].V/=fPlaneInfo[i].Q;
      nstrips+=fPlaneInfo[i].plnnum; ++nplanes;
    }
  }
  /////////////////////////////////////


  // Temporary quantities used in timing fit
  /////////////////////////////////////
  // Interpolate to get V for U planes and vice versa
  InterpolateTrackPosition();


  // Calculate The Timing Info For Each Track StripEnd
  SetupTimingInfo(&ugh, fFibreIndex);

  
  // Set U, V and time for each plane/stripend in the track
  int plnm=-1; int plnp=-1;
  SetTrackCoordinates(bpln,plnm,plnp);


  // Beginning Position and Direction
  double begCoord[3], begDir[3];
  SetVertexEndProperties(begCoord,begDir,0,plnm);

  // End Position and Direction
  double endCoord[3], endDir[3];
  SetVertexEndProperties(endCoord,endDir,1,plnp);
  /////////////////////////////////////



  // Calculate a simple dS and range, assuming track is travelling in positive Z direction
  /////////////////////////////////////
  double utmp(0.),vtmp(0.),ztmp(0.);
  double dq,du,dv,dz,ds; 
  double range_thru_detector(0.),range_thru_steel(0.);
  double sumz_thru_steel(0.),sumz_thru_detector(0.);
  double range_thru_steel_xtra(0.), range_thru_detector_xtra(0.);
  int flag=0; 
  for(i=0;i<npln;i++) {
    if(fPlaneInfo[i].plnvuw>-1) {
      if(flag) {
	du=fPlaneInfo[i].U-utmp; dv=fPlaneInfo[i].V-vtmp; dz=fPlaneInfo[i].Z-ztmp;
	ds=sqrt(du*du+dv*dv+dz*dz);
	if(dz>1.0) dq=2.0; else dq=1.0;
	range_thru_steel+=dq*ds*(0.0254/dz);
	sumz_thru_steel+=dq*0.0254;
	range_thru_detector+=ds;
	sumz_thru_detector+=dz;
      }
      fPlaneInfo[i].dS=range_thru_detector;
      fPlaneInfo[i].dSsteel=range_thru_steel;
      utmp=fPlaneInfo[i].U; vtmp=fPlaneInfo[i].V; ztmp=fPlaneInfo[i].Z;
      flag=1;
    }
  }

  if(range_thru_detector>0.0) {
    range_thru_steel_xtra = 0.0127*(1.0/begDir[2]+1.0/endDir[2]);
    range_thru_detector_xtra = 0.0296*(1.0/begDir[2]+1.0/endDir[2]);
    range_thru_steel+=range_thru_steel_xtra;
    range_thru_detector+=range_thru_detector_xtra;
  }
  /////////////////////////////////////



  // If we are considering Far Detector, calculate Beg & End traces  
  /////////////////////////////////////
  double begtrace(0.),endtrace(0.);
  double begtraceZ(0.),endtraceZ(0.);

  if(vldc->GetDetector()==Detector::kFar) {
    // For beginning
    double m[2] = {begDir[0]/begDir[2], begDir[1]/begDir[2]};
    double c[2] = {begCoord[0]-(m[0]*begCoord[2]), begCoord[1]-(m[1]*begCoord[2])};
    double Coord[3] = {begCoord[0], begCoord[1], begCoord[2]};
    double Trace[3] = {1.e99,1.e99,1.e99};
    
    bool TraceSuccess = CalculateTrace(m,c,Coord,Trace);
    if(TraceSuccess==true) {
      begtrace = pow(pow(Trace[0],2) + pow(Trace[1],2) + pow(Trace[2],2), 0.5);
      begtraceZ = fabs(Trace[2]);
    }

    
    // For end
    m[0] = endDir[0]/endDir[2];
    m[1] = endDir[1]/endDir[2];
    c[0] = endCoord[0]-(m[0]*endCoord[2]);
    c[1] = endCoord[1]-(m[1]*endCoord[2]);
    Coord[0] = endCoord[0]; Coord[1] = endCoord[1]; Coord[2] = endCoord[2];
    Trace[0] = 1.e99; Trace[1]=1.e99; Trace[2]=1.e99;
    
    TraceSuccess = CalculateTrace(m,c,Coord,Trace);
    if(TraceSuccess==true) {
      endtrace = pow(pow(Trace[0],2) + pow(Trace[1],2) + pow(Trace[2],2), 0.5);
      endtraceZ = fabs(Trace[2]);
    }
  }
  /////////////////////////////////////


  // Timing Calculations
  /////////////////////////////////////
  double dir(0.), mytimeoffset(0.);
  CalculateTimingFits(dir,mytimeoffset,cth);
  /////////////////////////////////////


  //Set Properties In CandTrackHandle - (Can Do This Now We Know The Direction From Timing)
  /////////////////////////////////////
  SetUVZ(&cth);

  int begplane, endplane;
  double dt=1.;

  if(dir>=0 || BeamFlag) {
    begplane=MinPlane;
    endplane=MaxPlane;

    cth.SetVtxTrace(begtrace);
    cth.SetVtxTraceZ(begtraceZ);

    cth.SetEndTrace(endtrace);
    cth.SetEndTraceZ(endtraceZ);

    cth.SetVtxT((mytimeoffset+StripListTime)/(3.0e8));
    cth.SetEndT((mytimeoffset+range_thru_detector+StripListTime)/(3.0e8));
  }
  else{
    begplane=MaxPlane;
    endplane=MinPlane;

    cth.SetVtxTrace(endtrace);
    cth.SetVtxTraceZ(endtraceZ);

    cth.SetEndTrace(begtrace);
    cth.SetEndTraceZ(begtraceZ);

    cth.SetVtxT((mytimeoffset-range_thru_detector+StripListTime)/(3.0e8));
    cth.SetEndT((mytimeoffset+StripListTime)/(3.0e8));

    dt=-1.;
  }

  cth.SetVtxPlane(begplane);
  cth.SetVtxZ(cth.GetZ(begplane));
  cth.SetVtxU(cth.GetU(begplane));
  cth.SetVtxV(cth.GetV(begplane));
  cth.SetDirCosU(dt*cth.GetDirCosU(begplane));
  cth.SetDirCosV(dt*cth.GetDirCosV(begplane));
  cth.SetDirCosZ(dt*cth.GetDirCosZ(begplane));
  
  cth.SetEndPlane(endplane);
  cth.SetEndZ(cth.GetZ(endplane));
  cth.SetEndU(cth.GetU(endplane));
  cth.SetEndV(cth.GetV(endplane));
  cth.SetEndDirCosU(dt*cth.GetDirCosU(endplane));
  cth.SetEndDirCosV(dt*cth.GetDirCosV(endplane));
  cth.SetEndDirCosZ(dt*cth.GetDirCosZ(endplane));


  // Store final time values
  SetT(&cth);

  // Set Range and dS
  SetdS(&cth);


  // Momentum From Range
  double range = cth.GetRange();
  if (range>0.) {cth.SetMomentum(GetMomFromRange(range));}
  else {cth.SetMomentum(0.);}

  // Calibration, for MIPs etc.
  Calibrator& cal = Calibrator::Instance();
  cal.ReInitialise(*vldc);
  Calibrate(&cth);
  /////////////////////////////////////


  for(i=0; i<500; ++i) {fHitArr[i].clear();}
  fPlaneInfo.clear();


  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::SetVertexEndProperties( double* Coord,  double* Dir,const int& end, const int& coordpln)
{
  const int npln = (int)fPlaneInfo.size();

  //Sanity check
  if(coordpln<0 || coordpln>=npln) return; //don't try to access elements for a non existent plane
  
  double mu(0.0),mv(0.0),u,v,z,w,precou,precov,precos,precoz;
  double Uw(0.0),Uwz(0.0),Uwzz(0.0),Uwu(0.0),Uwuu(0.0),Uwzu(0.0);
  double Vw(0.0),Vwz(0.0),Vwzz(0.0),Vwv(0.0),Vwvv(0.0),Vwzv(0.0);

  int Upts(0),Vpts(0),MAXpts(4);
  int increment=(end==0)?1:-1;
  double extrapdist=(end==0)?-0.02:0.04; //extra distance we have to extrapolate the track
  int i=coordpln;
  while(i>=0 && i<npln)
    {
      if(fPlaneInfo[i].plnvuw>-1)
	{
	  u=fPlaneInfo[i].U; v=fPlaneInfo[i].V; z=fPlaneInfo[i].Z; w=1.0; // remove pulse height weighting
	  
	  if(fPlaneInfo[i].plnvuw==0 && Upts<MAXpts)
	    {
	      Uw+=w;
	      Uwu+=w*u; Uwz+=w*z; Uwuu+=w*u*u;
	      Uwzu+=w*u*z; Uwzz+=w*z*z;
	      ++Upts;
	    }
	  
	  if(fPlaneInfo[i].plnvuw==1 && Vpts<MAXpts)
	    {
	      Vw+=w;
	      Vwv+=w*v; Vwz+=w*z; Vwvv+=w*v*v;
	      Vwzv+=w*v*z; Vwzz+=w*z*z;
	      ++Vpts;
	    }
	}
      if(Upts==MAXpts && Vpts==MAXpts) break;
      i+=increment;
    }
  
  if(Upts>1 && Vpts>1)
    {
      mu=(Uw*Uwzu-Uwz*Uwu)/(Uw*Uwzz-Uwz*Uwz);
      mv=(Vw*Vwzv-Vwz*Vwv)/(Vw*Vwzz-Vwz*Vwz);
    }
  
  precou=mu; precov=mv;
  precos=sqrt(precou*precou+precov*precov+1.0);
  precou=precou/precos; precov=precov/precos; precoz=1.0/precos;
  Dir[0]=precou; Dir[1]=precov; Dir[2]=precoz;

  Coord[0]=fPlaneInfo[coordpln].U+extrapdist*(Dir[0]/Dir[2]); 
  Coord[1]=fPlaneInfo[coordpln].V+extrapdist*(Dir[1]/Dir[2]); 
  Coord[2]=fPlaneInfo[coordpln].Z+extrapdist;

  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::InterpolateTrackPosition()
{
  // interpolate between views for U,V
  const int npln = (int)fPlaneInfo.size();

  int flag,vuw;
  int pln;
  int k,km1,kp1,km2,kp2;

  for(pln=0;pln<npln;pln++) {

    if(fPlaneInfo[pln].plnvuw>-1) {
      flag=0;
      vuw=fPlaneInfo[pln].plnvuw;
      k=0; km1=-1; km2=-1;

      while(pln-k>0 && km2<0) {
	k++; 
	if(fPlaneInfo[pln-k].plnvuw>-1 && fPlaneInfo[pln-k].plnvuw!=vuw) {
	  if(km1<0) km1=k; else km2=k;
	}
      }
	
      k=0; kp1=-1; kp2=-1;

      while(pln+k<npln-1 && kp2<0) {
	k++;

	if(fPlaneInfo[pln+k].plnvuw>-1 && fPlaneInfo[pln+k].plnvuw!=vuw) {
	  if(kp1<0) kp1=k; else kp2=k;
	}
      }

      if(km1>0 && kp1>0) { 
	if(vuw==0) fPlaneInfo[pln].V=fPlaneInfo[pln-km1].V
		     +((fPlaneInfo[pln+kp1].V-fPlaneInfo[pln-km1].V)/(fPlaneInfo[pln+kp1].Z-fPlaneInfo[pln-km1].Z))
		     *(fPlaneInfo[pln].Z-fPlaneInfo[pln-km1].Z);

	if(vuw==1) fPlaneInfo[pln].U=fPlaneInfo[pln-km1].U
		     +((fPlaneInfo[pln+kp1].U-fPlaneInfo[pln-km1].U)/(fPlaneInfo[pln+kp1].Z-fPlaneInfo[pln-km1].Z))
		     *(fPlaneInfo[pln].Z-fPlaneInfo[pln-km1].Z);
	flag=1;
      }
      else {
	if(km1>0) {
	  if(vuw==0) {
	    if(km2>0) {fPlaneInfo[pln].V=fPlaneInfo[pln-km1].V
			 +(fPlaneInfo[pln].Z-fPlaneInfo[pln-km1].Z)
			 *(fPlaneInfo[pln-km1].V-fPlaneInfo[pln-km2].V)/(fPlaneInfo[pln-km1].Z-fPlaneInfo[pln-km2].Z);}
	    else {fPlaneInfo[pln].V=fPlaneInfo[pln-km1].V;}
	  }
	  
	  if(vuw==1) {
	    if(km2>0) {fPlaneInfo[pln].U=fPlaneInfo[pln-km1].U
			 +(fPlaneInfo[pln].Z-fPlaneInfo[pln-km1].Z)
			 *(fPlaneInfo[pln-km1].U-fPlaneInfo[pln-km2].U)/(fPlaneInfo[pln-km1].Z-fPlaneInfo[pln-km2].Z);}
	    else {fPlaneInfo[pln].U=fPlaneInfo[pln-km1].U;}
	  }

	  flag=1;
	}
	 
	if(kp1>0) {
	  if(vuw==0) {
	    if(kp2>0) {fPlaneInfo[pln].V=fPlaneInfo[pln+kp1].V
			 +(fPlaneInfo[pln].Z-fPlaneInfo[pln+kp1].Z)
			 *(fPlaneInfo[pln+kp1].V-fPlaneInfo[pln+kp2].V)/(fPlaneInfo[pln+kp1].Z-fPlaneInfo[pln+kp2].Z);}
	    else {fPlaneInfo[pln].V=fPlaneInfo[pln+kp1].V;}
	  }
	  
	  if(vuw==1) {
	    if(kp2>0) {fPlaneInfo[pln].U=fPlaneInfo[pln+kp1].U
			 +(fPlaneInfo[pln].Z-fPlaneInfo[pln+kp1].Z)
			 *(fPlaneInfo[pln+kp1].U-fPlaneInfo[pln+kp2].U)/(fPlaneInfo[pln+kp1].Z-fPlaneInfo[pln+kp2].Z);}
	    else {fPlaneInfo[pln].U=fPlaneInfo[pln+kp1].U;}
	  }
	  
	  flag=1;
	}
      }
      
      if(flag==0){ fPlaneInfo[pln].plnvuw=-1; }
    }
  }
  
  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::ExtractHitProperties(const TrackCam* trkc, const int& bpln, CandTrackHandle& cth)
{
  //Loop over all the HitCam objects in this TrackCam object
  //Extract U/V/Z postion info as will as charge from each one
  //Add daughter links for the strips they represent to the CandTrackHandle.
  unsigned int nhits = trkc->GetEntries();
  int pln, hitplane;
  double PartnerBegTPos, PartnerEndTPos;

  int Counter=0;

  TrackCam* Partner = trkc->GetPartner();
  PartnerBegTPos=fabs(Partner->GetBegTPos());
  PartnerEndTPos=fabs(Partner->GetEndTPos());


  for(unsigned int i=0; i<nhits; ++i) {
    HitCam* hit = trkc->GetHit(i);

    if(hit->GetUID()<0) {continue;}

    hitplane=hit->GetPlane();
    
    // UV matching for FD cosmics
    if( ModuleType==1 && !BeamFlag && ( (PartnerBegTPos<3.9 && PartnerBegTPos>0.2 && hitplane<begplane1-4) 
					|| (PartnerEndTPos<3.9 && PartnerEndTPos>0.2 && hitplane>endplane1+4) ) ) {continue;}
    
    CandStripHandle* strip = hit->GetCandStripHandle();
    
    pln = hitplane-bpln;
    fHitArr[pln].push_back(hit);
    if(fPlaneInfo[pln].plnvuw<0) {
      fPlaneInfo[pln].plnvuw = hit->GetPlaneView();
      fPlaneInfo[pln].Z = hit->GetZPos();
    }

    if(fPlaneInfo[pln].plnvuw==0) fPlaneInfo[pln].U += hit->GetCharge()*hit->GetTPos();
    if(fPlaneInfo[pln].plnvuw==1) fPlaneInfo[pln].V += hit->GetCharge()*hit->GetTPos();
    fPlaneInfo[pln].Q += hit->GetCharge();
    cth.AddDaughterLink(*strip); 
    Counter++;

    if(hitplane>MaxPlane) {MaxPlane=hitplane;}
    if(hitplane<MinPlane) {MinPlane=hitplane;}

    (fPlaneInfo[pln].plnnum)++; 
  }

  if(Counter<2) {PassTrack=false;}


  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
bool AlgTrackCam::CalculateTrace(double* m, double* c, double* Coord, double* Trace)
{ 
  MSG("AlgTrackCam",Msg::kDebug) << "CalculateTrace" << endl;

  bool TraceSuccess=false;
  double TempTrace[3]={0.,0.,0.}; double TempSum=0.;
  double TraceLength(1.e99);

  for(int i=0; i<8; ++i) { // Consider the eight sides in turn
    TempSum=0.;
      
    DetectorSides(m,c,TempTrace, i);
      
    for(int j=0; j<3; ++j) {TempSum+=pow(Coord[j]-TempTrace[j],2);}
    TempSum=pow(TempSum,0.5);
      
    if(TempSum<TraceLength) {
      for(int j=0; j<3; ++j) {Trace[j]=TempTrace[j]-Coord[j];}
      TraceLength=TempSum;
      TraceSuccess=true;
    }
  }

  return TraceSuccess;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::DetectorSides(double* m, double* c, double* position, int side)
{
  position[2]=-1.e99;
  
  switch(side) {
  case 0: if(m[0]!=-m[1]) {position[2] = (pow(32.,0.5)-c[0]-c[1])/(m[0]+m[1]);}  break;
  case 1: if(m[0]!=0.)    {position[2] = (4.-c[0])/m[0];}                        break;
  case 2: if(m[0]!=m[1])  {position[2] = (pow(32.,0.5)-c[0]+c[1])/(m[0]-m[1]);}  break;
  case 3: if(m[1]!=0.)    {position[2] = (-4.-c[1])/m[1];}                       break;
  case 4: if(m[0]!=-m[1]) {position[2] = (-pow(32.,0.5)-c[0]-c[1])/(m[0]+m[1]);} break;
  case 5: if(m[0]!=0.)    {position[2] = (-4.-c[0])/m[0];}                       break;
  case 6: if(m[0]!=m[1])  {position[2] = (-pow(32.,0.5)-c[0]+c[1])/(m[0]-m[1]);} break;
  case 7: if(m[1]!=0.)    {position[2] = (4.-c[1])/m[1];}                        break;
  default: position[2] = -1.e99; break;
  }
  
  if(position[2]!=-1.e99) {
    position[0]=m[0]*position[2]+c[0];
    position[1]=m[1]*position[2]+c[1];  
  }
  else {position[0]=-1.e99; position[1]=-1.e99;}

  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::SetupTimingInfo(UgliGeomHandle* ugh, const double& fFibreIndex)
{
  double tpos(0.), fibre, ctime, digchg;

  HitCam* hit = 0; CandStripHandle* strip = 0;
  const unsigned int npln = fPlaneInfo.size();

  // Time of first strip in track
  StripListTime=9.e30;

  for(unsigned int pln=0;pln<npln;pln++) {

    for(unsigned int g=0; g<fHitArr[pln].size(); ++g) {
      hit = fHitArr[pln][g];
      strip = hit->GetCandStripHandle();  

      if( strip->GetPlaneView()==PlaneView::kU
	  || strip->GetPlaneView()==PlaneView::kX ) tpos = -fPlaneInfo[pln].V;
      if( strip->GetPlaneView()==PlaneView::kV
	  || strip->GetPlaneView()==PlaneView::kY ) tpos = fPlaneInfo[pln].U;
      PlexStripEndId stripid = strip->GetStripEndId();
      UgliStripHandle striphandle = ugh->GetStripHandle(stripid);
      
      if(strip->GetNDigit(StripEnd::kPositive)>0) { 
	fibre = striphandle.ClearFiber(StripEnd::kPositive)+striphandle.WlsPigtail(StripEnd::kPositive)+striphandle.GetHalfLength()-tpos; 
	ctime = 3.0e8*strip->GetTime(StripEnd::kPositive)-fFibreIndex*fibre; 
	digchg=strip->GetCharge(StripEnd::kPositive); 
	fPlaneInfo[pln].Qp+=digchg; fPlaneInfo[pln].CTp+=digchg*ctime;

	if(ctime<StripListTime) {StripListTime=ctime;}
      }
      
      if(strip->GetNDigit(StripEnd::kNegative)>0) { 
	fibre = striphandle.ClearFiber(StripEnd::kNegative)+striphandle.WlsPigtail(StripEnd::kNegative)+striphandle.GetHalfLength()+tpos; 
	ctime = 3.0e8*strip->GetTime(StripEnd::kNegative)-fFibreIndex*fibre; 
	digchg=strip->GetCharge(StripEnd::kNegative); 
	fPlaneInfo[pln].Qm+=digchg; fPlaneInfo[pln].CTm+=digchg*ctime;

	if(ctime<StripListTime) {StripListTime=ctime;}
      }   
    }
  }

  // Subtract StripListTime from each time measurement
  if(StripListTime<8.e30) {
    for(unsigned int pln=0;pln<npln;pln++) {
      if(fPlaneInfo[pln].plnvuw>-1) {
	fPlaneInfo[pln].CTp-=StripListTime*fPlaneInfo[pln].Qp;
	fPlaneInfo[pln].CTm-=StripListTime*fPlaneInfo[pln].Qm;
      }
    }
  }
  else {StripListTime=0.;}
  
  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::WeightsForTimingFits()
{
  // Timing Weights  
  // Parameters for time fit residual vs PEs
  // (consistent with AlgTrackFitCam::TimingFit(...) method)
  // double ErrorParam[3] = { 0.56, 0.50, -0.34 }; // (OLD)
  double ErrorParam[3] = { 0.58, 0.69, -0.33 }; // (NEW)
  double MinUncertainty = ErrorParam[0];
  double Uncertainty = MinUncertainty;
 
  const int npln = (int)fPlaneInfo.size();

  int ctflag;
  double ctmin,ctmax,ctave,ctrms;
  double sq,sqct,sqctct,swgt;
  double wm,wp;

  for(int i=0;i<npln;i++) {    

    fPlaneInfo[i].Wm = 0.0;
    fPlaneInfo[i].Wp = 0.0;

    if(fPlaneInfo[i].plnvuw>-1) {
      wm=0.0; wp=0.0;
      ctflag=0; ctmin=0.0; ctmax=0.0; ctave=0.0; ctrms=0.0;
      sq=0.0; sqct=0.0; sqctct=0.0; swgt=0.0;

      for(int j=-4;j<5;j++) {
	if( j!=0 && i+j>-1 && i+j<npln && fPlaneInfo[i+j].plnvuw>-1 ) {
	  if( fPlaneInfo[i+j].Qm>1.0 && !( fPlaneInfo[i+j].Qp>0.0 && !(fPlaneInfo[i+j].CTm-fPlaneInfo[i+j].CTp>-4.0&&fPlaneInfo[i+j].CTm-fPlaneInfo[i+j].CTp<4.0) ) ) {
	    if(ctflag) {
	      if(fPlaneInfo[i+j].CTm<ctmin) {ctmin=fPlaneInfo[i+j].CTm;}
	      if(fPlaneInfo[i+j].CTm>ctmax) {ctmax=fPlaneInfo[i+j].CTm;} 
	    }
	    else {ctmin=fPlaneInfo[i+j].CTm; ctmax=fPlaneInfo[i+j].CTm; ctflag=1;}

	    sq+=fPlaneInfo[i+j].Qm; sqct+=fPlaneInfo[i+j].Qm*fPlaneInfo[i+j].CTm; 
	    sqctct+=fPlaneInfo[i+j].Qm*fPlaneInfo[i+j].CTm*fPlaneInfo[i+j].CTm;
	    swgt+=1.0;
	  }
		
	  if( fPlaneInfo[i+j].Qp>1.0 && !( fPlaneInfo[i+j].Qm>0.0 && !(fPlaneInfo[i+j].CTp-fPlaneInfo[i+j].CTm>-4.0&&fPlaneInfo[i+j].CTp-fPlaneInfo[i+j].CTm<4.0) ) ) {
	    if(ctflag) {
	      if(fPlaneInfo[i+j].CTp<ctmin) {ctmin=fPlaneInfo[i+j].CTp;} 
	      if(fPlaneInfo[i+j].CTp>ctmax) {ctmax=fPlaneInfo[i+j].CTp;}
	    }
	    else {ctmin=fPlaneInfo[i+j].CTp; ctmax=fPlaneInfo[i+j].CTp; ctflag=1;}
	    
	    sq+=fPlaneInfo[i+j].Qp; sqct+=fPlaneInfo[i+j].Qp*fPlaneInfo[i+j].CTp; 
	    sqctct+=fPlaneInfo[i+j].Qp*fPlaneInfo[i+j].CTp*fPlaneInfo[i+j].CTp;
	    swgt+=1.0;
	  }
	}
      }


      if(swgt>1.0) {
	ctave=sqct/sq; ctrms=0.0;
	if((sqctct/sq)-(sqct/sq)*(sqct/sq)>0.0) {ctrms=sqrt((sqctct/sq)-(sqct/sq)*(sqct/sq));}

	if( (fPlaneInfo[i].Qm>1.0) && (fPlaneInfo[i].CTm-ctmin>-2.0 && fPlaneInfo[i].CTm-ctmax<2.0) 
	    && (fPlaneInfo[i].CTm-ctave<ctrms+3.0 && fPlaneInfo[i].CTm-ctave>-ctrms-3.0) ) 
	  {wm=fPlaneInfo[i].Qm;}
	
	if( (fPlaneInfo[i].Qp>1.0) && (fPlaneInfo[i].CTp-ctmin>-2.0 && fPlaneInfo[i].CTp-ctmax<2.0) 
	    && (fPlaneInfo[i].CTp-ctave<ctrms+3.0 && fPlaneInfo[i].CTp-ctave>-ctrms-3.0) )
	  {wp=fPlaneInfo[i].Qp;}
      }
  
      if( wm>0.0 ){
        if( wm<25 ) { Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*wm); }
        else{ Uncertainty=MinUncertainty; }
        fPlaneInfo[i].Wm = 1.0/pow(Uncertainty,2.0);
        // fPlaneInfo[i].Wm=wm; // (old linear weighting)
      }

      if( wp>0.0 ){
        if (wp<25 ){ Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*wp); }
        else { Uncertainty=MinUncertainty; }
        fPlaneInfo[i].Wp = 1.0/pow(Uncertainty,2.0);
        // fPlaneInfo[i].Wp=wp; // (old linear weighting)
      }

    }
  }

  return;
} 
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::SetTrackCoordinates(const int& bpln, int& plnm, int& plnp/*, CandTrackHandle& cth*/)
{
  int pln;
  const int npln = (int)fPlaneInfo.size();

  for(int i=0;i<npln;i++) {
    pln=bpln+i;
    if(fPlaneInfo[i].plnvuw>-1) {
      if(fPlaneInfo[i].Qm>0.0) {
	fPlaneInfo[i].CTm/=fPlaneInfo[i].Qm;
      }
      if(fPlaneInfo[i].Qp>0.0) {
	fPlaneInfo[i].CTp/=fPlaneInfo[i].Qp;
      }
      if(plnm<0||i<plnm) {plnm=i;} 
      if(plnp<0||i>plnp) {plnp=i;}
    }
  }
  
  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::CalculateTimingFits(double& dir, double& mytimeoffset, CandTrackHandle& cth)
{
  WeightsForTimingFits();
  const int npln = (int)fPlaneInfo.size();

  //Timeslope
  double ctimeslope=0.0,ctimeoffset=0.0,ctimeaverage=0.0, RMS=0.0, CTCut=0.0, MinCT=-3000.;
  double Sq,Sqs,Sqss,Sqt,Sqtt,Sqst;
  double s,t,w;
  int npts=0;
  int* SkipM = new int[npln];
  int* SkipP = new int[npln];
  for(int i=0;i<npln;i++) { SkipM[i]=0; SkipP[i]=0; }
  Sq=0.0; Sqs=0.0; Sqss=0.0; Sqt=0.0; Sqtt=0.0; Sqst=0.0;


  // On first iteration, carry out simple fit. Remove outlying points on subsequent passes.
  for(int itr=0; itr<3; ++itr) {

    for(int i=0;i<npln;i++) {

      if( fPlaneInfo[i].plnvuw>-1 ) {

	// For negative strip ends
	if( fPlaneInfo[i].Wm>0.0 && fPlaneInfo[i].CTm>MinCT ) {
	  s=fPlaneInfo[i].dS; t=fPlaneInfo[i].CTm; w=fPlaneInfo[i].Wm;

	  if(itr==0) {
	    Sqs+=w*s; Sqss+=w*s*s; Sqt+=w*t; Sqtt+=w*t*t; Sqst+=w*s*t;
	    Sq+=w; npts++;
	  }
	  else if(fabs(t-ctimeoffset-(s*ctimeslope))>CTCut && SkipM[i]==0) {
	    Sqs-=w*s; Sqss-=w*s*s; Sqt-=w*t; Sqtt-=w*t*t; Sqst-=w*s*t;
	    Sq-=w; npts--; SkipM[i]=1;
	  }
	}

	// For positive strip ends
	if( fPlaneInfo[i].Wp>0.0 && fPlaneInfo[i].CTp>MinCT ) {
	  s=fPlaneInfo[i].dS; t=fPlaneInfo[i].CTp; w=fPlaneInfo[i].Wp;

	  if(itr==0) {
	    Sqs+=w*s; Sqss+=w*s*s; Sqt+=w*t; Sqtt+=w*t*t; Sqst+=w*s*t;
	    Sq+=w; npts++;
	  }
	  else if(fabs(t-ctimeoffset-(s*ctimeslope))>CTCut && SkipP[i]==0) {
	    Sqs-=w*s; Sqss-=w*s*s; Sqt-=w*t; Sqtt-=w*t*t; Sqst-=w*s*t;
	    Sq-=w; npts--; SkipP[i]=1;
	  }
	}

      }
    }
  
    if(npts>2){
      ctimeslope=(Sq*Sqst-Sqs*Sqt)/(Sq*Sqss-Sqs*Sqs); 
      ctimeoffset=(Sqt*Sqss-Sqs*Sqst)/(Sq*Sqss-Sqs*Sqs);
      ctimeaverage=(Sqt/Sq);
      if( ((Sqtt/Sq)-((Sqt/Sq)*(Sqt/Sq)))>0.0 ) {
	RMS = pow((Sqtt/Sq)-((Sqt/Sq)*(Sqt/Sq)),0.5);
	CTCut = 3.+RMS;
      }
      else {CTCut = 3.5;}
    }

  }

  delete[] SkipM;
  delete[] SkipP;
  //////////////////////////////


  //Fits to S = cT0 +ct and S = cT0 -cT
  double Sw[2],Swt[2],Swtt[2];
  double chisqp=0.0,chisqm=0.0;
  int chisqndfp=0,chisqndfm=0;
  double time_score=0.0;
  double CTIntercept[2], Csigma[2],Ctrunc[2]; 
  double cp,cm;
  int Npts[2];
  MSG("AlgTrackCam", Msg::kDebug) << " POSITIVE FIT: " << endl;
  chisqp=-99999.9; cp=-99999.9;
  chisqm=-99999.9; cm=-99999.9;
  CTIntercept[0]=ctimeaverage; Csigma[0]=-99999.9; Ctrunc[0]=-99999.9;
  CTIntercept[1]=ctimeaverage; Csigma[1]=-99999.9; Ctrunc[1]=-99999.9;

  for(int itr=0;itr<2;itr++)
    {
      Swtt[0]=0.0; Swt[0]=0.0; Sw[0]=0.0; Npts[0]=0;
      Swtt[1]=0.0; Swt[1]=0.0; Sw[1]=0.0; Npts[1]=0;
      for(int pln=0;pln<npln;pln++)
	{
	  if( fPlaneInfo[pln].plnvuw>-1 )
	    {

	      if( fPlaneInfo[pln].Wm>0.0 && fPlaneInfo[pln].CTm>MinCT )
		{
		  w=fPlaneInfo[pln].Wm; 
		  //Fit to S = cT0 +ct
		  t=fPlaneInfo[pln].CTm-fPlaneInfo[pln].dS+CTIntercept[0];
		  if( Ctrunc[0]<0.0 || fabs(t)<Ctrunc[0] ) { Swtt[0]+=w*t*t; Swt[0]+=w*t; Sw[0]+=w; Npts[0]++; }
		  //Fit to S = cT0 -ct
		  t=fPlaneInfo[pln].CTm+fPlaneInfo[pln].dS+CTIntercept[1];
		  if( Ctrunc[1]<0.0 || fabs(t)<Ctrunc[1] ) { Swtt[1]+=w*t*t; Swt[1]+=w*t; Sw[1]+=w; Npts[1]++; }
		}
	      if( fPlaneInfo[pln].Wp>0.0 && fPlaneInfo[pln].CTp>MinCT )
		{
		  w=fPlaneInfo[pln].Wp; 
		  //Fit to S = cT0 +ct
		  t=fPlaneInfo[pln].CTp-fPlaneInfo[pln].dS+CTIntercept[0];
		  if( Ctrunc[0]<0.0 || fabs(t)<Ctrunc[0] ) { Swtt[0]+=w*t*t; Swt[0]+=w*t; Sw[0]+=w; Npts[0]++; }
		  //Fit to S = cT0 -ct
		  t=fPlaneInfo[pln].CTp+fPlaneInfo[pln].dS+CTIntercept[1];
		  if( Ctrunc[1]<0.0 || fabs(t)<Ctrunc[1] ) { Swtt[1]+=w*t*t; Swt[1]+=w*t; Sw[1]+=w; Npts[1]++; }
		}
	    }
	}
      if(Npts[0]>1)//Fit to S = cT0 +ct
	{
	  CTIntercept[0]=CTIntercept[0]-Swt[0]/Sw[0]; Csigma[0]=0.0; 
	  if((Swtt[0]/Sw[0])-(Swt[0]/Sw[0])*(Swt[0]/Sw[0])>0.0)
	    {
	      Csigma[0]=sqrt((Swtt[0]/Sw[0])-(Swt[0]/Sw[0])*(Swt[0]/Sw[0]));
	    }
	  chisqp=Csigma[0]; chisqndfp=Npts[0]-1; cp=-CTIntercept[0];
	  Ctrunc[0]=Csigma[0]+3.0;
	}
      if(Npts[1]>1)//Fit to S = cT0 -ct
	{
	  CTIntercept[1]=CTIntercept[1]-Swt[1]/Sw[1]; Csigma[1]=0.0;
	  if((Swtt[1]/Sw[1])-(Swt[1]/Sw[1])*(Swt[1]/Sw[1])>0.0)
	    {
	      Csigma[1]=sqrt((Swtt[1]/Sw[1])-(Swt[1]/Sw[1])*(Swt[1]/Sw[1]));
	    }
	  chisqm=Csigma[1]; chisqndfm=Npts[1]-1; cm=-CTIntercept[1];
	  Ctrunc[1]=Csigma[1]+3.0;
	}
    }
  MSG("AlgTrackCam", Msg::kDebug) << " *** CTIntercept[0]=" << CTIntercept[0] << " Csigma[0]=" << Csigma[0] << " *** " << endl;
  MSG("AlgTrackCam", Msg::kDebug) << " *** CTIntercept[1]=" << CTIntercept[1] << " Csigma[1]=" << Csigma[1] << " *** " << endl;
  

  dir=0.0; 
  time_score=0.0; mytimeoffset=0.0;
  if(chisqm>0.0 && chisqp>0.0)
    {
      if(chisqp>chisqm) { dir=-1.0; time_score=1-chisqm/chisqp; mytimeoffset=cm; }
      else { dir=+1.0; time_score=1-chisqp/chisqm; mytimeoffset=cp; }
      time_score=dir*time_score;
    }
  if(time_score>0.0) dir=1.0; if(time_score<0.0) dir=-1.0;

  MSG("AlgTrackCam", Msg::kDebug) << " timing parameters " << endl
				  << " chisqp=" << chisqp << " (" << chisqndfp << ") " 
				  << " chisqm=" << chisqm << " (" << chisqndfm << ") " << endl
				  << " score=" << time_score << endl;


  cth.SetTimeSlope((ctimeslope)/(3.0e8));
  cth.SetTimeOffset((ctimeoffset+StripListTime)/(3.0e8));
  cth.SetTimeForwardFitRMS(chisqp);
  cth.SetTimeForwardFitNDOF(chisqndfp);
  cth.SetTimeBackwardFitRMS(chisqm);
  cth.SetTimeBackwardFitNDOF(chisqndfm);

  return;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgTrackCam::Trace(const char * /* c */) const
{
}
////////////////////////////////////////////////////////////////////////