////////////////////////////////////////////////////////////////////////
// $Id: Track2DSR.cxx,v 1.45 2007/02/04 06:03:24 rhatcher Exp $
//
// Track2DSR
//
// Author:  R. Lee 2001.02.27
////////////////////////////////////////////////////////////////////////

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

#include "CandTrackSR/TrackClusterSR.h"
#include "CandTrackSR/Track2DSR.h"
#include "CandTrackSR/TrkClsSlpSR.h"
#include "Conventions/PlaneView.h"
#include "Conventions/Mphysical.h"
#include "Conventions/Munits.h"
#include "MessageService/MsgService.h"
#include "RecoBase/CandStripHandle.h"
#include "UgliGeometry/UgliGeomHandle.h"

ClassImp(Track2DSR)

//______________________________________________________________________
//CVSID("$Id: Track2DSR.cxx,v 1.45 2007/02/04 06:03:24 rhatcher Exp $");

//______________________________________________________________________
Track2DSR::Track2DSR()
{
  fIterate = 0;
  fZ0 = 0.;
  fTPos0 = 0.;
  fT0 = 0.;
  fCharge = 0.;
  fModified = 0;
  fParmTrack2DSlopeWeight = -0.25;
  fParmTrk2DAlpha = .4;               // cosmics, use .8 for non cosmics
  fDir = 1;
  fHoughExist = kFALSE;
  fHoughSlope = 0.;
  fHoughInter = 0.;
  fIsBad = false;
  fChi2 = -1;
}

//______________________________________________________________________
Track2DSR::Track2DSR(TrackClusterSR *tcluster)
{
  fTrkClsSlp.Clear();
  fIterate = 0;
  fZ0 = 0.;
  fTPos0 = 0.;
  fT0 = 0.;
  fCharge = 0.;
  Add(tcluster,0.);
  fParmTrack2DSlopeWeight = -0.25;
  fParmTrk2DAlpha = .4;               // cosmics, use .8 for non cosmics
  fDir = 1;
  fHoughExist = kFALSE;
  fHoughSlope = 0.;
  fHoughInter = 0.;
  fIsBad = false;
  fChi2 = -1;
}

//______________________________________________________________________
Track2DSR::Track2DSR(const Track2DSR &rhs) :    // Real copy-constructor
  TObject(rhs)
, fIterate(rhs.fIterate)
, fIsBad(rhs.fIsBad)
, fChi2(rhs.fChi2)
, fZ0(rhs.fZ0)
, fTPos0(rhs.fTPos0)
, fT0(rhs.fT0)
, fCharge(rhs.fCharge)
, fModified(rhs.fModified)
, fParmTrack2DSlopeWeight(rhs.fParmTrack2DSlopeWeight)
, fParmTrk2DAlpha(rhs.fParmTrk2DAlpha)
, fDir(rhs.fDir)
, fHoughSlope(rhs.fHoughSlope)
, fHoughInter(rhs.fHoughInter)
, fHoughExist(rhs.fHoughExist)
{

// Copy TObjArray's contents
  TIter rhsIter(&rhs.fTrkClsSlp);
  TObject *rhsObj;
  while ((rhsObj = rhsIter())) {
    TrkClsSlpSR *tcsObj;
    if ((tcsObj = dynamic_cast<TrkClsSlpSR*>(rhsObj))) {
      TrkClsSlpSR *newObj = new TrkClsSlpSR(*tcsObj);
      fTrkClsSlp.Add(newObj);
    }
  }
}

//______________________________________________________________________
Track2DSR::~Track2DSR()
{
  for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    if (tcs) {
      delete tcs;
    }
  }
}

