////////////////////////////////////////////////////////////////////////
// $Id: CandFitTrackSRHandle.cxx,v 1.43 2007/02/04 06:05:51 rhatcher Exp $
//
// CandFitTrackSRHandle
//
// CandFitTrackSRHandle is specialized access handle to CandFitTrackSR.
//
// Each concrete CandHandle must define a DupHandle function.
//
// Author:  R. Lee 2001.03.30
// Revised: B. Rebel 2003.07.15
////////////////////////////////////////////////////////////////////////

#include <cassert>
#include <iostream>

#include "CandFitTrackSR/CandFitTrackSRHandle.h"
#include "CandFitTrackSR/CandFitTrackSR.h"
#include "CandFitTrackSR/KalmanPlaneSR.h"
#include "CandFitTrackSR/SwimObjSR.h"
#include "CandTrackSR/TrackClusterSR.h"
#include "Conventions/Mphysical.h"
#include "MessageService/MsgFormat.h"
#include "MessageService/MsgService.h"
#include "Navigation/NavKey.h"
#include "Navigation/NavSet.h"
#include "Plex/PlexStripEndId.h"
#include "RecoBase/CandFitTrackHandle.h"
#include "RecoBase/CandStripHandle.h"
#include "UgliGeometry/UgliGeomHandle.h"
#include "UgliGeometry/UgliScintPlnHandle.h"
#include "UgliGeometry/UgliSteelPlnHandle.h"
#include "Algorithm/AlgConfig.h"
#include "TMath.h"

ClassImp(CandFitTrackSRHandle)

//______________________________________________________________________
CVSID("$Id: CandFitTrackSRHandle.cxx,v 1.43 2007/02/04 06:05:51 rhatcher Exp $");

// Swim state variable assignments
static const int kU      = 0;
static const int kV      = 1;
static const int kdUdZ   = 2;
static const int kdVdZ   = 3;
static const int kQoverP = 4;

//______________________________________________________________________
CandFitTrackSRHandle::CandFitTrackSRHandle()
{
}

//______________________________________________________________________
CandFitTrackSRHandle::CandFitTrackSRHandle(const CandFitTrackSRHandle &cdh) :
  CandFitTrackHandle(cdh)
{
}

//______________________________________________________________________
CandFitTrackSRHandle::CandFitTrackSRHandle(CandFitTrackSR *cd) :
  CandFitTrackHandle(cd)
{
}

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

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

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

