////////////////////////////////////////////////////////////////////////
// $Id: CandTrackSRHandle.cxx,v 1.25 2007/01/15 20:02:44 rhatcher Exp $
//
// CandTrackSRHandle
//
// CandTrackSRHandle is the specialized access handle to CandTrackSR.
//
// Each concrete CandHandle must define a DupHandle function.
//
// Author:  R. Lee 2001.02.26
//
// Also see <a href="../../root_crib/index.html">The ROOT Crib</a> and 
// <a href="../CandDigit.html"> CandDigit Classes</a> (part of
// <a href="../index.html">The MINOS Class User Guide</a>)End_Html
////////////////////////////////////////////////////////////////////////

#include <cassert>
#include <iostream>
#include <cmath>

#include "CandTrackSR/CandTrackSRHandle.h"
#include "CandTrackSR/CandTrackSR.h"
#include "CandTrackSR/Track2DSR.h"
#include "CandTrackSR/TrackClusterSR.h"
#include "MessageService/MsgService.h"
#include "MinosObjectMap/MomNavigator.h"
#include "Navigation/NavKey.h"
#include "Navigation/NavSet.h"
#include "RawData/RawChannelId.h"
#include "RawData/RawDigit.h"
#include "RawData/RawDigitDataBlock.h"
#include "RawData/RawRecord.h"
#include "RecoBase/ArrivalTime.h"
#include "RecoBase/CandClusterHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/LinearFit.h"

ClassImp(CandTrackSRHandle)

//______________________________________________________________________
CVSID("$Id: CandTrackSRHandle.cxx,v 1.25 2007/01/15 20:02:44 rhatcher Exp $");

//______________________________________________________________________
CandTrackSRHandle::CandTrackSRHandle()
{
}

//______________________________________________________________________
CandTrackSRHandle::CandTrackSRHandle(const CandTrackSRHandle &cdh) :
  CandTrackHandle(cdh)
{
}

//______________________________________________________________________
CandTrackSRHandle::CandTrackSRHandle(CandTrackSR *cd) :
  CandTrackHandle(cd)
{
}

//______________________________________________________________________
CandTrackSRHandle::~CandTrackSRHandle()
{
}

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


//______________________________________________________________________
void CandTrackSRHandle::Trace(const char *c) const
{
  MSG("Cand", Msg::kDebug)
    << "**********Begin CandTrackSRHandle::Trace(\"" << c << "\")" << endl
           << "Information from CandTrackSRHandle's CandHandle: " << endl;
  CandHandle::Trace(c);
  MSG("Cand", Msg::kDebug)
     << "**********End CandTrackSRHandle::Trace(\"" << c << "\")" << endl;
}

TObjArray *CandTrackSRHandle::GetClusterList()
{
  return dynamic_cast<CandTrackSR *>(GetCandBase())->fClusterList;
}

void CandTrackSRHandle::AddCluster(CandClusterHandle *cluster)
{
  if (!cluster) return;

  const TObjArray *clusterlist =
              (dynamic_cast<CandTrackSR*>(GetCandBase()))->fClusterList;

  if (clusterlist) {
    Bool_t found(0);
    TIter cliter(clusterlist);
    CandClusterHandle *target;
    while (!found &&
                (target = dynamic_cast<CandClusterHandle*>(cliter()))) {
      if (*cluster == *target) found = 1;
    }

    if (!found) {              // don't want to duplicate object in list
      (dynamic_cast<CandTrackSR*>(GetOwnedCandBase()))->fClusterList->
                                              Add(cluster->DupHandle());

      TIter striter(cluster->GetDaughterIterator());
      while (CandStripHandle *strip =
                            dynamic_cast<CandStripHandle*>(striter())) {
        AddDaughterLink(*strip);
      }
    }
  }
  else {                                           // Null fClusterList*
    MSG("TrackSR",Msg::kError) << "Null fClusterList*. No action taken."
                                                                << endl;
  }
}

Track2DSR *CandTrackSRHandle::GetUTrack() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fUTrack;
}

Track2DSR *CandTrackSRHandle::GetVTrack() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fVTrack;
}


void CandTrackSRHandle::SetUTrack(Track2DSR *track)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fUTrack = track;
}

void CandTrackSRHandle::SetVTrack(Track2DSR *track)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fVTrack = track;
}


