////////////////////////////////////////////////////////////////////////
// Package: CandTrackCam
//
// Handle for CandTrackCam
//
// marshall@hep.phy.cam.ac.uk
////////////////////////////////////////////////////////////////////////
#include "CandTrackCam/CandTrackCamHandle.h"
#include "MessageService/MsgService.h"

#include "RecoBase/CandStripHandle.h"
#include "RecoBase/LinearFit.h"
#include <cmath>

CVSID("$Id: CandTrackCamHandle.cxx,v 1.4 2007/01/15 20:02:43 rhatcher Exp $");


////////////////////////////////////////////////////////////////////////
CandTrackCamHandle::CandTrackCamHandle()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
CandTrackCamHandle::CandTrackCamHandle(const CandTrackCamHandle& handle)
    : CandTrackHandle(handle)
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
CandTrackCamHandle::CandTrackCamHandle(CandTrackCam* candidate)
    : CandTrackHandle(candidate)
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
CandTrackCamHandle::~CandTrackCamHandle()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
CandTrackCamHandle* CandTrackCamHandle::DupHandle() const
{
  return new CandTrackCamHandle(*this);
}
////////////////////////////////////////////////////////////////////////


// Implement interface methods here
////////////////////////////////////////////////////////////////////////
double CandTrackCamHandle::GetDirCos(int plane, int iuvz) const
{

  if (iuvz<0 || iuvz>=3) {
    MSG("TrackCam",Msg::kError) << "iuvz out of range\n";
    return 0.;
  }

  TIter stripItr(GetDaughterIterator());
  double uzpos[1000],upos[1000],uph[1000];
  int uplane[1000];
  double vzpos[1000],vpos[1000],vph[1000];
  int vplane[1000];
  for (int i=0; i<1000; i++) {
    uzpos[i] = 0.;
    upos[i] = 0.;
    uph[i] = 0.;
    uplane[i] = 0;
    vzpos[i] = 0.;
    vpos[i] = 0.;
    vph[i] = 0.;
    vplane[i] = 0;
  }
  while (CandStripHandle *strip = dynamic_cast<CandStripHandle*>(stripItr())) {
    int iplane = strip->GetPlane();
    if (iplane<0 || iplane>=1000) {
      MSG("TrackCam",Msg::kError) << "plane out of range\n";
    }
    else {
      switch (strip->GetPlaneView()) {
      case PlaneView::kU:
        uph[iplane] += strip->GetCharge();
        upos[iplane] += strip->GetCharge()*strip->GetTPos();
        uzpos[iplane] = strip->GetZPos();
        uplane[iplane] = iplane;
        break;
      case PlaneView::kV:
        vph[iplane] += strip->GetCharge();
        vpos[iplane] += strip->GetCharge()*strip->GetTPos();
        vzpos[iplane] = strip->GetZPos();
        vplane[iplane] = iplane;
        break;
      default:
        break;
      }
    }
  }
  for (int i=0; i<1000; i++) {
    if (uph[i]>0) upos[i] /= uph[i];
    if (vph[i]>0) vpos[i] /= vph[i];
  }
  double uzfit[5],ufit[5];
  double uwfit[5] = {0.,0.,0.,0.,0.};
  double vzfit[5],vfit[5];
  double vwfit[5] = {0.,0.,0.,0.,0.};
  for (int i=0; i<5; i++) {
    uzfit[i] = ufit[i] = vzfit[i] = vfit[i] = 0.0;
    int dplane[2]={-1,-1};
    int jbest[2]={-1,-1};
    for (int j=0; j<1000; j++) {
      if (uph[j]>0. && (dplane[0]<0 || TMath::Abs(uplane[j]-plane)<dplane[0])) {
        dplane[0] = TMath::Abs(uplane[j]-plane);
        uzfit[i] = uzpos[j];
        ufit[i] = upos[j];
        uwfit[i] = 1.;
        jbest[0] = j;
      }
      if (vph[j]>0. && (dplane[1]<0 || TMath::Abs(vplane[j]-plane)<dplane[1])) {
        dplane[1] = TMath::Abs(vplane[j]-plane);
        vzfit[i] = vzpos[j];
        vfit[i] = vpos[j];
        vwfit[i] = 1.;
        jbest[1] = j;
      }
    }
    if (jbest[0]>=0) uph[jbest[0]] = 0.;
    if (jbest[1]>=0) vph[jbest[1]] = 0.;
  }
  double uparm[2],ueparm[2];
  double vparm[2],veparm[2];
  LinearFit::Weighted(5,uzfit,ufit,uwfit,uparm,ueparm);
  LinearFit::Weighted(5,vzfit,vfit,vwfit,vparm,veparm);
  double dudz = uparm[1];
  double dvdz = vparm[1];
  double ddz[3] = {uparm[1],vparm[1],1.};
  
  return ddz[iuvz]/sqrt(1.+dudz*dudz+dvdz*dvdz);
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double CandTrackCamHandle::GetDirCosU(int plane) const
{
  return GetDirCos(plane,0);
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double CandTrackCamHandle::GetDirCosV(int plane) const
{
  return GetDirCos(plane,1);
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
double CandTrackCamHandle::GetDirCosZ(int plane) const
{
  return GetDirCos(plane,2);
}
////////////////////////////////////////////////////////////////////////


ClassImp(CandTrackCamHandle)