//______________________________________________________________________
Bool_t Track2DSR::operator==(const Track2DSR& rhs) const
{

// compare simple members
  if ( !(this->fIterate    == rhs.fIterate    &&
         this->fIsBad      == rhs.fIsBad      &&
         this->fChi2       == rhs.fChi2       &&
         this->fZ0         == rhs.fZ0         &&
         this->fTPos0      == rhs.fTPos0      &&
         this->fCharge     == rhs.fCharge     &&
         this->fModified   == rhs.fModified   &&
         this->fParmTrack2DSlopeWeight == rhs.fParmTrack2DSlopeWeight &&
         this->fParmTrk2DAlpha         == rhs.fParmTrk2DAlpha         &&
         this->fDir        == rhs.fDir        &&
         this->fHoughSlope == rhs.fHoughSlope &&
         this->fHoughInter == rhs.fHoughInter &&
         this->fHoughExist == rhs.fHoughExist )) {
    return false;
  }

// compare TObjArray
  if (this->fTrkClsSlp.GetEntries() != rhs.fTrkClsSlp.GetEntries()) {
    return false;
  }
  else {
    TIter thisIter(&this->fTrkClsSlp);
    TIter rhsIter(&rhs.fTrkClsSlp);
    TObject *thisObj, *rhsObj;
    while ( ( (thisObj = thisIter.Next() ) != NULL) &&
            ( (rhsObj  = rhsIter.Next()  ) != NULL)    ) {
      TrkClsSlpSR *thisTCS = dynamic_cast<TrkClsSlpSR*>(thisObj); 
      TrkClsSlpSR *rhsTCS  = dynamic_cast<TrkClsSlpSR*>(rhsObj);
      if (!thisTCS->IsEquivalent(rhsTCS)) {
        return false;
      }
    }
  }
  return true;
}

//______________________________________________________________________
void Track2DSR::Add(TrackClusterSR *tcluster, Double_t slope)
{

  fModified = 1;
  CandStripHandle *strip = dynamic_cast<CandStripHandle*>
    (tcluster->GetStripList()->First());
  UgliGeomHandle ugh(*strip->GetVldContext());
  PlexStripEndId stripid = strip->GetStripEndId();
  UgliScintPlnHandle planehandle = ugh.GetScintPlnHandle(stripid);
  Bool_t found(0);
  if (fTrkClsSlp.GetLast()<0) {
    fZ0 = planehandle.GetZ0();
    fTPos0 = strip->GetTPos();
  }
  else {
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      if (fDir*tcs->fTrackCluster->GetZPos()<fDir*fZ0) {
        fZ0 = planehandle.GetZ0();
        fTPos0 = tcs->fTrackCluster->GetTPos();
        found = 1;
      }
    }
    if (fDir*planehandle.GetZ0()<fDir*fZ0) {
      fZ0 = planehandle.GetZ0();
      fTPos0 = strip->GetTPos();
      found = 1;
    }
  }
  Bool_t foundbef(1);
  Int_t ibef=0;
  for (Int_t i=0; i<=fTrkClsSlp.GetLast() && foundbef; i++) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    if (fDir*tcs->fTrackCluster->GetZPos()>fDir*planehandle.GetZ0()) {
      foundbef = 0;
    }
    ibef = i;
  }
  TrkClsSlpSR *tcsnew = new TrkClsSlpSR(tcluster);
  tcsnew->SetForwardSlope(slope);
  tcsnew->SetBackwardSlope(0.);
  tcsnew->SetIndex(1);
  if (!foundbef) {
    TrkClsSlpSR *lasttc =
        dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(fTrkClsSlp.GetLast()));
    fTrkClsSlp.Add(lasttc);
    for (int i=fTrkClsSlp.GetLast()-1; i>ibef; i--) {
      fTrkClsSlp[i] = fTrkClsSlp[i-1];
    }
    fTrkClsSlp[ibef] = tcsnew;
  } else {
    fTrkClsSlp.Add(tcsnew);
  }
  if (!found) {
    fT0 += (tcluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
                              Mphysical::c_light)*tcluster->GetCharge();
  }
  else {
// vertex changed, need to recalculate
    fT0 = 0.;
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      strip = dynamic_cast<CandStripHandle*>
        (tcs->fTrackCluster->GetStripList()->First());
      stripid = strip->GetStripEndId();
      planehandle = ugh.GetScintPlnHandle(stripid);
      fT0 +=
           (tcs->fTrackCluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
                    Mphysical::c_light)*tcs->fTrackCluster->GetCharge();
    }
  }
  fCharge += tcluster->GetCharge();
}