Double_t CandTrackSRHandle::GetDirCos(Int_t plane, Int_t iuvz) const
{

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

  TIter stripItr(GetDaughterIterator());
  Double_t uzpos[1000],upos[1000],uph[1000];
  Int_t uplane[1000];
  Double_t vzpos[1000],vpos[1000],vph[1000];
  Int_t 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_t iplane = strip->GetPlane();
    if (iplane<0 || iplane>=1000) {
      MSG("TrackSR",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_t uzfit[5],ufit[5];
  Double_t uwfit[5] = {0.,0.,0.,0.,0.};
  Double_t vzfit[5],vfit[5];
  Double_t 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_t dplane[2]={-1,-1};
    Int_t 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_t uparm[2],ueparm[2];
  Double_t vparm[2],veparm[2];
  LinearFit::Weighted(5,uzfit,ufit,uwfit,uparm,ueparm);
  LinearFit::Weighted(5,vzfit,vfit,vwfit,vparm,veparm);
  Double_t dudz = uparm[1];
  Double_t dvdz = vparm[1];
  Double_t ddz[3] = {uparm[1],vparm[1],1.};


  Double_t dt = 1.;
  if (GetTimeSlope()<0.) dt = -1.;
  
  return dt*ddz[iuvz]/sqrt(1.+dudz*dudz+dvdz*dvdz);
}

Double_t CandTrackSRHandle::GetDirCosU(Int_t plane) const
{
  return GetDirCos(plane,0);
}

Double_t CandTrackSRHandle::GetDirCosV(Int_t plane) const
{
  return GetDirCos(plane,1);
}

Double_t CandTrackSRHandle::GetDirCosZ(Int_t plane) const
{
  return GetDirCos(plane,2);
}

Double_t CandTrackSRHandle::GetDirCosU() const
{
  return CandRecoHandle::GetDirCosU();
}
  
Double_t CandTrackSRHandle::GetDirCosV() const
{
  return CandRecoHandle::GetDirCosV();
}

Double_t CandTrackSRHandle::GetDirCosZ() const
{
  return CandRecoHandle::GetDirCosZ();
}

Double_t CandTrackSRHandle::GetScore() const
{
//  Double_t score = CandTrackHandle::GetScore();
// for now don't require beginning and ending planes to match
// perhaps implement when detector is more stable
  Double_t score = (Double_t)(GetNDaughters());
/*
  if (GetTimeSlope()>0.) {
    score += GetEndPlane();
  } else {
    score -= GetEndPlane();
  }
*/
//  score -= (GetUTrack()->GetChi2()+GetVTrack()->GetChi2());
  score += GetCharge()/10.; // for cosmics, should put parameter here
  return score;
}

Double_t CandTrackSRHandle::GetHoughDirCosU() const
{
  Track2DSR *utrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fUTrack;
  Track2DSR *vtrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fVTrack;
  assert(utrack);
  assert(vtrack);
  Double_t dudz=0.;
  Double_t dvdz=0.;
  if (!utrack->GetHoughExist() || !vtrack->GetHoughExist()) {
    return -999.;
  }
  dudz = utrack->GetHoughSlope();
  dvdz = vtrack->GetHoughSlope();
  if (GetTimeSlope()>=0.) {
    return dudz/sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
  else { 
    return -1.*dudz/sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
}

Double_t CandTrackSRHandle::GetHoughDirCosV() const
{
  Track2DSR *utrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fUTrack;
  Track2DSR *vtrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fVTrack;
  assert(utrack);
  assert(vtrack);
  Double_t dudz=0.;
  Double_t dvdz=0.;
  if (!utrack->GetHoughExist() || !vtrack->GetHoughExist()) {
    return -999.;
  }
  dudz = utrack->GetHoughSlope();
  dvdz = vtrack->GetHoughSlope();
  if (GetTimeSlope()>=0.) {
    return dvdz/sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
  else { 
    return -1.*dvdz/sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
}

Double_t CandTrackSRHandle::GetHoughDirCosZ() const
{
  Track2DSR *utrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fUTrack;
  Track2DSR *vtrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fVTrack;
  assert(utrack);
  assert(vtrack);
  Double_t dudz=0.;
  Double_t dvdz=0.;
  if (!utrack->GetHoughExist() || !vtrack->GetHoughExist()) {
    return -999.;
  }
  dudz = utrack->GetHoughSlope();
  dvdz = vtrack->GetHoughSlope();
  if (GetTimeSlope()>=0.) {
    return 1./sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
  else { 
    return -1./sqrt(1.+dudz*dudz+dvdz*dvdz);
  }
}

Double_t CandTrackSRHandle::GetHoughResid2() const
{
  Track2DSR *utrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fUTrack;
  Track2DSR *vtrack = dynamic_cast<const CandTrackSR*>(GetCandBase())->fVTrack;
  assert(utrack);
  assert(vtrack);
  if (!utrack->GetHoughExist() || !vtrack->GetHoughExist()) {
    return -999.;
  }
  Double_t dudz = utrack->GetHoughSlope();
  Double_t dvdz = vtrack->GetHoughSlope();
  Double_t u0 = utrack->GetHoughIntercept();
  Double_t v0 = vtrack->GetHoughIntercept();
  Int_t begplane = GetBegPlane();
  Int_t endplane = GetEndPlane();
  Int_t nplane = TMath::Abs(endplane-begplane)+1;
  Float_t *tpos = new Float_t[nplane];
  Float_t *zpos = new Float_t[nplane];
  Float_t *ph = new Float_t[nplane];
  Int_t *planeview = new Int_t[nplane];
  TIter stripItr(GetDaughterIterator());
  Int_t idir=1;
  if (GetTimeSlope()<0.) {
    idir = -1;
  }
  Double_t resid2 = 0.;
  for (int i=0; i<nplane; i++) {
    stripItr.Reset();
    planeview[i] = -1;
    tpos[i] = 0.;
    zpos[i] = 0.;
    ph[i] = 0.;
    while (CandStripHandle *strip = dynamic_cast<CandStripHandle*>(stripItr())) {
      if (strip->GetPlane()==begplane+i*idir) {
        planeview[i] = strip->GetPlaneView();
        tpos[i] += strip->GetTPos()*strip->GetCharge();
        zpos[i] = strip->GetZPos();
        ph[i] += strip->GetCharge();
      }
    }
    if (ph[i]>0.) {
      tpos[i] /= ph[i];
      if (planeview[i]==PlaneView::kU) {
        Double_t dtpos = tpos[i]-(u0+dudz*zpos[i]);
        resid2 += dtpos*dtpos;
      }
      if (planeview[i]==PlaneView::kV) {
        Double_t dtpos = tpos[i]-(v0+dvdz*zpos[i]);
        resid2 += dtpos*dtpos;
      }
    }
  }
  delete [] tpos;
  delete [] zpos;
  delete [] ph;
  delete [] planeview;
  return resid2;
}

//______________________________________________________________________
Double_t CandTrackSRHandle::GetTimeWeight(const CandDigitHandle *digit) const
{

// for now use total PMT pulse height
// in the future may want to calculate which pulse arrives first

  Double_t npe = digit->GetCharge()/60.; // use p.e. when available

  npe = max(npe,1.); // minimum 1 p.e.
  ArrivalTime arrtime;
  return arrtime.Weight(npe);

}

Int_t CandTrackSRHandle::GetNTrackPlane(PlaneView::PlaneView_t planeview_t) const
{
  CandStripHandleItr stripItr(GetDaughterIterator());
  CandStripHandleKeyFunc *stripKf = stripItr.CreateKeyFunc();
  stripKf->SetFun(CandStripHandle::KeyFromPlane);
  stripItr.GetSet()->AdoptSortKeyFunc(stripKf);
  stripKf = 0;

  Int_t plane=0;
  Int_t oldplane=0;
  CandStripHandle *strip;
  while ((strip = dynamic_cast<CandStripHandle*>(stripItr()))) {
    TIter digitItr(strip->GetDaughterIterator());
    if (IsInShower(strip)<=1) {
      PlaneView::PlaneView_t planeview = strip->GetPlaneView();
      if (planeview!=PlaneView::kA && planeview!=PlaneView::kB &&
          (planeview_t==PlaneView::kUnknown || planeview==planeview_t)) {
        if (!plane || strip->GetPlane()!=oldplane) {
          plane++;
        }
        oldplane = strip->GetPlane();
      }
    }
  }
  return plane;
}

void CandTrackSRHandle::SetNTrackStrip(Int_t n)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fNTrackStrip = n;
}

void CandTrackSRHandle::SetNTrackDigit(Int_t n)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fNTrackDigit = n;
}

void CandTrackSRHandle::SetNTimeFitDigit(Int_t n)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fNTimeFitDigit = n;
}

void CandTrackSRHandle::SetTimeFitChi2(Double_t x)
{
  dynamic_cast<CandTrackSR *>(GetOwnedCandBase())->fTimeFitChi2 = x;
}

Int_t CandTrackSRHandle::GetNTrackStrip() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fNTrackStrip;
}

Int_t CandTrackSRHandle::GetNTrackDigit() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fNTrackDigit;
}

Int_t CandTrackSRHandle::GetNTimeFitDigit() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fNTimeFitDigit;
}

Double_t CandTrackSRHandle::GetTimeFitChi2() const
{
  return dynamic_cast<const CandTrackSR *>(GetCandBase())->fTimeFitChi2;
}


XXXITRIMP(CandTrackSRHandle)