//----------------------------------------------------------------------
Int_t CandFitTrackSRHandle::GetKalmanLast() const
{
  return dynamic_cast<const CandFitTrackSR *>
    (GetCandBase())->fPlaneList->GetLast();
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::ClearKalmanPlaneList(Bool_t deletekp)
{
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  if(deletekp){
    KalmanPlaneSR *kp = 0;
    for(int i=0; i<=KalmanPlaneList->GetLast(); ++i){
      kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
      delete kp;
    }
  }
  KalmanPlaneList->Clear();
  KalmanPlaneList->Compress();
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrent = 0;
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentU = 0;
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentV = 0;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::ClearTrackClusterList()
{
  TObjArray *TrackClusterList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fTrackClusterList;
  TrackClusterSR *tc = 0;
  for(int i=0; i<=TrackClusterList->GetLast(); ++i){
    tc = dynamic_cast<TrackClusterSR*>(TrackClusterList->At(i));
    delete tc;
  }
  TrackClusterList->Clear();
  TrackClusterList->Compress();
}

//----------------------------------------------------------------------
KalmanPlaneSR *CandFitTrackSRHandle::GetKalmanPlane(Int_t indx)
{
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  return dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(indx));
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::RemoveKalmanPlane(Int_t indx, Int_t idir)
{
// assumes first kalmanplane in kalmanplanelist is never removed
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  KalmanPlaneSR *kp = 0;
  if(indx>=0) kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(indx));
  else {
    kp = GetCurrentKalmanPlane();
    indx = KalmanPlaneList->IndexOf(kp);
  }
  KalmanPlaneSR *newkplast = 0;
  if(idir==0){
    assert(indx-1>=0);
    newkplast = GetKalmanPlane(indx-1);
  } 
  else{
    assert(indx+1<=GetKalmanLast());
    newkplast = GetKalmanPlane(indx+1);
  }
  SetCurrentKalmanPlane(newkplast);
  KalmanPlaneList->RemoveAt(indx);
  KalmanPlaneList->Compress();
  delete kp;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::CompressKalmanPlaneList()
{
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  KalmanPlaneList->Compress();
}

//----------------------------------------------------------------------
KalmanPlaneSR * CandFitTrackSRHandle::AddKalmanPlaneAt(KalmanPlaneSR *kpnew, Int_t indx)
{
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  Int_t ilast = KalmanPlaneList->GetLast();

  // KalmanPlaneList owns objects, so create new KalmanPlaneSR based on input
  KalmanPlaneSR *kpnewer = new KalmanPlaneSR(kpnew);
  KalmanPlaneSR *kp = 0;
  for(int i=ilast; i>=indx; --i){
    kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
    KalmanPlaneList->AddAtAndExpand(kp,i+1);
  }
  KalmanPlaneList->AddAtAndExpand(kpnewer,indx);
  return kpnewer;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::RemoveForward(KalmanPlaneSR *kp)
{
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>
    (GetOwnedCandBase())->fPlaneList;
  if(!kp) kp = GetCurrentKalmanPlane();
  Int_t indx = KalmanPlaneList->IndexOf(kp);
  KalmanPlaneSR *newkplast = GetKalmanPlane(indx-1);
  SetCurrentKalmanPlane(newkplast);
  Int_t nmax = KalmanPlaneList->GetLast();
  for(Int_t i=indx; i<=nmax; ++i){
    KalmanPlaneList->RemoveAt(i);
    kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
    delete kp;
  }
  KalmanPlaneList->Compress();
}


//----------------------------------------------------------------------
const KalmanPlaneSR *CandFitTrackSRHandle::GetKalmanPlane(Int_t indx) const
{
  TObjArray *KalmanPlaneList = dynamic_cast<const CandFitTrackSR*>
    (GetCandBase())->fPlaneList;
  return dynamic_cast<const KalmanPlaneSR*>(KalmanPlaneList->At(indx));
}


//----------------------------------------------------------------------
Double_t CandFitTrackSRHandle::GetInitialQP() const
{
  return dynamic_cast<const CandFitTrackSR*>(GetCandBase())->fInitialQP;
}


//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetInitialQP(Double_t qp0)
{
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fInitialQP = qp0;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetVtx(const KalmanPlaneSR *currentkp)
{
  if(currentkp->GetFilStateValue(kQoverP,1)!=0) 
    SetInitialQP(currentkp->GetFilStateValue(kQoverP,1));
  if(currentkp->GetFilStateValue(kU,1)!=0) 
    SetVtxU(currentkp->GetFilStateValue(kU,1));
  if(currentkp->GetFilStateValue(kV,1)!=0) 
    SetVtxV(currentkp->GetFilStateValue(kV,1));
  SetVtxZ(currentkp->GetTrackCluster()->GetZPos());
  Double_t dudz = currentkp->GetFilStateValue(kdUdZ,1);
  Double_t dvdz = currentkp->GetFilStateValue(kdVdZ,1);
  Double_t dsdz = TMath::Sqrt(1.+dudz*dudz+dvdz*dvdz)*(Double_t)(currentkp->GetZDir());
  if(dudz!=0) SetDirCosU(dudz/dsdz);
  if(dvdz!=0)SetDirCosV(dvdz/dsdz);
  if(dsdz!=1) SetDirCosZ(1./dsdz);
  SetVtxPlane(currentkp->GetTrackCluster()->GetPlane());

}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetEnd(const KalmanPlaneSR *currentkp)
{
  SetEndQP(currentkp->GetFilStateValue(kQoverP,0));
  if(currentkp->GetFilStateValue(kU,1)!=0) 
    SetEndU(currentkp->GetFilStateValue(kU,0));
  if(currentkp->GetFilStateValue(kV,1)!=0) 
     SetEndV(currentkp->GetFilStateValue(kV,0));
  SetEndZ(currentkp->GetTrackCluster()->GetZPos());
  Double_t dudz = currentkp->GetFilStateValue(kdUdZ,0);
  Double_t dvdz = currentkp->GetFilStateValue(kdVdZ,0);
  Double_t dsdz = TMath::Sqrt(1.+dudz*dudz+dvdz*dvdz)*(Double_t)(currentkp->GetZDir());
  if(dudz!=0) SetEndDirCosU(dudz/dsdz);
  if(dvdz!=0) SetEndDirCosV(dvdz/dsdz);
  if(dsdz!=1) SetEndDirCosZ(1./dsdz);
  SetEndPlane(currentkp->GetTrackCluster()->GetPlane());
}

//______________________________________________________________________

TObjArray * CandFitTrackSRHandle::GetTrackClusterList() {
  return dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fTrackClusterList;
}

//______________________________________________________________________

TObjArray * CandFitTrackSRHandle::GetPlaneList(){
  return dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fPlaneList;
}
//______________________________________________________________________

Int_t  CandFitTrackSRHandle::GetNChangedFitPoint(){
  return dynamic_cast<const CandFitTrackSR*>(GetCandBase())->fNChangedFitPoint;
}
//______________________________________________________________________

void CandFitTrackSRHandle::SetNChangedFitPoint(Int_t i){
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fNChangedFitPoint=i;
}
//----------------------------------------------------------------------
KalmanPlaneSR *CandFitTrackSRHandle::GetCurrentKalmanPlane(PlaneView::PlaneView_t planeview_t) 
{
  if (planeview_t==PlaneView::kU) 
    return dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentU;
  else if (planeview_t==PlaneView::kV) 
    return dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentV;
  else 
    return dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrent;
  
}
//----------------------------------------------------------------------
const KalmanPlaneSR *CandFitTrackSRHandle::GetCurrentKalmanPlane(PlaneView::PlaneView_t planeview_t) const
{
  if (planeview_t==PlaneView::kU) 
    return dynamic_cast<const CandFitTrackSR*>(GetCandBase())->fCurrentU;
  else if (planeview_t==PlaneView::kV) 
    return dynamic_cast<const CandFitTrackSR*>(GetCandBase())->fCurrentV;
  else 
    return dynamic_cast<const CandFitTrackSR*>(GetCandBase())->fCurrent;
  
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetCurrentKalmanPlane(KalmanPlaneSR *kp)
{
  dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrent = kp;  
  if(kp->GetTrackCluster()->GetPlaneView()==PlaneView::kU){
    dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentU = kp;
  }
  else if(kp->GetTrackCluster()->GetPlaneView()==PlaneView::kV){
    dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fCurrentV = kp;
  }
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::AddUpstreamPlaneList(TObjArray *uplist)
{
  Int_t nup = uplist->GetLast()+1;
  Int_t nmax = GetKalmanLast();
  TObjArray *KalmanPlaneList = dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase())->fPlaneList;
  KalmanPlaneSR *kp = 0;
  for(Int_t i=nmax; i>=0; --i){
    kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
    KalmanPlaneList->AddAtAndExpand(kp,i+nup);
  }
  for(Int_t i=0; i<=uplist->GetLast(); ++i){
    kp = dynamic_cast<KalmanPlaneSR*>(uplist->At(i));
    KalmanPlaneList->AddAt(kp,nup-i-1);
  }

}

//----------------------------------------------------------------------
Bool_t CandFitTrackSRHandle::GetBadFit(Int_t iplane) const
{
  map<TrackClusterSR *,Bool_t>::iterator iter;
  const TObjArray *KalmanPlaneList = dynamic_cast<const CandFitTrackSR*>
    (GetCandBase())->fPlaneList;
  Bool_t found(0);
 
  KalmanPlaneSR *kp = 0;
  for(int i=0; i<=KalmanPlaneList->GetLast() && !found; ++i){
    kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
    if(kp->GetTrackCluster()->GetPlane()==iplane){
      found = true;
      iter = (dynamic_cast<const CandFitTrackSR *>
	      (GetCandBase())->fBadFit).find(kp->GetTrackCluster());

      if(iter!=(dynamic_cast<const CandFitTrackSR *>
		(GetCandBase())->fBadFit).end()){
        return 1;
      }
      return 0;
    }//end if this is the plain
  }//end loop over kalman planes
  return 0;
}

//----------------------------------------------------------------------
Bool_t CandFitTrackSRHandle::GetBadFit(TrackClusterSR *tc) const
{
  map<TrackClusterSR *,Bool_t>::iterator iter;
  iter = (dynamic_cast<const CandFitTrackSR *>
	  (GetCandBase())->fBadFit).find(tc);
  if (iter!=(dynamic_cast<const CandFitTrackSR *>
	     (GetCandBase())->fBadFit).end()) {
    return 1;
  }
  return 0;
}

//----------------------------------------------------------------------
Int_t CandFitTrackSRHandle::GetNGoodFit() const
{
  Int_t ngood=0;
  const KalmanPlaneSR *kp = 0;
  for(int i=0; i<=GetKalmanLast(); ++i){
    kp = GetKalmanPlane(i);
    if (!GetBadFit(kp->GetTrackCluster())) ngood++;
  }
  return ngood;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetBadFit(TrackClusterSR *tc)
{
  dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fBadFit[tc] = 1;
}


//----------------------------------------------------------------------
Float_t CandFitTrackSRHandle::GetPlanePreChi2(Int_t iplane) const
{
  map<Int_t,Float_t>::iterator iter;
  iter = (dynamic_cast<const CandFitTrackSR *>(GetCandBase())->fPlanePreChi2).find(iplane);
  if (iter!=(dynamic_cast<const CandFitTrackSR *>(GetCandBase())->fPlanePreChi2).end()) {
    return dynamic_cast<const CandFitTrackSR *>(GetCandBase())->fPlanePreChi2[iplane];
  }
  return -1.;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetPlanePreChi2(Int_t iplane, Double_t chi2)
{
  dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fPlanePreChi2[iplane] = (Float_t)chi2;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::AddTrackCluster(TrackClusterSR *tc) {
  TrackClusterSR *newtc = new TrackClusterSR(*tc);
  CandFitTrackSR *track = dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase());
  track->fTrackClusterList->Add(newtc);
}


//----------------------------------------------------------------------
Int_t CandFitTrackSRHandle::GetNChangedFitPoint() const
{
  return dynamic_cast<const CandFitTrackSR *>(GetCandBase())->fNChangedFitPoint;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetVtxExtrapolate(Int_t nplane, 
                                             PlaneView::PlaneView_t planeview_t)
{
  if(planeview_t == PlaneView::kU){
    dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fVtxExtrapolate[0] = nplane;
  } 
  else if(planeview_t == PlaneView::kV){
    dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fVtxExtrapolate[1] = nplane;
  }
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetEndExtrapolate(Int_t nplane, 
                                             PlaneView::PlaneView_t planeview_t)
{
  if(planeview_t == PlaneView::kU){
    dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fEndExtrapolate[0] = nplane;
  } 
  else if (planeview_t == PlaneView::kV){
    dynamic_cast<CandFitTrackSR *>(GetOwnedCandBase())->fEndExtrapolate[1] = nplane;
  }
}

//----------------------------------------------------------------------
Int_t CandFitTrackSRHandle::GetVtxExtrapolate(PlaneView::PlaneView_t planeview_t) const
{
  const CandFitTrackSR *track = dynamic_cast<const CandFitTrackSR *>(GetCandBase());
  if(planeview_t == PlaneView::kU){
    return track->fVtxExtrapolate[0];
  } 
  else if(planeview_t == PlaneView::kV){
    return track->fVtxExtrapolate[1];
  }
  else{
    return min(track->fVtxExtrapolate[0],track->fVtxExtrapolate[1]);
  }
}

//----------------------------------------------------------------------
Int_t CandFitTrackSRHandle::GetEndExtrapolate(PlaneView::PlaneView_t planeview_t) const
{
  const CandFitTrackSR *track = dynamic_cast<const CandFitTrackSR *>(GetCandBase());
  if(planeview_t == PlaneView::kU){
    return track->fEndExtrapolate[0];
  } 
  else if(planeview_t == PlaneView::kV){
    return track->fEndExtrapolate[1];
  } 
  else{
    return min(track->fEndExtrapolate[0],track->fEndExtrapolate[1]);
  }
}


//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetdUdZ(Int_t plane, Float_t ddz)
{
  CandFitTrackSR *track = dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase());
  track->fdUdZ[plane] = ddz;
}

//----------------------------------------------------------------------
void CandFitTrackSRHandle::SetdVdZ(Int_t plane, Float_t ddz)
{
  CandFitTrackSR *track = dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase());
  track->fdVdZ[plane] = ddz;
}

//----------------------------------------------------------------------
Float_t CandFitTrackSRHandle::GetdUdZ(Int_t plane) const
{
  const CandFitTrackSR *track = dynamic_cast<const CandFitTrackSR*>(GetCandBase());
  if ((track->fdUdZ).count(plane)) {
    return track->fdUdZ[plane];
  }
  return -99999.;
}

//----------------------------------------------------------------------
Float_t CandFitTrackSRHandle::GetdVdZ(Int_t plane) const
{
  const CandFitTrackSR *track = dynamic_cast<const CandFitTrackSR*>(GetCandBase());
  if ((track->fdVdZ).count(plane)) {
    return track->fdVdZ[plane];
  }
  return -99999.;
}


//----------------------------------------------------------------------
void CandFitTrackSRHandle::ClearMaps()
{
  CandFitTrackSR *track = dynamic_cast<CandFitTrackSR*>(GetOwnedCandBase());
  track->fdUdZ.clear();
  track->fdVdZ.clear();
  track->fPlanePreChi2.clear();
  CandFitTrackHandle::ClearMaps();
}


XXXITRIMP(CandFitTrackSRHandle)