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

#include "TrackCam.h"
#include "HitCam.h"
#include "RecoBase/CandSliceHandle.h"

ClassImp(TrackCam)

  //CVSID("$Id: TrackCam.cxx,v 1.3 2006/05/26 16:19:47 marshall Exp $");

////////////////////////////////////////////////////////////////////////
TrackCam::TrackCam(CandSliceHandle* slice) :
  fSlice(0), fBegPlane(999), fEndPlane(-999), fBegZ(999.), fEndZ(-999.),
  fUID(0), fPartner(0), fPlaneView(-1)
{
  fSlice = slice;
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
TrackCam::~TrackCam()
{
  HitsInTrack.clear();
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
void TrackCam::AddHit(HitCam* hit)
{
  if(this->GetEntries()==0) { fPlaneView=hit->GetPlaneView(); }

  if(this->ContainsHit(hit)==true) {return;}
  HitsInTrack.push_back(hit);   

  if( fBegPlane > hit->GetPlane()){ fBegPlane = hit->GetPlane(); fBegZ = hit->GetZPos();}
  if( fEndPlane < hit->GetPlane()){ fEndPlane = hit->GetPlane(); fEndZ = hit->GetZPos();}

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


////////////////////////////////////////////////////////////////////////
bool TrackCam::ContainsHit(HitCam* hit) const
{
  for(unsigned int i=0; i<HitsInTrack.size(); ++i) {
    if(hit==HitsInTrack[i]) {return true;}
  }

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


////////////////////////////////////////////////////////////////////////
HitCam* TrackCam::GetHit(unsigned int i) const
{
  if(i<HitsInTrack.size()) {return HitsInTrack[i];}
  else {return 0;}
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double TrackCam::GetBegTPos()
{
  double tot=0.0,begt=0.0;
  unsigned int nhits = HitsInTrack.size();

  for(unsigned int i=0; i<nhits; ++i) {
    //find the hits on the first plane in the track
    HitCam* hit = HitsInTrack[i];

    if(hit->GetPlane()==fBegPlane) {
      begt+=hit->GetTPos();
      tot+=1.0;
    }
  }
  if(tot>0) return (begt/tot); 
  else return 0;
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double TrackCam::GetEndTPos()
{
  double tot=0.0,endt=0.0;
  unsigned int nhits = HitsInTrack.size();

  for(unsigned int i=0; i<nhits; ++i) {
    //find the hits on the first plane in the track
    HitCam* hit = HitsInTrack[i];

    if(hit->GetPlane()==fEndPlane) {
      endt+=hit->GetTPos();
      tot+=1.0;
    }
  }
  if(tot>0) return (endt/tot); 
  else return 0;
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double TrackCam::GetDir(int Plane1, int Plane2)
{
  double z=0.0,t=0.0;
  double sw=0.0,swx=0.0,swx2=0.0;
  double swy=0.0,swyx=0.0;
  unsigned int nhits = HitsInTrack.size();

  int Counter=0; int ThisPlane; int LastPlaneAdded=-999;

  for(unsigned int i=0; i<nhits; ++i) {
    HitCam* hit = HitsInTrack[i];

    ThisPlane=hit->GetPlane();

    //find the hits in the correct region of the track
    if(ThisPlane>=Plane1 && ThisPlane<=Plane2) {
      z=hit->GetZPos(); 
      t=hit->GetTPos();
      sw+=1.0; 
      swx+=z; 
      swx2+=z*z; 
      swy+=t; 
      swyx+=t*z;

      Counter++; LastPlaneAdded=ThisPlane;
    }
  }
  

  // If we haven't added enough hits to the straight-line fit.
  // Only likely to affect ND track extrapolation.
  ///////////////////
  if(Counter==1) {
    HitCam* NextHit = 0;
    int MaxPlane=-20; int MinPlane=500;

    if(LastPlaneAdded==Plane1) {
      for(unsigned int i=0; i<nhits; ++i) {
	HitCam* hit = HitsInTrack[i];
	ThisPlane=hit->GetPlane();

	if(ThisPlane>Plane1 && ThisPlane<MinPlane) {MinPlane=ThisPlane; NextHit=hit;}
      }
    }

    else if(LastPlaneAdded==Plane2) {
      for(unsigned int i=0; i<nhits; ++i) {
	HitCam* hit = HitsInTrack[i];
	ThisPlane=hit->GetPlane();

	if(ThisPlane<Plane2 && ThisPlane>MaxPlane) {MaxPlane=ThisPlane; NextHit=hit;}
      }
    }

    // Add extra hit to the fit.
    if(NextHit) {
      z=NextHit->GetZPos(); 
      t=NextHit->GetTPos();
      sw+=1.0; 
      swx+=z; 
      swx2+=z*z; 
      swy+=t; 
      swyx+=t*z;
    }
  }
  ///////////////////


  if((swx*swx-sw*swx2)!=0) {return (swx*swy-sw*swyx)/(swx*swx-sw*swx2);}

  else return 0;
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double TrackCam::GetBegDir()
{
  return this->GetDir(fBegPlane,fBegPlane+10);
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double TrackCam::GetEndDir()
{
  return this->GetDir(fEndPlane-10,fEndPlane);
}
////////////////////////////////////////////////////////////////////////