//______________________________________________________________________
void Track2DSR::CalculateBackwardSlope()
{
  fModified = 0;
  Int_t endplane = GetEndPlane();
  for (Int_t i=fTrkClsSlp.GetLast(); i>=0; i--) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    if (tcs->fTrackCluster->GetPlane()==endplane) {
      tcs->SetBackwardSlope(0.);
    }
    else {
      Double_t slope=0.;
      TrkClsSlpSR *tcs0 =
                         dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i+1));
      slope =
        (tcs->fTrackCluster->GetTPos()-tcs0->fTrackCluster->GetTPos()) /
        (tcs->fTrackCluster->GetZPos()-tcs0->fTrackCluster->GetZPos());
      if (i<fTrkClsSlp.GetLast()-1) {
        slope =
        (1.-fParmTrk2DAlpha)*slope+fParmTrk2DAlpha*tcs0->GetBackwardSlope();
      }
      tcs->SetBackwardSlope(slope);
    }
  }
}

//______________________________________________________________________
void Track2DSR::Clear(Option_t* /* option */)
{
  for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    delete tcs;
  }
  fTrkClsSlp.Clear();
  fZ0 = 0.;
  fTPos0 = 0.;
  fT0 = 0.;
  fCharge = 0.;
  fModified = 0;
}

//______________________________________________________________________
void Track2DSR::Compress()
{
  fTrkClsSlp.Compress();
}

//______________________________________________________________________
Track2DSR *Track2DSR::Dup()
{
  Track2DSR *newtrack = new Track2DSR(*this);
  return newtrack;
}

//______________________________________________________________________
Bool_t Track2DSR::IsBad() const
{ 
  return fIsBad;
}

//______________________________________________________________________
Bool_t Track2DSR::IsEquivalent(const TObject *rhs) const
{
  const Track2DSR *rTrack = dynamic_cast<const Track2DSR*>(rhs);
  return (*this) == (*rTrack);
}

//______________________________________________________________________
void Track2DSR::RemoveAt(Int_t indx)
{
  fModified = 1;
  TrkClsSlpSR *tcsold = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(indx));
  CandStripHandle *strip = dynamic_cast<CandStripHandle*>
    (tcsold->fTrackCluster->GetStripList()->First());
  UgliGeomHandle ugh(*strip->GetVldContext());
  PlexStripEndId stripid = strip->GetStripEndId();
  UgliScintPlnHandle planehandle = ugh.GetScintPlnHandle(stripid);
  Bool_t found(0);
  if (planehandle.GetZ0()==fZ0) {
    found = 1;
  }

  fTrkClsSlp.RemoveAt(indx);
  fTrkClsSlp.Compress();
  if (!found) {
    fT0 -=
        (tcsold->fTrackCluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
                 Mphysical::c_light)*tcsold->fTrackCluster->GetCharge();
  }
  else {

// vertex changed, need to recalculate
    fT0 = 0.;
    fZ0 = 0.;
    fTPos0 = 0.;
    Bool_t first(1);
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      if (tcsold !=
        tcs && (first || fDir*tcs->fTrackCluster->GetZPos()<fDir*fZ0)) {
        first = 0;
        fZ0 = tcs->fTrackCluster->GetZPos();
        fTPos0 = tcs->fTrackCluster->GetTPos();
      }
    }
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      if (tcsold != tcs) {
        strip = dynamic_cast<CandStripHandle*>
          (tcs->fTrackCluster->GetStripList()->First());
        stripid = strip->GetStripEndId();
        planehandle = ugh.GetScintPlnHandle(stripid);
        fT0 +=
           (tcs->fTrackCluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
                    Mphysical::c_light)*tcs->fTrackCluster->GetCharge();
      }
    }
  }
  fCharge -= tcsold->fTrackCluster->GetCharge();
  delete tcsold;

}

//______________________________________________________________________
Double_t Track2DSR::GetBackwardSlope(Int_t i)
{
  if (fModified) {
    CalculateBackwardSlope();
  }
  TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
  return tcs->GetBackwardSlope();
  
}

//______________________________________________________________________
Int_t Track2DSR::GetBegPlane() const
{
  Int_t plane=0;
  for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    if (!i || fDir*tcs->fTrackCluster->GetPlane()<fDir*plane) {
      plane = tcs->fTrackCluster->GetPlane();
    }
  }
  return plane;
}

//______________________________________________________________________
Double_t Track2DSR::GetChi2()
{
  fChi2 = 0.;
  if (fModified) {
    CalculateBackwardSlope();
  }
  for (Int_t i=0; i<fTrkClsSlp.GetLast(); i++) {
    TrkClsSlpSR *tcs0 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    TrkClsSlpSR *tcs1 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i+1));
    if(tcs1->fTrackCluster->GetTPosError()!=0){
     Double_t slope = GetSlope(i);
     Double_t dzpos = tcs1->fTrackCluster->GetZPos()-
                      tcs0->fTrackCluster->GetZPos();
     Double_t tpos = tcs0->fTrackCluster->GetTPos()+dzpos*slope;
     Double_t dtpos = tpos-tcs1->fTrackCluster->GetTPos();
     fChi2 += (dtpos*dtpos)/(tcs1->fTrackCluster->GetTPosError()*tcs1->fTrackCluster->GetTPosError());
    }
  }
  
  //  cout << " track chi2 " << fChi2/(fTrkClsSlp.GetLast()+1) << endl;
  return fChi2;
}

//______________________________________________________________________
TrackClusterSR *Track2DSR::GetCluster(Int_t i) const
{
  TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
  assert(tcs);
  return tcs->fTrackCluster;
}

//______________________________________________________________________
Int_t Track2DSR::GetDirection() const
{
  return fDir;
}

//______________________________________________________________________
Int_t Track2DSR::GetEndPlane() const
{
  Int_t plane=0;
  for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
    TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    if (!i || fDir*tcs->fTrackCluster->GetPlane()>fDir*plane) {
      plane = tcs->fTrackCluster->GetPlane();
    }
  }
  return plane;
}

//______________________________________________________________________
Double_t Track2DSR::GetForwardSlope(Int_t i) const
{
  TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
  return tcs->GetForwardSlope();
}

//______________________________________________________________________
Bool_t Track2DSR::GetHoughExist() const
{
  return fHoughExist;
}

//______________________________________________________________________
Double_t Track2DSR::GetHoughIntercept() const
{
  if (!fHoughExist) {
    return -9999999.;
  }
  return fHoughInter;
}

//______________________________________________________________________
Double_t Track2DSR::GetHoughSlope() const
{
  if (!fHoughExist) {
    return -9999999.;
  }
  return fHoughSlope;
}

//______________________________________________________________________
Int_t Track2DSR::GetLast() const
{
  return fTrkClsSlp.GetLast();
}

//______________________________________________________________________
PlaneView::PlaneView_t Track2DSR::GetPlaneView() const
{
  if (fTrkClsSlp.GetLast()<0) {
    return PlaneView::kUnknown;
  }
  TrkClsSlpSR *firstcluster =
                         dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.First());
  return firstcluster->fTrackCluster->GetPlaneView();
}

//______________________________________________________________________
Double_t Track2DSR::GetSlope(Int_t i)
{

//  TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
//  Double_t slopef = tcs->GetForwardSlope();
//  Double_t slopeb = tcs->fBackwardSlope;
  Double_t slopef = GetForwardSlope(i);
  Double_t slopeb = GetBackwardSlope(i);
  Double_t slopefw = 1.-exp(fParmTrack2DSlopeWeight*(Double_t)(i));
  Double_t slopebw =
                1.-exp(fParmTrack2DSlopeWeight*(Double_t)(GetLast()-i));
  if (i==0 && GetLast()==0) {
    return slopef;
  } else {
    return (slopef*slopefw+slopeb*slopebw)/(slopefw+slopebw);
  }
}

//______________________________________________________________________
Double_t Track2DSR::GetT0() const
{
  return fT0/fCharge;
}

//______________________________________________________________________
Double_t Track2DSR::GetTPos0() const
{
  return fTPos0;
}

//______________________________________________________________________
Double_t Track2DSR::GetZ0() const
{
  return fZ0;
}

//______________________________________________________________________
void Track2DSR::SetCluster(TrackClusterSR *tcluster, Double_t slope,
                                                             Int_t indx)
{
  fModified = 1;
  CandStripHandle *strip = dynamic_cast<CandStripHandle*>
    (tcluster->GetStripList()->First());
  UgliGeomHandle ugh(*strip->GetVldContext());
  PlexStripEndId stripid = strip->GetStripEndId();
  UgliScintPlnHandle planehandle = ugh.GetScintPlnHandle(stripid);
  Bool_t found(0);
  if (fTrkClsSlp.GetLast()<0) {
    fZ0 = planehandle.GetZ0();
    fTPos0 = strip->GetTPos();
  }
  else {
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      if (fDir*tcs->fTrackCluster->GetZPos()<fDir*fZ0) {
        fZ0 = planehandle.GetZ0();
        fTPos0 = tcs->fTrackCluster->GetTPos();
        found = 1;
      }
    }
  }
  TrkClsSlpSR *tcsnew = new TrkClsSlpSR(tcluster);
  tcsnew->SetForwardSlope(slope);
  tcsnew->SetBackwardSlope(0.);
  tcsnew->SetIndex(1);
  fTrkClsSlp.AddAt(tcsnew,indx);
  if (!found) {
    fT0 += (tcluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
           Mphysical::c_light)*tcluster->GetCharge();
  }
  else {

// vertex changed, need to recalculate
    fT0 = 0.;
    for (Int_t i=0; i<=fTrkClsSlp.GetLast(); i++) {
      TrkClsSlpSR *tcs = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
      strip = dynamic_cast<CandStripHandle*>
        (tcs->fTrackCluster->GetStripList()->First());
      stripid = strip->GetStripEndId();
      planehandle = ugh.GetScintPlnHandle(stripid);
      fT0 +=
           (tcs->fTrackCluster->GetBegTime()-(planehandle.GetZ0()-fZ0) /
                    Mphysical::c_light)*tcs->fTrackCluster->GetCharge();
    }
  }
  fCharge += tcluster->GetCharge();
}

//______________________________________________________________________
void Track2DSR::SetDirection(Int_t idir)
{
  fDir = idir;
}

//______________________________________________________________________
void Track2DSR::SetHoughIntercept(Double_t inter)
{
  fHoughInter = inter;
  fHoughExist = kTRUE;
}

//______________________________________________________________________
void Track2DSR::SetHoughSlope(Double_t slope)
{
  fHoughSlope = slope;
  fHoughExist = kTRUE;
}

//______________________________________________________________________
void Track2DSR::SetIsBad(Bool_t isBad)
{
  fIsBad = isBad;
  return;
}

Bool_t Track2DSR::IsFocussed() const
{
  TrkClsSlpSR *tcs0 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(0));
  TrkClsSlpSR *tcs1 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(fTrkClsSlp.GetLast()));
  Int_t idir=1;
  if(tcs0->fTrackCluster->GetZPos()>tcs1->fTrackCluster->GetZPos())idir=-1;
  Int_t nfocus=0;
  for (Int_t i=0; i<fTrkClsSlp.GetLast()-1; i++) {
    tcs0 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    tcs1 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i+1));
    TrkClsSlpSR *tcs2 =dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i+2));
    if(idir==-1){
      tcs0 = dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i+2));
      tcs2 =dynamic_cast<TrkClsSlpSR*>(fTrkClsSlp.At(i));
    }
    Double_t tslope =  (tcs1->fTrackCluster->GetTPos()-tcs0->fTrackCluster->GetTPos())/(tcs1->fTrackCluster->GetZPos()-tcs0->fTrackCluster->GetZPos());
    Double_t textrap = tcs1->fTrackCluster->GetTPos() + tslope*(tcs2->fTrackCluster->GetZPos()-tcs1->fTrackCluster->GetZPos());
    if(TMath::Abs(textrap) - TMath::Abs(tcs2->fTrackCluster->GetTPos())>=0){
      nfocus++;
    }
    else{
      nfocus--;
    } 
  }
  if(nfocus>=0) return true;
  return false;
}
XXXITRIMP(Track2DSR)