////////////////////////////////////////////////////////////////////////
// $Id: AlgFitTrackSR.cxx,v 1.90 2007/11/11 08:44:45 rhatcher Exp $
//
// AlgFitTrackSR.cxx
//
// AlgFitTrackSR is a concrete FitTrackSR Algorithm class.
//
// Author:  R. Lee 2001.03.30
// Revised: B. Rebel 2003.06.27
////////////////////////////////////////////////////////////////////////

#include <assert.h>
extern "C" {
#include <unistd.h>    // sysconf
#include <sys/times.h> // times()
}

#include "Algorithm/AlgConfig.h"
#include "Candidate/CandContext.h"
#include "CandData/CandHeader.h"
#include "CandFitTrackSR/AlgFitTrackSR.h"
#include "CandFitTrackSR/CandFitTrackSRHandle.h"
#include "CandFitTrackSR/KalmanPlaneSR.h"
#include "CandTrackSR/CandTrackSRHandle.h"
#include "CandTrackSR/TrackClusterSR.h"
#include "Conventions/Mphysical.h"
#include "Conventions/Munits.h"
#include "Conventions/PlaneView.h"
#include "DataUtil/GetMomFromRange.h"
#include "MessageService/MsgService.h"
#include "MessageService/MsgFormat.h"
#include "MinosObjectMap/MomNavigator.h"
#include "Navigation/NavKey.h"
#include "Navigation/NavSet.h"
#include "RecoBase/CandSliceHandle.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "RecoBase/ArrivalTime.h"
#include "RecoBase/PropagationVelocity.h"
#include "RecoBase/LinearFit.h"
#include "UgliGeometry/UgliGeomHandle.h"
#include "BField/BField.h"
#include "Swimmer/SwimGeo.h"
#include "Swimmer/SwimSwimmer.h"
#include "Swimmer/SwimStepper.h"
#include "Swimmer/SwimPrintStepAction.h"
#include "Swimmer/SwimParticle.h"
#include "Swimmer/SwimZCondition.h"
#include "Swimmer/SwimPlaneInterface.h"
#include "TMatrixD.h"
#include "TMath.h"

//make a NavKey to sort Clusters by plane number
NavKey KeyOnClusterPlane( const TrackClusterSR *tc){
  return tc->GetPlane();
}

ClassImp(AlgFitTrackSR)

//______________________________________________________________________
CVSID("$Id: AlgFitTrackSR.cxx,v 1.90 2007/11/11 08:44:45 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;
static const double kInvSqrt2 = TMath::Sqrt(0.5);

//______________________________________________________________________
AlgFitTrackSR::AlgFitTrackSR()
{
}

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

//______________________________________________________________________
void AlgFitTrackSR::RunAlg(AlgConfig &ac, CandHandle &ch,
                                                        CandContext &cx)
{
  // check inputs


  pRev=0;
  pRev2=0;
  pFor=0;
  assert(ch.InheritsFrom("CandFitTrackSRHandle"));
  CandFitTrackSRHandle &cfh = dynamic_cast<CandFitTrackSRHandle &>(ch);
  cfh.SetPass(0);

  assert(cx.GetDataIn());
  assert(cx.GetDataIn()->InheritsFrom("TObjArray"));

  CandTrackHandle *track0 = 0;
  const TObjArray *tclist = 0;
  const TObjArray *cxin =                        
	dynamic_cast<const TObjArray *>(cx.GetDataIn());
  for (Int_t i=0; i<=cxin->GetLast(); i++) {
    TObject *tobj = cxin->At(i);
    if (tobj->InheritsFrom("CandTrackHandle")) {
      track0 = dynamic_cast<CandTrackHandle*>(tobj);
      MSG("EventNum", Msg::kDebug) << "event number " 
			<< track0->GetCandRecord()->GetCandHeader()->GetSnarl() 
			<< endl;
    }
    if (tobj->InheritsFrom("TObjArray")) {
      tclist = dynamic_cast<TObjArray*>(tobj);
    }
  }
  assert(track0);

  // set finder track
  cfh.SetFinderTrack(track0);

  // get detector type
  const VldContext* vldcptr = track0->GetVldContext();
  assert(vldcptr);
  fVldContext = *vldcptr;
  fDetector = fVldContext.GetDetector();
  UgliGeomHandle ugh(fVldContext);

  // input algorithm parameters

  fParmMaxIterate = ac.GetInt("MaxIterate");
  fParmMaxIterate2 = ac.GetInt("MaxIterate2");
  fParmQPDiff = ac.GetDouble("QPDiff");
  fParmMinPlanePE = ac.GetDouble("MinPlanePE");
  fParmLastPlane = ac.GetInt("LastPlane");
  fParmIsCosmic = ac.GetInt("IsCosmic");
  fParmMaxLocalChi2 = ac.GetDouble("MaxLocalChi2");
  fParmMaxLocalPreChi2 = ac.GetDouble("MaxLocalPreChi2");
  fParmMaxAngleCovariance = ac.GetDouble("MaxAngleCovariance");
  fParmNSkipView = ac.GetInt("NSkipView");
  fParmNSkipActive = ac.GetInt("NSkipActive");
  fParmPassReducedChi2 = ac.GetDouble("PassReducedChi2");
  fParmPassPlaneAsymmetry = ac.GetDouble("PassPlaneAsymmetry");
  fParmPassMinPlaneAsymmetry = ac.GetInt("PassMinPlaneAsymmetry");
  fParmMaxImpactParameter = ac.GetDouble("MaxImpactParameter");
  fParmMinClusterCharge = ac.GetDouble("MinClusterCharge");
  fParmMaxClusterNStrip = ac.GetInt("MaxClusterNStrip");
  fParmDeltaChi2 = ac.GetDouble("DeltaChi2");
  fParmDeltaCovariance = ac.GetDouble("DeltaCovariance");
  fParmMisalignmentError = ac.GetDouble("MisalignmentError")*Munits::mm;
  fParmTPosError2 = ac.GetDouble("TPosError2");
  fParmInitialPositionError2 = ac.GetDouble("InitialPositionError2");
  fParmInitialSlopeError2 = ac.GetDouble("InitialSlopeError2");
  fParmInitialQPError2 = ac.GetDouble("InitialQPError2");
  fParmMaxLocalChi2 = ac.GetDouble("MaxLocalChi2");
  fParmMaxLocalPreChi2 = ac.GetDouble("MaxLocalPreChi2");
  fParmCovarianceScale = ac.GetDouble("CovarianceScale");

  //initialize the parameters from registry for the Kalman Plane stuff
  InitKalmanFitParameters(ac);

  // verify that looping won't ask for planes that don't exist
  const float way_downstream = 999999.;
  PlexPlaneId lastpln = ugh.GetPlaneIdFromZ(way_downstream);
  if (lastpln.GetPlane() < fParmLastPlane ) {
    fParmLastPlane = lastpln.GetPlane();
  }

  Int_t idir = -1;
  if(track0->GetTimeSlope()>0. || !fParmIsCosmic) idir = 1;

  //get an iterator over the strips in this slice
  CandStripHandle *strip = 0;
  CandStripHandleItr stripItr(track0->GetCandSlice()->GetDaughterIterator());
  
  //sort by plane and strip then by time using the navigation 2 sort functionality
  stripItr.SetSort2Fun(CandStripHandle::StripSRKeyFromPSEId, CandStripHandle::StripSRKeyFromBegTime);
    
  MSG("AlgFitTrackSR", Msg::kDebug) << "sorted the strips by time" << endl;
  
  stripItr.SetDirection(idir);
  stripItr.Reset();

  //create a TObjArray to hold the cluster objects you are going to make
  TObjArray *trackClusterList = new TObjArray(1,0);

  //fill the trackClusterList with the objects in tclist, if it is there
  if(tclist) MakeSliceClusterList(trackClusterList, tclist, stripItr, cfh);
  else MakeTrackClusterList(trackClusterList, stripItr, cfh, idir);

  MSG("AlgFitTrackSR", Msg::kDebug) << "made cluster list" << endl;
  
// clone the trackClusterList so that you can have a master list of 
// track clusters and one that keeps track of which clusters are used
// in the fit track

  TObjArray *clusterList(trackClusterList);
  
  //make an iterator over clusterList and sort it by plane number
  TrackClusterSRItr clusterItr(clusterList);
  TrackClusterSRKeyFunc *clusterKf = clusterItr.CreateKeyFunc();
  clusterKf->SetFun(KeyOnClusterPlane);
  clusterItr.GetSet()->AdoptSortKeyFunc(clusterKf);
  clusterKf = 0;
  clusterItr.SetDirection(idir);

  clusterItr.ResetFirst();
  TrackClusterSR *cluster = 0;

  TObjArray planeClusterList(1000);
  Int_t iplane=0;
  Bool_t first=true;
  TObjArray * planeClusters;
  while( (cluster = clusterItr())) {
    if(cluster->GetPlane()!=iplane || first){
      iplane=cluster->GetPlane();
      MSG("FitTrackSR", Msg::kDebug) << "new cluster on plane " << iplane << endl;
      planeClusters = new TObjArray(1,0);
      planeClusterList.AddAt(planeClusters,iplane);
      planeClusters->Add(cluster);
      first=false;
    }
    else{
      MSG("FitTrackSR", Msg::kDebug) << "adding cluster on plane " << iplane << endl;
      planeClusters=(TObjArray *)planeClusterList.At(iplane);
      if(!planeClusters) MSG("AlgFitTrackSR", Msg::kFatal) << "planeCluster array not defined for plane " << iplane << endl;
      planeClusters->Add(cluster);
    }
  }
  
  Bool_t dir = kIterForward;
  if(idir!=1) dir=kIterBackward;
  TIter planeItr(&planeClusterList,idir);
  TrackClusterSR * tc=0;
  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (tc = (TrackClusterSR *)clusterItr.Next()) ){
      
      MSG("FitTrackSR", Msg::kDebug) << "current cluster on plane " 
				     << tc->GetPlane() << " is valid "
				     << (Int_t)tc->IsValid() << endl;
    }
  }
  MSG("AlgFitTrackSR", Msg::kDebug) << "made cluster iterator" << endl;

  //get another iterator over the strips in the track this time
  CandStripHandleItr trackStripItr(track0->GetDaughterIterator());
  
  //sort by plane and strip then by time using the navigation 2 sort functionality
  trackStripItr.SetSort2Fun(CandStripHandle::StripSRKeyFromPSEId, CandStripHandle::StripSRKeyFromBegTime);
  
  /* for now leave in the old code to sort in 2 dimensions just in case
  CandStripHandleKeyFunc *trackStripKf = trackStripItr.CreateKeyFunc();
  //using a 2d sort - first sort on the PlexStripEndId
  trackStripKf->SetFun(CandStripHandle::StripSRKeyFromPSEId);
  trackStripItr.GetSet()->AdoptSortKeyFunc(trackStripKf,kTRUE,kFALSE);
  //now sort by time
  trackStripKf = trackStripItr.CreateKeyFunc();
  trackStripKf->SetFun(CandStripHandle::StripSRKeyFromBegTime);
  trackStripItr.GetSet()->AdoptSortKeyFunc(trackStripKf,kFALSE);
  trackStripKf = 0;
  */
    
  MSG("AlgFitTrackSR", Msg::kDebug) << "got iterator sorted over track strips" << endl;

  trackStripItr.SetDirection(idir);
  trackStripItr.Reset();

  Int_t begPlane = -1;
  Int_t endPlane = -1;

  //now mark the clusters to use in the track by setting the IsValid flag to true
  bool goodTrack = MarkTrackClusters(track0, planeClusterList, trackStripItr, cfh, 
                                     begPlane, endPlane);

  //set the track parameters based on the track passed into the algorithm
  SetTrackParameters(track0, cfh, idir);

  //set the endpoints and their direction cosines if track0 is a CandTrackSR
  SetTrackEndParameters(begPlane, endPlane, cfh, track0);

  MSG("AlgFitTrackSR", Msg::kDebug) << "start the fitting" << endl;
  
  //this track has fewer than 3 hits at least one view, so just add the strips
  //in the daughter list of track0 to cfh and quit
  if(!goodTrack){
	  
	  MSG("AlgFitTrackSR", Msg::kDebug) << "not a good track, bail" << endl;  
	// add daughter strips from the current track and bail
    CandStripHandleItr trk0stripItr(track0->GetDaughterIterator());
    while( (strip = trk0stripItr()) ){
      cfh.AddDaughterLink(*strip);
    }
	
	//need to set the end points
	
    return;
  }
  else{ 
    Int_t istatus = 0;
    //now to actually do the track fitting.  
 
    Int_t dosearch=1;
    Int_t niterate = DoKalmanFit(planeClusterList, cfh, istatus, idir, dosearch);
    if(istatus>=0){
      //get the first and last planes for the track
      KalmanPlaneSR *kp0 = cfh.GetKalmanPlane(0);
      KalmanPlaneSR *kp1 = cfh.GetKalmanPlane(cfh.GetKalmanLast());
      cfh.SetVtx(kp0);
      cfh.SetEnd(kp1);

      //set up variables for first and last planes in each view for track
      const KalmanPlaneSR *kpu0 = 0;
      const KalmanPlaneSR *kpv0 = 0;
      const KalmanPlaneSR *kpu1 = 0;
      const KalmanPlaneSR *kpv1 = 0;
      
      // clear STL maps--we only want fit planes as keys
      cfh.ClearMaps();
      SetPlaneParameters(cfh, kpu0, kpu1, kpv0, kpv1);
    }
    // check to see whether full fit converged.  If not, fall back to using
    // only track points.
    if(istatus<0 
           || cfh.GetNDOF()==0 
           || (cfh.GetNDOF()!=0 && cfh.GetChi2()/cfh.GetNDOF()>fParmPassReducedChi2) 
           ) {
      dosearch = 0;
      idir = -1;
      if(track0->GetTimeSlope()>0. || ! fParmIsCosmic) idir = 1;

      //fill the trackClusterList with the objects in tclist, if it is there
      cfh.ClearTrackClusterList();
      trackClusterList->Clear();
      trackClusterList->Compress();
      if(tclist) MakeSliceClusterList(trackClusterList, tclist, stripItr,
				      cfh);
      else MakeTrackClusterList(trackClusterList, stripItr, cfh, idir);

      //now mark the clusters to use in the track by setting the IsValid flag to true
      begPlane = -1;
      endPlane = -1;
      goodTrack = MarkTrackClusters(track0, planeClusterList, trackStripItr, cfh, 
					 begPlane, endPlane);
   
      //set the track parameters based on the track passed into the algorithm
      SetTrackParameters(track0, cfh, idir);
      
      //set the endpoints and their direction cosines if track0 is a CandTrackSR
      SetTrackEndParameters(begPlane, endPlane, cfh, track0);
      niterate = DoKalmanFit(planeClusterList, cfh, istatus, idir, dosearch);      
    }

    //set the number of iterations for the fit
    cfh.SetNIterate(niterate);

    if(istatus>=0){
      //get the first and last planes for the track
      KalmanPlaneSR *kp0 = cfh.GetKalmanPlane(0);
      KalmanPlaneSR *kp1 = cfh.GetKalmanPlane(cfh.GetKalmanLast());
      cfh.SetVtx(kp0);
      cfh.SetEnd(kp1);

      //set up variables for first and last planes in each view for track
      const KalmanPlaneSR *kpu0 = 0;
      const KalmanPlaneSR *kpv0 = 0;
      const KalmanPlaneSR *kpu1 = 0;
      const KalmanPlaneSR *kpv1 = 0;
      
      // clear STL maps--we only want fit planes as keys
      cfh.ClearMaps();
      SetPlaneParameters(cfh, kpu0, kpu1, kpv0, kpv1);

      //gotta add those strips to the daughter list for this track
      MakeDaughterStripList(cfh);
            
      // create array of strips not in fit track
      Double_t planepe[1000];
      for(int i=0; i<1000; ++i){
        planepe[i] = 0.;
      }

      stripItr.ResetFirst();
      CandStripHandle *strip = 0;
      while( (strip = stripItr()) ){
        if(strip->GetPlane()>=0) 
          planepe[strip->GetPlane()] += strip->GetCharge(CalDigitType::kPE);
      }//end loop over strips to get charge for each plane

      // Find planes closest to vertex and end with adjacent planes meeting
      // PE cut
      Int_t plane0 = -999;
      Int_t plane1 = -999;
      for(int i=0; i<1000; ++i){
        if(planepe[i]>=fParmMinPlanePE && planepe[i+1]>=fParmMinPlanePE){
          if(idir>0){
            if(plane0<0) plane0 = i;
            if(plane1<0 || i+1>plane1) plane1 = i+1;
          } 
          else{
            if(plane1<0) plane1 = i;
            if(plane0<0 || i+1>plane0) plane0 = i+1;
          }
        }//end if enough signal on these two planes
      }//end loop over planes to find the first and last ones

    
      
      Double_t vtxqp = kp0->GetFilStateValue(4,1);

      // tweak qp to force agreement with truth !!!!
      vtxqp *= 1.01+0.1*TMath::Abs(vtxqp);
      
      //if we are looking at cosmic rays, need to swim the track out of the 
      //detector
      if(fParmIsCosmic) SwimVertexAndEndPoints(cfh, kp0, kp1, 
					       kpu0, kpu1, 
					       kpv0, kpv1, 
                                               planepe, plane0, plane1, 
					       vtxqp, idir);

      if(vtxqp==0.){
        // infinite momentum, choose finite number to prevent floating point exceptions
        vtxqp = 0.000001; // 1 PeV
        cfh.SetEMCharge(0.);
      } 
      
      //  set momentum and charge sign
      cfh.SetMomentumCurve(1./TMath::Abs(vtxqp));
      if(vtxqp>0.) cfh.SetEMCharge(1.);
      else if(vtxqp<0.) cfh.SetEMCharge(-1.);
      
      //  fill time and range maps
      SetT(&cfh);
      SetdS(&cfh);
    
      // set the momentum from range; taken from PDG R/M vs p/M plot for Fe  
      Double_t range = cfh.GetRange();
      // if(range>0.) cfh.SetMomentumRange(0.105658*exp(1.0363*log(range)-4.383));
      // until June 2006 used to be   if(range>0.) cfh.SetMomentum(0.048 + 1.660e-3*range + 3.057e-8*range*range);     

      if (range>0.){
        cfh.SetMomentum(GetMomFromRange(range));   
      }
      else cfh.SetMomentum(0.);      
      Calibrate(&cfh);
      
    } // end if istatus>=0

    cfh.ClearTrackClusterList();
    cfh.ClearKalmanPlaneList();
    
    //check and see if the track passes the cuts for a good track
    Float_t nplaneu = (Float_t)(cfh.GetNPlane(PlaneView::kU));
    Float_t nplanev = (Float_t)(cfh.GetNPlane(PlaneView::kV));
    Int_t nplane = cfh.GetNPlane();
    if(istatus>=0 && cfh.GetNDOF()>0 
       && cfh.GetChi2()/cfh.GetNDOF()<=fParmPassReducedChi2
       && fParmDeltaChi2<=0. && fParmDeltaCovariance<=0.
       && (nplane<fParmPassMinPlaneAsymmetry 
           || TMath::Abs((nplaneu-nplanev)/(1.*nplane))<=fParmPassPlaneAsymmetry )
       )
      cfh.SetPass(1);
    else {
      cfh.SetPass(0);
      SetTrackParameters(track0, cfh, idir);
      CandStripHandleItr trkstripItr(cfh.GetDaughterIterator());
      while( (strip = trkstripItr()) ){
	cfh.RemoveDaughter(strip);
      }
      CandStripHandleItr trk0stripItr(track0->GetDaughterIterator());
      while( (strip = trk0stripItr()) ){
	cfh.AddDaughterLink(*strip);
      }
      // fill time and range maps
      SetT(&cfh);
      SetUVZ(&cfh);
      SetdS(&cfh);
      Calibrate(&cfh);
    }
  }//end if enough planes to try fitting the track

  //delete the track clusters you found earlier
  trackClusterList->Delete();
  delete trackClusterList;

  for (Int_t iplane=0;iplane<1000;iplane++){
    planeClusters=(TObjArray *)planeClusterList.At(iplane);
    if(planeClusters){
      delete planeClusters;
    }
  }
  //  cout << " # for swim " << pFor << " # rev swim " << pRev << " # rev search swim " << pRev2 << endl;
  return;
}

//-------------------------------------------------------------------
void AlgFitTrackSR::InitKalmanFitParameters(AlgConfig &ac)
{

  fParmDState[kU] = ac.GetDouble("KalmanDState1");
  fParmDState[kV] = ac.GetDouble("KalmanDState2");
  fParmDState[kdUdZ] = ac.GetDouble("KalmanDState3");
  fParmDState[kdVdZ] = ac.GetDouble("KalmanDState4");
  fParmDState[kQoverP] = ac.GetDouble("KalmanDState5");
  fParmPlnRadLen = ac.GetDouble("KalmanPlnRadLen");
  fParmdedx = ac.GetDouble("Kalmandedx");
  fParmTPosError2 = ac.GetDouble("TPosError2");
  fParmMaxQP = ac.GetDouble("KalmanMaxQP");
  fParmQPRangeCheck = ac.GetInt("KalmanQPRangeCheck");
  fParmMaxQPFrac = ac.GetDouble("KalmanMaxQPFrac");
  fParmMaxAngle = ac.GetDouble("KalmanMaxAngle");
  fSwimmer = ac.GetInt("Swimmer");
 MSG("FitTrackSR", Msg::kDebug) << "initial state = " << fParmDState[kU] << " " 
                                 << fParmDState[kV] << " " << fParmDState[kdUdZ] << " " 
                                 << fParmDState[kdVdZ] << " " << fParmDState[kQoverP] << endl;
  return;
}  

//----------------------------------------------------------------------
void AlgFitTrackSR::InitializeFit(CandFitTrackSRHandle &cfh,Int_t idir, Int_t iterate)
{

  KalmanPlaneSR *kp = 0;
  for(Int_t i=0; i<=cfh.GetKalmanLast(); ++i){
    kp = cfh.GetKalmanPlane(i);
    kp->SetPredictPlane(-999,idir);
  }

  KalmanPlaneSR *currentkp = cfh.GetCurrentKalmanPlane();
  //fill up the covariance matrix for the current kp
  for(Int_t i=0; i<5; ++i){
    for(Int_t j=0; j<5; ++j){
      currentkp->SetFilCovarianceMatrixValue(idir,i,j,0.);
    }
  }

  currentkp->SetFilCovarianceMatrixValue(idir,kU,kU,fParmInitialPositionError2);
  currentkp->SetFilCovarianceMatrixValue(idir,kV,kV,fParmInitialPositionError2);
  currentkp->SetFilCovarianceMatrixValue(idir,kdUdZ,kdUdZ,fParmInitialSlopeError2);
  currentkp->SetFilCovarianceMatrixValue(idir,kdVdZ,kdVdZ,fParmInitialSlopeError2);
  currentkp->SetFilCovarianceMatrixValue(idir,kQoverP,kQoverP,fParmInitialQPError2);

  Double_t cov_xqp = currentkp->GetFilCovarianceMatrixValue(idir,kQoverP,kQoverP)*0.01*0.3*0.025;
  cov_xqp *= (Double_t)(currentkp->GetZDir());
  
  currentkp->SetFilCovarianceMatrixValue(idir,kU,kQoverP,cov_xqp);
  currentkp->SetFilCovarianceMatrixValue(idir,kV,kQoverP,cov_xqp);
  currentkp->SetFilCovarianceMatrixValue(idir,kQoverP,kU,cov_xqp);
  currentkp->SetFilCovarianceMatrixValue(idir,kQoverP,kV,cov_xqp);
  
  //set the filtered state vector with either the vertex or end 
  //point values depending on whether the fit is in the forward or
  //reverse direction
  if(idir==0){
    // we begin with the vertex position
    currentkp->SetFilStateValue(kU,0,cfh.GetVtxU());
    currentkp->SetFilStateValue(kV,0,cfh.GetVtxV());
    currentkp->SetFilStateValue(kdUdZ,0,cfh.GetDirCosU()/cfh.GetDirCosZ());
    currentkp->SetFilStateValue(kdVdZ,0,cfh.GetDirCosV()/cfh.GetDirCosZ());
    currentkp->SetFilStateValue(kQoverP,0,cfh.GetInitialQP());
  }
  else{
    currentkp->SetFilStateValue(kU,1,cfh.GetEndU());
    currentkp->SetFilStateValue(kV,1,cfh.GetEndV());
    currentkp->SetFilStateValue(kdUdZ,1,cfh.GetEndDirCosU()/cfh.GetEndDirCosZ());
    currentkp->SetFilStateValue(kdVdZ,1,cfh.GetEndDirCosV()/cfh.GetEndDirCosZ());
    currentkp->SetFilStateValue(kQoverP,1,cfh.GetEndQP());
  }
    
  MSG("FitTrackSR",Msg::kDebug) << "Initializing fit, direction = " << idir 
                                << " iteration = " << iterate << " plane = "
                                << currentkp->GetTrackCluster()->GetPlane() << " " 
                                << currentkp->GetFilStateValue(kU,idir) << " " 
                                << currentkp->GetFilStateValue(kV,idir) << " " 
                                << currentkp->GetFilStateValue(kdUdZ,idir) << " " 
                                << currentkp->GetFilStateValue(kdVdZ,idir) << " " 
                                << currentkp->GetFilStateValue(kQoverP,idir) << endl;

  if(!(idir==0 && iterate==0) && iterate>=0){
    fCov[kU][kU] *= fParmCovarianceScale;
    fCov[kV][kV] *= fParmCovarianceScale;
    fCov[kdUdZ][kdUdZ] *=fParmCovarianceScale;
    fCov[kdVdZ][kdVdZ] *= fParmCovarianceScale;
    fCov[kQoverP][kQoverP] *=fParmCovarianceScale;
    
    // make sure none of these are larger than our very first covariance elements
    fCov[kU][kU] = min(fCov[kU][kU],fParmInitialPositionError2);
    fCov[kV][kV] = min(fCov[kV][kV],fParmInitialPositionError2);
    fCov[kdUdZ][kdUdZ] = min(fCov[kdUdZ][kdUdZ],fParmInitialSlopeError2);
    fCov[kdVdZ][kdVdZ] = min(fCov[kdVdZ][kdVdZ],fParmInitialSlopeError2);
    fCov[kQoverP][kQoverP] = min(fCov[kQoverP][kQoverP],fParmInitialQPError2);

    Double_t cov_xqp = fParmInitialQPError2*0.01*0.3*0.025;
    for(Int_t i=0; i<2; ++i){
      if(TMath::Abs(fCov[i][kQoverP])>cov_xqp) fCov[i][kQoverP] = (fCov[i][kQoverP] > 0 ? cov_xqp : -cov_xqp);
      if(TMath::Abs(fCov[kQoverP][i])>cov_xqp) fCov[kQoverP][i] = (fCov[kQoverP][i] > 0 ? cov_xqp : -cov_xqp);
    }//end loop over covariance stuff

    cov_xqp /= 0.06;
    for(Int_t i=2; i<4; ++i){
      if(TMath::Abs(fCov[i][kQoverP])>cov_xqp) fCov[i][kQoverP] = (fCov[i][kQoverP] > 0 ? cov_xqp : -cov_xqp);
      if(TMath::Abs(fCov[kQoverP][i])>cov_xqp) fCov[kQoverP][i] = (fCov[kQoverP][i] > 0 ? cov_xqp : -cov_xqp);
    }//end loop to fill covariance matrix stuff

    //set the filtered covariance matrix of the current kp to be the same as fCov
    currentkp->SetFilCovarianceMatrix(idir,fCov);

  }//end if !(idir==0 && iterate==0) && iterate>=0
  
  MsgFormat ffmt("%10.5f");
  MSG("FitTrackSR",Msg::kDebug) << "Covariance Matrix\n";
  for (Int_t i=0; i<5; i++) {
    MSG("FitTrackSR",Msg::kDebug) << ffmt(currentkp->GetFilCovarianceMatrixValue(idir,i,kU))
                                 << ffmt(currentkp->GetFilCovarianceMatrixValue(idir,i,kV))
                                 << ffmt(currentkp->GetFilCovarianceMatrixValue(idir,i,kdUdZ))
                                 << ffmt(currentkp->GetFilCovarianceMatrixValue(idir,i,kdVdZ))
                                 << ffmt(currentkp->GetFilCovarianceMatrixValue(idir,i,kQoverP))
                                 << endl;
  }

  if(idir==0){
    cfh.SetVtxZ(currentkp->GetTrackCluster()->GetZPos());
    cfh.SetVtxPlane(currentkp->GetTrackCluster()->GetPlane());
  }
}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::AddToFit(CandFitTrackSRHandle & cfh, 
			      TrackClusterSR *tc, 
			      Int_t iterate)
{

  
// < 0 = failure
// 0 = success

  KalmanPlaneSR *kpnew = new KalmanPlaneSR(tc);
  kpnew->SetRange(cfh.GetRange(kpnew->GetTrackCluster()->GetPlane()));
  
  if(cfh.GetTimeSlope()>0. || !fParmIsCosmic) kpnew->SetZDir(1);
  else kpnew->SetZDir(-1);
  
  return AddToFit(cfh, kpnew, iterate,1);
}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::AddToFit(CandFitTrackSRHandle & cfh, 
			      KalmanPlaneSR *thiskp,
			      Int_t iterate, Bool_t docheck)
{

 

// < 0 = failure
// 0 = success

// this method assumes that the plane at thiskp is not already in the kalmanplanelist
// note also that this method takes ownership of thiskp
// if docheck is false, user is responsible for insertion into kalmanplanelist
// before call to AddToFit

  TObjArray * KalmanPlaneList=cfh.GetPlaneList();
 
  Bool_t found(0);
  Int_t ifound=0;
  Int_t ibef=-1;
  Int_t ilast;

  KalmanPlaneSR *kpnew = 0;
  KalmanPlaneSR *kp = 0;
  KalmanPlaneSR *kpold = 0;
  
  if(docheck){
    
    for(Int_t i=0; i<=KalmanPlaneList->GetLast(); ++i){
      
      // assume only one track cluster per plane in track
      kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
      
      if(kp->GetTrackCluster()->GetPlane() == thiskp->GetTrackCluster()->GetPlane()) {
        found = 1;
        ifound = i;
        kpnew = kp;

        // kalmanplane already in list, need to delete thiskp
        delete thiskp;
      } 
      else if((kp->GetZDir())*(kp->GetTrackCluster()->GetPlane())<(kp->GetZDir())*(thiskp->GetTrackCluster()->GetPlane())){
        ibef = i;
      }
    }//end loop over kalman planes

    //didnt find a plane to bound this one, so add it at the last place in the list
    if(!found){
      kpnew = thiskp;

      ilast = KalmanPlaneList->GetLast();
      
      for(int i=ilast; i>ibef; --i){
        kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
        KalmanPlaneList->AddAtAndExpand(kp,i+1);
      }

      KalmanPlaneList->AddAtAndExpand(kpnew,ibef+1);
    }//end didnt find a plane
  }//end if docheck 
  else kpnew = thiskp;
  
  cfh.SetCurrentKalmanPlane(kpnew);

  //if this is the first kp added to the track, initialize the fit
  if(KalmanPlaneList->GetLast()==0 
     || (docheck && ((found && ifound==0) || ibef<0))) InitializeFit(cfh,0,iterate);
  else{
  
    //get the plane of the kp immediately before the one to add to the 
    //fit
    kpold = dynamic_cast<KalmanPlaneSR*>
      (KalmanPlaneList->At(KalmanPlaneList->IndexOf(kpnew)-1));
    
    //fit from the previously fit plane to the one to add to the fit
    pFor++;
    Int_t nswimfail = FitFrom(kpold,kpnew,0,iterate);
    if(nswimfail<0){
      return nswimfail;
    }

    cfh.SetNSwimFail(cfh.GetNSwimFail()+nswimfail);
    for(int i1=0; i1<5; ++i1){
      for(int i2=0; i2<5; ++i2){
        fCov[i1][i2] = kpnew->GetFilCovarianceMatrixValue(0,i1,i2);
      }
    }//end loop to fill fCov covariance matrix
    MSG("FitTrackSR",Msg::kDebug) << "fitting forward, prechi2 = " 
                                  << kpnew->GetPreChi2(0) << " filchi2 = " 
                                  << kpnew->GetFilChi2(0) << endl;


    cfh.SetEnd(kpnew);
  }//end else

  return 0;
}


//-------------------------------------------------------------------
Int_t AlgFitTrackSR::Predict(KalmanPlaneSR *prevkp,
			     KalmanPlaneSR *currentkp,  
			     Int_t idir)
{

//
// < 0 indicates failure
// return value =  0        success
// > 0 indicates number of swim failures
//
  Int_t nswimfail=0;
  MSG("FitTrackSR", Msg::kDebug) << "in Predict" << endl;
  //make sure you are looking at different planes
  if(prevkp->GetTrackCluster()->GetPlane()!=currentkp->GetPredictPlane(idir)) {
    
    Int_t swimstatus = CalculatePropagator(prevkp,currentkp,idir);
    
    if(swimstatus<0){
    MSG("FitTrackSR",Msg::kDebug) << "failed to calculate propagator" << endl;
      return swimstatus;
    }

    nswimfail += swimstatus;

    //get the Q_{k-1} matrix
    CalculateNoise(prevkp, currentkp, idir); 

    //get the C_{k}^{k-1} matrix
    CalculatePreCovariance(prevkp, currentkp, idir); 

    //extrapolate from the previously fit plane to the plane to add
    //to the fit using the swimmer - this takes the place of the F_{k-1}x_{k-1}
    //matrix multiplication in the Kalman Filtering equations.
    swimstatus = CalculatePreState(prevkp, currentkp, idir);

    if(swimstatus<0){
    MSG("FitTrackSR",Msg::kDebug) << "failed to calculate predicted state" << endl;
      return swimstatus;
    }

    nswimfail += swimstatus;
  }//end if on a different plane than the predict plane

  CalculatePreChi2(currentkp, idir,currentkp->GetTrackCluster()->GetTPosError());

  //set the plane you predicted the fit for the current one from
  currentkp->SetPredictPlane(prevkp->GetTrackCluster()->GetPlane(), idir);

  //i am not sure if the following lines are needed or not - they are done in
  //the CalculatePreState method, but that is only called if the plane to add
  //to the fit is different plane than the previously fit plane - dunno, guess
  //i will leave them in

  //---------------------------------------------------------------------------//
  if(TMath::Abs(currentkp->GetPreStateValue(kdUdZ, idir))>fParmMaxAngle){
    if(currentkp->GetPreStateValue(kdUdZ, idir)>0.) 
      currentkp->SetPreStateValue(kdUdZ, idir, fParmMaxAngle);
    if(currentkp->GetPreStateValue(kdUdZ, idir)<0.) 
      currentkp->SetPreStateValue(kdUdZ, idir, -fParmMaxAngle);
  }

  if(TMath::Abs(currentkp->GetPreStateValue(kdVdZ, idir))>fParmMaxAngle){
    if(currentkp->GetPreStateValue(kdVdZ, idir)>0.) 
      currentkp->SetPreStateValue(kdVdZ, idir, fParmMaxAngle);
    if(currentkp->GetPreStateValue(kdVdZ, idir)<0.) 
      currentkp->SetPreStateValue(kdVdZ, idir, -fParmMaxAngle);
  }
  
  Double_t maxqp = fParmMaxQP;

  //use a value of about 2 MeV/(g/cm^2) for the energy loss in the material
  //you would divide the max qp frac allowed by that value and then by the range to
  //get the max qp.  but multiplication is a faster operation, so multiply by
  //500 instead.

  if ( fParmQPRangeCheck ) {
      if(currentkp->GetRange()>0. && fParmMaxQPFrac*500./currentkp->GetRange()<maxqp) {
          maxqp = fParmMaxQPFrac*500./currentkp->GetRange();
      }
  }
    
  if(idir==0 && TMath::Abs(currentkp->GetPreStateValue(kQoverP, idir))>maxqp){
    if(currentkp->GetPreStateValue(kQoverP, idir)>0.) 
      currentkp->SetPreStateValue(kQoverP, idir, maxqp);
    else currentkp->SetPreStateValue(kQoverP, idir, -maxqp);
  }
  //---------------------------------------------------------------------------//

  return nswimfail;
}


//-------------------------------------------------------------------
Int_t AlgFitTrackSR::Filter(KalmanPlaneSR *currentkp, Int_t idir)
{
//
// return value =  0        success
//

  //get the K_{k} matrix
  CalculateGain(currentkp, idir,currentkp->GetTrackCluster()->GetTPosError());
  
  //do the C_{k} = (I-K_{k}H_{k})C_{k}^{k-1} step
  CalculateFilCovariance(currentkp, idir);

  //find the filtered state
  CalculateFilState(currentkp, idir);

  // calculate noise and precovariance matrices onward again, values may have changed

  CalculateFilChi2(currentkp, idir,currentkp->GetTrackCluster()->GetTPosError());

  //reset the predicted from plane -- filtered state has changed
  currentkp->SetPredictPlane(-999, idir); 

  if(TMath::Abs(currentkp->GetFilStateValue(kdUdZ,idir))>fParmMaxAngle){
    if(currentkp->GetFilStateValue(kdUdZ,idir)>0.) 
      currentkp->SetFilStateValue(kdUdZ, idir, fParmMaxAngle);
    if(currentkp->GetFilStateValue(kdUdZ,idir)<0.) 
      currentkp->SetFilStateValue(kdUdZ, idir, -fParmMaxAngle);;
  }
  if(TMath::Abs(currentkp->GetFilStateValue(kdVdZ, idir))>fParmMaxAngle){
    if(currentkp->GetFilStateValue(kdVdZ, idir)>0.) 
      currentkp->SetFilStateValue(kdVdZ, idir, fParmMaxAngle);
    if(currentkp->GetFilStateValue(kdVdZ, idir)<0.) 
      currentkp->SetFilStateValue(kdVdZ, idir, -fParmMaxAngle);
  }

  Double_t maxqp = fParmMaxQP;

  //use a value of about 2 MeV/(g/cm^2) for the energy loss in the material
  //you would divide the max qp frac allowed by that value and then by the range to
  //get the max qp.  but multiplication is a faster operation, so multiply by
  //500 instead.
  if ( fParmQPRangeCheck ) {
      if(currentkp->GetRange()>0. && fParmMaxQPFrac*500./currentkp->GetRange()<maxqp) {
          maxqp = fParmMaxQPFrac*500./currentkp->GetRange();
      }
  }

  if(idir==0 && TMath::Abs(currentkp->GetFilStateValue(kQoverP,idir))>maxqp){
    if(currentkp->GetFilStateValue(kQoverP,idir)>0.) 
      currentkp->SetFilStateValue(kQoverP, idir, maxqp);
    else currentkp->SetFilStateValue(kQoverP, idir, -maxqp);
  }
 
  return 0;
}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::FitFrom(KalmanPlaneSR *prevkp, 
                                    KalmanPlaneSR *currentkp, 
			     Int_t idir, Int_t /*iterate*/)
{
  

//
// < 0 = failure
// return value =  0        success
//

  Int_t nswimfail = 0;
MSG("FitTrackSR",Msg::kDebug) << "in FitFrom: " 
                                << "  Fitting direction " << idir << " " 
                                << currentkp->GetTrackCluster()->GetPlane() << " " 
                                << currentkp->GetTrackCluster()->GetTPos() 
                                << " min/max strip = " 
                                << currentkp->GetTrackCluster()->GetMinStrip() << "/" 
                                << currentkp->GetTrackCluster()->GetMaxStrip() 
                                << currentkp->GetZDir() << endl;
 

 //predict the state of the plane to add to the fit based on the 
  //previously fit plane
  nswimfail = Predict(prevkp, currentkp, idir);

  if(nswimfail<0){
    return nswimfail;
  }

  Int_t istatus = 0;
  istatus = Filter(currentkp, idir);

  MSG("FitTrackSR",Msg::kDebug)  << "fit point " << idir << " "  << " " 
                                 << currentkp->GetTrackCluster()->GetPlane() << " " 
                                 << currentkp->GetTrackCluster()->GetTPos() << endl;

  MSG("FitTrackSR",Msg::kDebug) << "fit qp = " 
                                << currentkp->GetFilStateValue(kQoverP,idir) 
                                << " sigma q/p= "
                                << currentkp->GetFilCovarianceMatrixValue(0,
                                                                          kQoverP,kQoverP)
                                << "/" 
                                << currentkp->GetFilCovarianceMatrixValue(1,
                                                                          kQoverP,kQoverP)
                                << " range (g/cm**2) = " << currentkp->GetRange() 
                                << " maximum qp from range = " << fParmMaxQPFrac 
                                << "/0.002/" << currentkp->GetRange() << " = " 
                                << (currentkp->GetRange()>0. 
                                    ? fParmMaxQPFrac/0.002/currentkp->GetRange() : 0.) 
                                << " maxqpparm = " << fParmMaxQP << endl;

  return nswimfail;
}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::CalculatePropagator(KalmanPlaneSR *prevkp, 
					 KalmanPlaneSR *currentkp,
					 Int_t idir)
{
  // we are actually calculating the tranpose
  MSG("FitTrackSR", Msg::kDebug) << "In CalculatePropagator" << endl;
  Int_t nswimfail=0;
  Double_t dstate[5];
  Double_t thisFilState[5];

  for(Int_t i=0; i<5; ++i){

    //set the filtered state based on the previously fit plane
    thisFilState[i] = prevkp->GetFilStateValue(i,idir);
    dstate[i] = fParmDState[i];
    
    if(i==4){
      dstate[i] = (fParmDState[i]*thisFilState[kQoverP]*thisFilState[kQoverP]
                   /(1.-fParmDState[i]*TMath::Abs(thisFilState[kQoverP])));
      dstate[i] = fParmDState[i]*TMath::Abs(thisFilState[kQoverP]);
      if(dstate[i]<.01) dstate[i] = .01;
      // corresponds to constant fParmDState[kQoverP] energy interval
    }
    //    MSG("FitTrackSR", Msg::kDebug) << "thisFilState[" << i << "] = " 
    //				<< thisFilState[i] << " dstate[" << i 
    //				<< "] = " << dstate[i] << endl;
  }

  Bool_t swimstatus = false;
  Int_t nswim=0;
  Double_t swimstate[5];
  Double_t pstate[] = {0.,0.,0.,0.,0.};
  Double_t nstate[] = {0.,0.,0.,0.,0.};

  //loop over the vectors and swim the particle until you have a good
  //swim or you excede the maximum iterations
  while(!swimstatus && nswim<100){
    
    swimstatus = true;

    //loop over all the states and change them by +/- dstate and swim from
    //that point to the next z pos. this figures out the propagator matrix
    for(Int_t i=0; i<5; ++i){
      if(i==4){
        dstate[i] = (fParmDState[i]*thisFilState[kQoverP]*thisFilState[kQoverP]
                     /(1.-fParmDState[i]*TMath::Abs(thisFilState[kQoverP])));
        dstate[i] = fParmDState[i]*TMath::Abs(thisFilState[kQoverP]);
        if(dstate[i]<.01) dstate[i] = .01;
      }

      swimstate[kU] = thisFilState[kU];
      swimstate[kV] = thisFilState[kV];
      swimstate[kdUdZ] = thisFilState[kdUdZ];
      swimstate[kdVdZ] = thisFilState[kdVdZ];
      swimstate[kQoverP] = thisFilState[kQoverP];
      
      swimstate[i] += dstate[i];

      if(fSwimmer!=2) MSG("FitTrackSR",Msg::kError) << "fSwimmer!=2 currently unsupported" << endl;      
      else if(fSwimmer==2){
        
        // Using Swimmer package
        if(!(Swim(swimstate, pstate, prevkp->GetTrackCluster()->GetZPos(),
                  currentkp->GetTrackCluster()->GetZPos(), idir, 
                  prevkp->GetVldContext()))) swimstatus = false;
      

        // if outside fiducial detector, return failure flag
        if(TMath::Abs(pstate[kU])>10. || TMath::Abs(pstate[kV])>10.){
          return -1;
        }
        
        swimstate[kU] = thisFilState[kU];
        swimstate[kV] = thisFilState[kV];
        swimstate[kdUdZ] = thisFilState[kdUdZ];
        swimstate[kdVdZ] = thisFilState[kdVdZ];
        swimstate[kQoverP] = thisFilState[kQoverP];
        
        swimstate[i] -= dstate[i];
        
        if(!(Swim(swimstate, nstate, prevkp->GetTrackCluster()->GetZPos(),
                  currentkp->GetTrackCluster()->GetZPos(), idir, 
                  currentkp->GetVldContext()))) swimstatus = false;
      

        // if outside fiducial detector, return failure flag
        if(TMath::Abs(nstate[kU])>10. || TMath::Abs(nstate[kV])>10.){
                 MSG("FitTrackSR",Msg::kDebug) << "outside detector, pstate u,v = " << pstate[kU] 
                                       << " " << pstate[kV] << " fail" << endl;  
          return -1;
        }
  

        //get the value of the propagator matrix terms for the current column of the
        //matrix
        Double_t propValue = 0.;
        for(Int_t j=0; j<5; ++j){
        
          if(dstate[i]!=0.)  propValue = (pstate[j]-nstate[j])/(2.*dstate[i]);
          else{
            if(j==i) propValue = 1.;
            else propValue = 0.;
          }
          prevkp->SetPropagatorMatrixValue(idir, i, j, propValue);
        }
      
      }//end if fSwimmer is for the Swim package
    }//end loop over matrix rows

    if(!swimstatus){

      // swim failed, double momentum
      thisFilState[kQoverP] *= 0.5;
      ++nswimfail;
 MSG("FitTrackSR",Msg::kDebug) << "failed swim, doubling momentum to q/p = " 
                                   << thisFilState[kQoverP] << " nswim = " << nswim << endl;
    }

    ++nswim;

  } 

  return nswimfail;
}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculateNoise(KalmanPlaneSR *prevkp, 
				   KalmanPlaneSR *currentkp, 
				   Int_t idir, Int_t befaft)
{


  MSG("FitTrackSR", Msg::kDebug) << "In CalculateNoise" << endl;

  //find noise from the previous kp to the kp to add to the fit
  KalmanPlaneSR *thiskp = prevkp;
  KalmanPlaneSR *newkp = currentkp;
  Int_t thisdir = idir;

  if(befaft){
    thiskp = currentkp;
    newkp = prevkp;
    thisdir = 1-thisdir;
  }

  Int_t dplane = (currentkp->GetTrackCluster()->GetPlane()
                  - prevkp->GetTrackCluster()->GetPlane());
  Int_t izdir = (dplane>0 ? 0 : 1);

  // izdir gives the direction of the swim (0 = forward, 1 = backward)
  // relative to z
  // this is different from thisdir/idir which gives direction of swim
  // relative to time
  // idir=0 means forward in time, idir=1 means backward in time
  // a particle could be going forward in z and forward in time or
  // any of the 4 combinations (for cosmics and atmospherics)
  Double_t dudz = thiskp->GetFilStateValue(kdUdZ,idir);
  Double_t dvdz = thiskp->GetFilStateValue(kdVdZ,idir);
  Double_t dsdz = sqrt(1.+dudz*dudz+dvdz*dvdz);
  Double_t qp = thiskp->GetFilStateValue(kQoverP,idir);

  // when qp is too small, errors go to zero, so place lower bound
  // 0.01 (= 100 GeV/c) is reasonable
  if(TMath::Abs(qp)<0.01) qp = (qp>0 ? 0.01 : -0.01);
  
  Double_t locradlen = TMath::Abs(dsdz*(Double_t)(dplane)*fParmPlnRadLen);

  // we should really use an average of 1/p in calculating sigmams2
  Double_t sigmams2 = (0.0136*TMath::Abs(qp)*TMath::Sqrt(locradlen)
                       *(1.+0.038*TMath::Log(locradlen)));
  sigmams2 *= sigmams2;

  Double_t p3p3 = 0.5*sigmams2*(1.+dudz*dudz)*dsdz*dsdz;
  Double_t p3p4 = 0.5*sigmams2*dudz*dvdz*dsdz*dsdz;
  Double_t p4p4 = 0.5*sigmams2*(1.+dvdz*dvdz)*dsdz*dsdz;

//  SwimPlaneInterfaceListSR *spil = 
//    SwimPlaneInterfaceListSR::Instance(const_cast<VldContext*>(GetVldContext()));

  SwimGeo *spil = SwimGeo::Instance(*(const_cast<VldContext*>(currentkp->GetVldContext())));

  Double_t z1 = spil->GetSteel(prevkp->GetTrackCluster()->GetZPos(),izdir)->GetZ();
  Double_t z2 = spil->GetSteel(z1,izdir)->GetZ();

  // treat iron as discrete scatter; in future can make this more rigorous
  Double_t dzscatter = TMath::Abs(currentkp->GetTrackCluster()->GetZPos()-0.5*(z1+z2));
  Double_t dzscatter2 = dzscatter*dzscatter;
  Double_t eloss = fParmdedx*TMath::Abs((Double_t)(dplane));

  VldContext vldc = *(prevkp->GetVldContext());
  UgliGeomHandle ugh(vldc);
  BField bf(vldc);
  TVector3 uvz;
  uvz(0) = thiskp->GetFilStateValue(kU,idir);
  uvz(1) = thiskp->GetFilStateValue(kV,idir);
  // rwh: KalmanPlane's TrackCluster z is not necessarily inside the steel
  //uvz(2) = thiskp->GetTrackCluster()->GetZPos();
  // translate an semi-arbitrary z to z0 of the nearest steel plane
  uvz(2) = ugh.GetNearestSteelPlnHandle(thiskp->GetTrackCluster()->GetZPos()).GetZ0();
  TVector3 xyz(kInvSqrt2*(uvz[0]-uvz[1]),kInvSqrt2*(uvz[0]+uvz[1]),uvz[2]);
  TVector3 bxyz = bf.GetBField(xyz);

  // rotate bxyz into buvz
  // bxyz in Tesla, convert into GeV/c/m
  // warning: numbers are hard coded here
  TVector3 buvz(kInvSqrt2*(bxyz[1]+bxyz[0]),kInvSqrt2*(bxyz[1]-bxyz[0]),bxyz[2]);
  
  buvz[0] *= 0.3;
  buvz[1] *= 0.3;
  buvz[2] *= 0.3;

  // we assume the uncertainty in energy loss to follow a Landau distribution,
  // and use the FWHM from Ahlen (RMP, Vol. 52, p. 143, 1980)
  Double_t sigmaeloss2 = 0.25*eloss*dsdz*qp*qp;
  sigmaeloss2 *= sigmaeloss2;

  currentkp->SetNoiseMatrixValue(idir,kU,kU,dzscatter2*p3p3);
  currentkp->SetNoiseMatrixValue(idir,kU,kV,dzscatter2*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kU,kdUdZ,dzscatter*p3p3);
  currentkp->SetNoiseMatrixValue(idir,kU,kdVdZ,dzscatter*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kU,kQoverP,
                                 (dzscatter*sigmaeloss2*TMath::Abs((Double_t)(dplane))
                                  *buvz(1)*0.0254*Munits::m*dsdz*(1.+dudz*dudz)));
  currentkp->SetNoiseMatrixValue(idir,kV,kU,dzscatter2*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kV,kV,dzscatter2*p4p4);
  currentkp->SetNoiseMatrixValue(idir,kV,kdUdZ,dzscatter*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kV,kdVdZ,dzscatter*p4p4);
  currentkp->SetNoiseMatrixValue(idir,kV,4,
                                 (dzscatter*sigmaeloss2*TMath::Abs((Double_t)(dplane))
                                  *buvz(0)*0.0254*Munits::m*dsdz*(1.+dvdz*dvdz)));
  currentkp->SetNoiseMatrixValue(idir,kdUdZ,kU,dzscatter*p3p3);
  currentkp->SetNoiseMatrixValue(idir,kdUdZ,kV,dzscatter*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kdUdZ,kdUdZ,p3p3);
  currentkp->SetNoiseMatrixValue(idir,kdUdZ,kdVdZ,p3p4);
  currentkp->SetNoiseMatrixValue(idir,kdUdZ,kQoverP,
                                 (sigmaeloss2*TMath::Abs((Double_t)(dplane))*buvz(1)
                                  *0.0254*Munits::m*dsdz*(1.+dudz*dudz)));
  currentkp->SetNoiseMatrixValue(idir,kdVdZ,kU,dzscatter*p3p4);
  currentkp->SetNoiseMatrixValue(idir,kdVdZ,kV,dzscatter*p4p4);
  currentkp->SetNoiseMatrixValue(idir,kdVdZ,kdUdZ,p3p4);
  currentkp->SetNoiseMatrixValue(idir,kdVdZ,kdVdZ,p4p4);
  currentkp->SetNoiseMatrixValue(idir,kdVdZ,kQoverP,
                                 (sigmaeloss2*TMath::Abs((Double_t)(dplane))*buvz(0)
                                  *0.0254*Munits::m*dsdz*(1.+dvdz*dvdz)));
  currentkp->SetNoiseMatrixValue(idir,kQoverP,kU,
                                 (dzscatter*sigmaeloss2*TMath::Abs((Double_t)(dplane))
                                  *buvz(1)*0.0254*Munits::m*dsdz*(1.+dudz*dudz)));
  currentkp->SetNoiseMatrixValue(idir,kQoverP,kV,
                                 (dzscatter*sigmaeloss2*TMath::Abs((Double_t)(dplane))
                                  *buvz(0)*0.0254*Munits::m*dsdz*(1.+dvdz*dvdz)));
  currentkp->SetNoiseMatrixValue(idir,kQoverP,kdUdZ,
                                 (sigmaeloss2*TMath::Abs((Double_t)(dplane))*buvz(1)
                                  *0.0254*Munits::m*dsdz*(1.+dudz*dudz)));
  currentkp->SetNoiseMatrixValue(idir,kQoverP,kdVdZ,
                                 (sigmaeloss2*TMath::Abs((Double_t)(dplane))*buvz(0)
                                  *0.0254*Munits::m*dsdz*(1.+dvdz*dvdz)));
  currentkp->SetNoiseMatrixValue(idir,kQoverP,kQoverP,sigmaeloss2);

  MSG("FitTrackSR",Msg::kDebug) << "du/dz, dv/dz, ds/dz, qp = " << dudz << " " 
                                << dvdz << " " << dsdz << " " << qp << endl
                                << "dplane, dzscatter, z1, z2, radlen = " << dplane 
                                << " " << dzscatter << " " << z1 << " " << z2 << " " 
                                << locradlen << endl
                                << "sigmams2 = " << sigmams2 << endl
                                << "p3p3, p3p4, p4p4 = " << p3p3 << " " << p3p4 << " " 
                                << p4p4 << endl;

}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculatePreCovariance(KalmanPlaneSR *prevkp, 
					   KalmanPlaneSR *currentkp, 
					   Int_t idir)
{ 
  MSG("FitTrackSR", Msg::kDebug) << "In CalculatePreCovariance" << endl;
  //get the propagator and filtered covariance matricies from the
  //previously fit kp
  TMatrixD propagator(5,5);
  TMatrixD filCovariance(5,5);

  for(Int_t i = 0; i<5; ++i){
    for(Int_t j = 0; j<5; ++j){
      propagator(i,j) = prevkp->GetPropagatorMatrixValue(idir,i,j);
      filCovariance(i,j) = prevkp->GetFilCovarianceMatrixValue(idir,i,j);
      
      //start the calculation of the pre-filtered covariance matrix
      //for the kp to add to the fit.  set it = noise matrix of previously
      //fit plane
      currentkp->SetPreCovarianceMatrixValue(idir,i,j,prevkp->GetNoiseMatrixValue(idir,i,j));
    }
  }

  //remember that propagator is really the transpose of the propagator matrix
  //so in the following cov0 = F_{k-1}C_{k-1}
  TMatrixD cov0(propagator,TMatrixD::kTransposeMult,filCovariance);
  
  //the following makes cov0 = F_{k-1}C_{k-1}F^{T}_{k-1}
  cov0 *= propagator;

  //finish finding the pre-filtered covariance matrix - 
  //C_{k}^{k-1} = F_{k-1}C_{k-1}F^{T}_{k-1}+Q_{k-1}
  Double_t preCovValue = 0.;
  for(Int_t i = 0; i<5; ++i){
    for(Int_t j = 0; j<5; ++j){
      preCovValue = currentkp->GetPreCovarianceMatrixValue(idir,i,j)+cov0(i,j);
      currentkp->SetPreCovarianceMatrixValue(idir,i,j,preCovValue);
    }
  }

  // diagonal elements should be positive
  Double_t covlim = 1.e-8;
  if(currentkp->GetPreCovarianceMatrixValue(idir,kU,kU)<covlim) 
    currentkp->SetPreCovarianceMatrixValue(idir,kU,kU,covlim);
  if(currentkp->GetPreCovarianceMatrixValue(idir,kV,kV)<covlim) 
    currentkp->SetPreCovarianceMatrixValue(idir,kV,kV,covlim);
  if(currentkp->GetPreCovarianceMatrixValue(idir,kdUdZ,kdUdZ)<covlim) 
    currentkp->SetPreCovarianceMatrixValue(idir,kdUdZ,kdUdZ,covlim);
  if(currentkp->GetPreCovarianceMatrixValue(idir,kdVdZ,kdVdZ)<covlim) 
    currentkp->SetPreCovarianceMatrixValue(idir,kdVdZ,kdVdZ,covlim);
  if(currentkp->GetPreCovarianceMatrixValue(idir,kQoverP,kQoverP)<covlim) 
    currentkp->SetPreCovarianceMatrixValue(idir,kQoverP,kQoverP,covlim);

}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::CalculatePreState(KalmanPlaneSR *prevkp, 
				       KalmanPlaneSR *currentkp,
				       Int_t idir)
{

  //Swim from the previously fit plane to the plane to add to the fit
  //to get the estimate of the state vector at the new plane - this 
  //takes care of the F_{k-1}x_{k-1} matrix multiplication in the
  //Kalman filter equations
  Int_t nswimfail=0;

  if(fSwimmer!=2) MSG("FitTrackSR",Msg::kError) << "fSwimmer!=2 currently unsupported" 
                                                << endl;
  MSG("FitTrackSR", Msg::kDebug) << "current Filtered State = [" 
                                 << prevkp->GetFilStateValue(kU,idir) << ","
                                 << prevkp->GetFilStateValue(kV,idir) << ","
                                 << prevkp->GetFilStateValue(kdUdZ,idir) << ","
                                 << prevkp->GetFilStateValue(kdVdZ,idir) << ","
                                 << prevkp->GetFilStateValue(kQoverP,idir) << "]" 
                                 << endl;
    
  if(fSwimmer==2){
    
    // Using Swimmer package
    Double_t swimstate[5];
    Double_t state[] = {0.,0.,0.,0.,0.};
    
    swimstate[kU] = prevkp->GetFilStateValue(kU,idir);
    swimstate[kV] = prevkp->GetFilStateValue(kV,idir);
    swimstate[kdUdZ] = prevkp->GetFilStateValue(kdUdZ,idir);
    swimstate[kdVdZ] = prevkp->GetFilStateValue(kdVdZ,idir);
    swimstate[kQoverP] = prevkp->GetFilStateValue(kQoverP,idir);
    
    Bool_t swimstatus = false;
    Int_t nswim=0;
    
    while(!swimstatus && nswim<100){

      swimstatus = Swim(swimstate, state, prevkp->GetTrackCluster()->GetZPos(),
                        currentkp->GetTrackCluster()->GetZPos(),idir,
                        currentkp->GetVldContext());
      
      // if outside fiducial detector, return failure flag
      if(TMath::Abs(state[kU])>5. || TMath::Abs(state[kV])>5.){
       MSG("FitTrackSR",Msg::kDebug) << "outside detector, u,v = " << state[kU] << " " 
                                      << state[kV] << " fail" << endl;
        
        return -1;
      }

      if(!swimstatus){
        
        // swim failed, double momentum
        swimstate[kQoverP] *= 0.5;          
     MSG("FitTrackSR",Msg::kDebug) << "swim failed, doubling momentum to q/p = " 
                                      << swimstate[kQoverP] << endl;

        ++nswimfail;
      }
      ++nswim;
    }//end loop to swim.

    currentkp->SetPreStateValue(kU,idir,state[kU]);
    currentkp->SetPreStateValue(kV,idir,state[kV]);
    currentkp->SetPreStateValue(kdUdZ,idir,state[kdUdZ]);
    currentkp->SetPreStateValue(kdVdZ,idir,state[kdVdZ]);
    currentkp->SetPreStateValue(kQoverP,idir,state[kQoverP]);
  }//end if using Swim package

  if(TMath::Abs(currentkp->GetPreStateValue(kdUdZ,idir))>fParmMaxAngle){
    currentkp->SetPreStateValue(kdUdZ,idir,
                                (currentkp->GetPreStateValue(kdUdZ,idir)>0 
                                 ? fParmMaxAngle : -fParmMaxAngle));
  }

  if(TMath::Abs(currentkp->GetPreStateValue(kdVdZ,idir))>fParmMaxAngle){
    currentkp->SetPreStateValue(kdVdZ,idir,
                                (currentkp->GetPreStateValue(kdVdZ,idir)>0 
                                 ? fParmMaxAngle : -fParmMaxAngle));
  }

  Double_t maxqp = fParmMaxQP;

  //use a value of about 2 MeV/(g/cm^2) for the energy loss in the material
  //you would divide the max qp frac allowed by that value and then by the range to
  //get the max qp.  but multiplication is a faster operation, so multiply by
  //500 instead.
  if ( fParmQPRangeCheck ) {
      if(currentkp->GetRange()>0. && fParmMaxQPFrac*500./currentkp->GetRange()<maxqp) {
          maxqp = fParmMaxQPFrac*500./currentkp->GetRange();
      }
  }

  if(TMath::Abs(currentkp->GetPreStateValue(kQoverP,idir))>maxqp){
    currentkp->SetPreStateValue(kQoverP,idir,
                                (currentkp->GetPreStateValue(kQoverP,idir)>0 ? maxqp : -maxqp));
  }

  return nswimfail;
}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculatePreChi2(KalmanPlaneSR *currentkp, 
				     Int_t idir, Double_t poserr)
{

  Int_t index = 0;

  //default view is U, change if this is a V view plane
  if(currentkp->GetTrackCluster()->GetPlaneView()==PlaneView::kV) index = 1;

  Double_t dtpos = 0.;

  dtpos = currentkp->GetPreStateValue(index,idir)-currentkp->GetTrackCluster()->GetTPos();

  Double_t preChi2 = dtpos*dtpos/(poserr*poserr);
  
  currentkp->SetPreChi2(preChi2, idir);
 MSG("FitTrackSR",Msg::kDebug) << "prechi2 = " << preChi2 << "  dtpos = " 
                                << dtpos << "  tposerr2 = " << poserr*poserr 
                                << "  cov = " 
                                << currentkp->GetPreCovarianceMatrixValue(idir,index,index) 
                                << endl;

}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculateGain(KalmanPlaneSR *currentkp, 
				  Int_t idir, Double_t poserr)
{

  //the following is the 
  //K_{k} = C_{k}^{k-1}H_{k}^{T}(V_{k}+H_{k}C_{k}^{k-1}H_{k}^{T})^{-1}
  //step of the filtering equations
  //remember that 
  //V_{k} = measurement error, 
  //H_{k} = [1,0,0,0,0] for U view; [0,1,0,0,0] for V view
  //the definition of H_{k} makes the H_{k}C_{k}^{k-1}H_{k}^{T} product
  //a scalar and picks out only certain elements of C_{k}^{k-1}

 
  Int_t index = 0;

  //default U view, change if V view
  if(currentkp->GetTrackCluster()->GetPlaneView()==PlaneView::kV) index = 1;


  currentkp->SetGainValue(kU,idir,
                          (currentkp->GetPreCovarianceMatrixValue(idir,kU,index)/
                           (currentkp->GetPreCovarianceMatrixValue(idir,index,index)
                            +poserr*poserr)));
  currentkp->SetGainValue(kV,idir,
                          (currentkp->GetPreCovarianceMatrixValue(idir,kV,index)/
                           (currentkp->GetPreCovarianceMatrixValue(idir,index,index)
                            +poserr*poserr)));
  currentkp->SetGainValue(kdUdZ,idir,
                          (currentkp->GetPreCovarianceMatrixValue(idir,kdUdZ,index)/
                           (currentkp->GetPreCovarianceMatrixValue(idir,index,index)
                            +poserr*poserr)));
  currentkp->SetGainValue(kdVdZ,idir,
                          (currentkp->GetPreCovarianceMatrixValue(idir,kdVdZ,index)/
                           (currentkp->GetPreCovarianceMatrixValue(idir,index,index)
                            +poserr*poserr)));
  currentkp->SetGainValue(kQoverP,idir,
                          (currentkp->GetPreCovarianceMatrixValue(idir,kQoverP,index)/
                           (currentkp->GetPreCovarianceMatrixValue(idir,index,index)
                            +poserr*poserr)));
  
}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculateFilState(KalmanPlaneSR *currentkp, 
				      Int_t idir)
{
 

  Int_t index = 0;

  //default U view, change if V view
  if(currentkp->GetTrackCluster()->GetPlaneView()==PlaneView::kV) index = 1;

  //the following is the m_{k} - H_{k}F_{k-1}x_{k-1}
  //bit of the Kalman filter equations
  //remember that 
  //m_{k} = measurement, 
  //H_{k} = [1,0,0,0,0] for U view; [0,1,0,0,0] for V view
  //the definition of H_{k} makes the H_{k}F_{k-1}x_{k-1} product
  //a scalar and picks out only certain elements of F_{k-1}x_{k-1}
  //also, F_{k-1}x_{k-1} is found by the swimmer and put in the 
  //pre filtered state vector of the plane to add to the fit

  Double_t xfac = (currentkp->GetTrackCluster()->GetTPos()
                   - currentkp->GetPreStateValue(index,idir));

  currentkp->SetFilStateValue(kU,idir, currentkp->GetPreStateValue(kU,idir)
                              +currentkp->GetGainValue(kU,idir)*xfac);
  currentkp->SetFilStateValue(kV,idir, currentkp->GetPreStateValue(kV,idir)
                              +currentkp->GetGainValue(kV,idir)*xfac);
  currentkp->SetFilStateValue(kdUdZ,idir, currentkp->GetPreStateValue(kdUdZ,idir)
                              +currentkp->GetGainValue(kdUdZ,idir)*xfac);
  currentkp->SetFilStateValue(kdVdZ,idir, currentkp->GetPreStateValue(kdVdZ,idir)
                              +currentkp->GetGainValue(kdVdZ,idir)*xfac);
  currentkp->SetFilStateValue(kQoverP,idir, currentkp->GetPreStateValue(kQoverP,idir)
                              +currentkp->GetGainValue(kQoverP,idir)*xfac);

  if(TMath::Abs(currentkp->GetFilStateValue(kdUdZ,idir))>fParmMaxAngle){
    currentkp->SetFilStateValue(kdUdZ,idir,(currentkp->GetFilStateValue(kdUdZ,idir)>0 
                                            ? fParmMaxAngle : -fParmMaxAngle));
  }

  if(TMath::Abs(currentkp->GetFilStateValue(kdVdZ,idir))>fParmMaxAngle){
    currentkp->SetFilStateValue(kdVdZ,idir,(currentkp->GetFilStateValue(kdVdZ,idir)>0 
                                            ? fParmMaxAngle : -fParmMaxAngle));
  }

  Double_t maxqp = fParmMaxQP;

  //use a value of about 2 MeV/(g/cm^2) for the energy loss in the material
  //you would divide the max qp frac allowed by that value and then by the range to
  //get the max qp.  but multiplication is a faster operation, so multiply by
  //500 instead.
  if ( fParmQPRangeCheck ) {
      if(currentkp->GetRange()>0. && fParmMaxQPFrac*500./currentkp->GetRange()<maxqp) {
          maxqp = fParmMaxQPFrac*500./currentkp->GetRange();
      }
  }

  if(TMath::Abs(currentkp->GetFilStateValue(kQoverP,idir))>maxqp){
    currentkp->SetFilStateValue(kQoverP,idir,(currentkp->GetFilStateValue(kQoverP,idir)>0 
                                              ? maxqp : -maxqp));
  }


}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculateFilChi2(KalmanPlaneSR *currentkp, 
				     Int_t idir, Double_t poserr)
{

  Int_t index = 0;

  //default U view, change if V view
  if(currentkp->GetTrackCluster()->GetPlaneView()==PlaneView::kV) index = 1;

  Double_t dtpos = 0.;

  dtpos = currentkp->GetFilStateValue(index,idir)-currentkp->GetTrackCluster()->GetTPos();
  
  Double_t filChi2 = dtpos*dtpos/(poserr*poserr);
  
  currentkp->SetFilChi2(filChi2, idir);
  MSG("FitTrackSR",Msg::kDebug) << "filchi2 = " << filChi2 << "  dtpos = " 
                                << dtpos << "  tposerr2 = " << poserr*poserr 
                                << "  cov = " 
                                << currentkp->GetFilCovarianceMatrixValue(idir,index,index) 
                                << endl;

}

//----------------------------------------------------------------------
void AlgFitTrackSR::CalculateFilCovariance(KalmanPlaneSR *currentkp, 
					   Int_t idir)
{
  //do the final step of the Kalman filter
  //C_{k} = (I - K_{k}H_{k})C_{k}^{k-1}
  
  Int_t index = 0;

  //default U view, change if V view
  if(currentkp->GetTrackCluster()->GetPlaneView()==PlaneView::kV) index = 1;
  
  TMatrixD preCov(5,5);
  for(Int_t i = 0; i<5; ++i){
    for(Int_t j = 0; j<5; ++j){
      preCov(i,j) = currentkp->GetPreCovarianceMatrixValue(idir,i,j);
    }
  }

  TMatrixD filCovariance(5,5);

  //set the filtered covariance matrix to the identity matrix
  filCovariance.UnitMatrix();

  //subtract K_{k}H_{k} from the identity
  filCovariance(kU,index) -= currentkp->GetGainValue(kU,idir);
  filCovariance(kV,index) -= currentkp->GetGainValue(kV,idir);
  filCovariance(kdUdZ,index) -= currentkp->GetGainValue(kdUdZ,idir);
  filCovariance(kdVdZ,index) -= currentkp->GetGainValue(kdVdZ,idir);
  filCovariance(kQoverP,index) -= currentkp->GetGainValue(kQoverP,idir);
  
  //multiply the difference by the pre filtered covariance matrix
  filCovariance *= preCov;

  // diagonal elements should be positive
  Double_t covlim = 1.e-8;
  if(filCovariance(kU,kU)<covlim) filCovariance(kU,kU) = covlim;
  if(filCovariance(kV,kV)<covlim) filCovariance(kV,kV) = covlim;
  if(filCovariance(kdUdZ,kdUdZ)<covlim)
    filCovariance(kdUdZ,kdUdZ) = covlim;
  if(filCovariance(kdVdZ,kdVdZ)<covlim)
    filCovariance(kdVdZ,kdVdZ) = covlim;
  if(filCovariance(kQoverP,kQoverP)<covlim) filCovariance(kQoverP,kQoverP) = covlim;
  
  for(Int_t i = 0; i<5; ++i){
    for(Int_t j = 0; j<5; ++j){
      currentkp->SetFilCovarianceMatrixValue(idir,i,j, filCovariance(i,j));
    }
  }
}

//----------------------------------------------------------------------
Bool_t AlgFitTrackSR::Swim(Double_t* swimstate, Double_t* state,
			   Double_t zPos, Double_t finalZ, Int_t idir, 
			   const VldContext* vldc)
{

  // return false if angles get too large or momentum is too small

  //-------------------------------------------------------------------
  // Using Swimmer package
  //-------------------------------------------------------------------
  SwimSwimmer s(*vldc);
  
  // Add an action to the stepper - in this case a debug printout
  //  SwimPrintStepAction printStep;
  //  s.GetStepper()->AddStepAction(&printStep);

  // Setup the initial condition for the stepper  
  //the swimmer works in xyz coordinates, not uvz, so need to rotate 
  //the coordinates before putting them into a position vector
  Double_t x = kInvSqrt2*(swimstate[kU]-swimstate[kV]);
  Double_t y = kInvSqrt2*(swimstate[kU]+swimstate[kV]);
	
  TVector3 position(x,y,zPos);
  double charge = 0.0;

  if(swimstate[kQoverP]>0.0) charge = 1.0;
  else if(swimstate[kQoverP]<0.0) charge = -1.0;
  else{

    // infinite momentum, straight line extrapolation
    Double_t delz = finalZ - zPos;
    state[kU] = swimstate[kU] + swimstate[kdUdZ]*delz;
    state[kV] = swimstate[kV] + swimstate[kdVdZ]*delz;
    state[kdUdZ] = swimstate[kdUdZ];
    state[kdVdZ] = swimstate[kdVdZ];
    state[kQoverP] = swimstate[kQoverP];
    return 1;
  }

  double pz;
    // forward in time: idir=0; backward in time: idir = 1
    // Momentum is always forward in time
  if((finalZ>zPos && idir==0) || (finalZ<zPos && idir==1))
    pz = 1.0/(TMath::Sqrt(1+swimstate[kdUdZ]*swimstate[kdUdZ]
                          +swimstate[kdVdZ]*swimstate[kdVdZ])
              *TMath::Abs(swimstate[kQoverP])); 
  
  else if((finalZ<zPos && idir==0) || (finalZ>zPos && idir==1))
    pz = -1.0/(TMath::Sqrt(1+swimstate[kdUdZ]*swimstate[kdUdZ]
                           +swimstate[kdVdZ]*swimstate[kdVdZ])
               *TMath::Abs(swimstate[kQoverP]));
  else{
    state[kU] = swimstate[kU];
    state[kV] = swimstate[kV];
    state[kdUdZ] = swimstate[kdUdZ];
    state[kdVdZ] = swimstate[kdVdZ];
    state[kQoverP] = swimstate[kQoverP];
    return 1;
  }

  //the swimmer works in xyz coordinates, not uvz, so need to rotate 
  //the coordinates before putting them into a position vector
  Double_t dxdz = kInvSqrt2*(swimstate[kdUdZ]-swimstate[kdVdZ]);
  Double_t dydz = kInvSqrt2*(swimstate[kdUdZ]+swimstate[kdVdZ]);

  TVector3 momentum(pz*dxdz,pz*dydz,pz);
  SwimParticle muon(position,momentum);
  muon.SetCharge(charge);
  SwimZCondition zc(finalZ);
  bool done;

  if(idir==0) done = s.SwimForward(muon,zc);
  else if(idir==1) done = s.SwimBackward(muon,zc);
  else abort();

  if(muon.GetDirection().Z()==0.0){
    state[kU] = swimstate[kU];
    state[kV] = swimstate[kV];
    state[kdUdZ] = swimstate[kdUdZ];
    state[kdVdZ] = swimstate[kdVdZ];
    state[kQoverP] = swimstate[kQoverP];
    return 0;
  }

  if(muon.GetMomentumModulus()==0.0){
    state[kU] = swimstate[kU];
    state[kV] = swimstate[kV];
    state[kdUdZ] = swimstate[kdUdZ];
    state[kdVdZ] = swimstate[kdVdZ];
    state[kQoverP] = swimstate[kQoverP];
    return 0;
  }

  state[kU] = kInvSqrt2*(muon.GetPosition().X()+muon.GetPosition().Y());
  state[kV] = kInvSqrt2*(muon.GetPosition().Y()-muon.GetPosition().X());
  
  dxdz = muon.GetDirection().X()/muon.GetDirection().Z();
  dydz = muon.GetDirection().Y()/muon.GetDirection().Z();
  state[kdUdZ] = kInvSqrt2*(dxdz+dydz);
  state[kdVdZ] = kInvSqrt2*(dydz-dxdz);
  state[kQoverP] = muon.GetCharge()/muon.GetMomentumModulus();
  //  MSG("FitTrackSR", Msg::kDebug) << "state = " << state[kU] << " " 
  //                              << state[kV] << " " 
  //                              << state[kdUdZ] << " " 
  //                               << state[kdVdZ] << " " 
  //                               << state[kQoverP] << endl;
  if(TMath::Abs(state[kdUdZ])>2.*fParmMaxAngle 
     || TMath::Abs(state[kdVdZ])>2.*fParmMaxAngle 
     || TMath::Abs(state[kQoverP])>fParmMaxQP){
 MSG("FitTrackSR",Msg::kDebug) << "swimming failed, u,v,du/dz,dv,dz/qp = " 
                                  << state[kU] << " " 
                                  << state[kV] << " " 
                                  << state[kdUdZ] << " " 
                                  << state[kdVdZ] << " " 
                                  << state[kQoverP] << endl; 
   return 0;
  }

  return 1;
}


//______________________________________________________________________
void AlgFitTrackSR::Trace(const char * /* c */) const
{
}

//----------------------------------------------------------------------
//method to fill a TObjArray with clusters from the track found in AlgTrackSRList
void AlgFitTrackSR::MakeSliceClusterList(TObjArray *trackClusterList,
                                         const TObjArray *tclist,
                                         CandStripHandleItr &stripItr,
                                         CandFitTrackSRHandle &cfh)
{
 
  stripItr.Reset();
  
  // select track clusters which are in this slice

  Int_t oldplane = 0;
  Int_t minstrip = 0;
  Int_t maxstrip = 0;

  CandStripHandle *strip = 0;
  TrackClusterSR *tc = 0;

  while( (strip = stripItr()) ){
	  MSG("AlgFitTrackSR", Msg::kDebug) << "plane = " << strip->GetPlane() << "/" << strip->GetZPos()
	  << " strip = " << strip->GetStrip() << "/" << strip->GetTPos() 
	  << " charge = " << strip->GetCharge() << endl;
	  
    if(strip->GetPlane()!=oldplane || strip->GetStrip()<minstrip 
       || strip->GetStrip()>maxstrip) {
       
      Bool_t found(0);
      for(int i=0; !found && i<=tclist->GetLast(); ++i){
        tc = dynamic_cast<TrackClusterSR*>(tclist->At(i));
        assert(tc);
        if((found = tc->IsContained(strip))){
          TrackClusterSR *newtc = new TrackClusterSR(*tc);
          newtc->SetIsValid(true);
          trackClusterList->AddLast(newtc);
	  MSG("FitTrackSR",Msg::kDebug) << "Adding trackcluster " 
                                           << tc->GetPlane() << " " 
                                           << tc->GetMinStrip() 
                                           << " " << tc->GetMaxStrip() 
                                           << " is valid " 
                                           << (Int_t)tc->IsValid() 
                                           << " to list" << endl; 
	  cfh.AddTrackCluster(newtc);
          oldplane = tc->GetPlane();
          minstrip = tc->GetMinStrip();
          maxstrip = tc->GetMaxStrip();
        }//end if this cluster contains the current strip
      }//end loop over clusters
    }//end if this strip is in a new cluster
  }//end loop over strips in slice

  return;
}

//-----------------------------------------------------------------
//this method makes a list of track cluster objects just using the 
//strips in the current slice.  i could have made it such that stripItr
//only contained strips from one view or the other, but as that functionality
//isnt needed elsewhere, i decided not to do so.
void AlgFitTrackSR::MakeTrackClusterList(TObjArray *trackClusterList,
                                         CandStripHandleItr &stripItr,
                                         CandFitTrackSRHandle &cfh,
                                         Int_t direction)
{
 
  MSG("AlgFitTrackSR", Msg::kDebug) << "in MakeTrackClusterList" << endl;
  stripItr.Reset();

  // create track clusters
  Int_t oldplane=0;
  Int_t oldstrip=0;
  Double_t oldtime=0.;
  TrackClusterSR *tcluster=0;
  CandStripHandle *strip = 0;

  while( (strip = stripItr()) ){
    if(!strip->GetDemuxVetoFlag() 
       && (strip->GetPlaneView()==PlaneView::kU 
           || strip->GetPlaneView()==PlaneView::kV)){
        
		MSG("AlgFitTrackSR", Msg::kDebug) << "plane = " << strip->GetPlane() << "/" << strip->GetZPos()
		<< " strip = " << strip->GetStrip() << "/" << strip->GetTPos() 
		<< " charge = " << strip->GetCharge() << endl;
		
      if(tcluster && strip->GetPlane()==oldplane 
         && direction*strip->GetStrip()<=direction*oldstrip+3){
        tcluster->AddStrip(strip);
        oldstrip = strip->GetStrip();
        oldtime = strip->GetBegTime();
      }
      else{
        TrackClusterSR *trackcluster = new TrackClusterSR(strip,
                                                fParmMisalignmentError);
        trackClusterList->AddLast(trackcluster);
        oldplane = strip->GetPlane();
        oldstrip = strip->GetStrip();
        oldtime = strip->GetBegTime();
        tcluster = trackcluster;
      }//end if this is a new cluster
    }//end if strip is in the detector and not vetoed by DeMux 
  }//end loop over strips 
        
 
  for(Int_t i=0; i<=trackClusterList->GetLast(); ++i){
   tcluster = dynamic_cast<TrackClusterSR*>(trackClusterList->At(i));

   //get rid of clusters that are too wide or too little charge
   if(tcluster->GetNStrip()>fParmMaxClusterNStrip 
      || tcluster->GetCharge()<fParmMinClusterCharge){
     trackClusterList->RemoveAt(i);
     delete tcluster;
   }
   else{
  MSG("AlgFitTrackSR",Msg::kDebug) << "Adding trackcluster " 
                                      << tcluster->GetPlane() 
                                      << " " << tcluster->GetMinStrip() << " " 
                                      << tcluster->GetMaxStrip() << " is valid " 
                                      << (Int_t)tcluster->IsValid() 
                                      << "  to list" << endl;
     cfh.AddTrackCluster(tcluster);
   }   
  }//end loop over clusters to remove the bad ones

  trackClusterList->Compress();

  return;
}

//-----------------------------------------------------------------------
//this method sets the initial track parameters based on the passed in track
void AlgFitTrackSR::SetTrackParameters(const CandTrackHandle *track0, 
                                       CandFitTrackSRHandle &cfh,
                                       Int_t direction)
{

  cfh.SetCandSlice(track0->GetCandSlice());
  cfh.SetDirCosU(track0->GetDirCosU());
  cfh.SetDirCosV(track0->GetDirCosV());
  cfh.SetDirCosZ(track0->GetDirCosZ());
  cfh.SetVtxU(track0->GetVtxU());
  cfh.SetVtxV(track0->GetVtxV());
  cfh.SetVtxZ(track0->GetVtxZ());
  cfh.SetVtxT(track0->GetVtxT());
  cfh.SetVtxPlane(track0->GetVtxPlane());
  cfh.SetEndDirCosU(track0->GetEndDirCosU());
  cfh.SetEndDirCosV(track0->GetEndDirCosV());
  cfh.SetEndDirCosZ(track0->GetEndDirCosZ());
  cfh.SetEndU(track0->GetEndU());
  cfh.SetEndV(track0->GetEndV());
  cfh.SetEndZ(track0->GetEndZ());
  cfh.SetEndT(track0->GetEndT());
  cfh.SetEndPlane(track0->GetEndPlane());
  cfh.SetTimeSlope(track0->GetTimeSlope());
  cfh.SetTimeOffset(track0->GetTimeOffset());
  cfh.SetMomentumRange(track0->GetMomentum());
  Double_t initQP=0.0;
  if(track0->GetMomentum()!=0 && !fParmIsCosmic) initQP=-1./track0->GetMomentum();
  cfh.SetInitialQP(initQP);
  if(!fParmIsCosmic) cfh.SetEndQP(fParmMaxQP);
  // set ds, range
  for(Int_t iplane = cfh.GetVtxPlane(); iplane*direction<=cfh.GetEndPlane()*direction; 
      iplane+=direction){
    if(track0->IsTPosValid(iplane)){
      // u and v coordinates have been calculated for this plane
 //loop over the clusters in the track
      cfh.SetTrackPointError(iplane,track0->GetTrackPointError(iplane));
      cfh.SetdS(iplane,track0->GetdS(iplane));
      cfh.SetRange(iplane,track0->GetRange(iplane));
    }
  }

  //set stuff if the track was of the SR variety
  const CandTrackSRHandle *tracksr = 0;
  if (track0->InheritsFrom("CandTrackSRHandle")) {
    tracksr = dynamic_cast<const CandTrackSRHandle*>(track0);
    cfh.SetNTrackStrip(tracksr->GetNTrackStrip());
    cfh.SetNTrackDigit(tracksr->GetNTrackDigit());
    cfh.SetNTimeFitDigit(tracksr->GetNTimeFitDigit());
    cfh.SetTimeFitChi2(tracksr->GetTimeFitChi2());
  }
  cfh.SetNIterate(-1);

  return;
}

//-------------------------------------------------------------------
//this method finds the number of active planes skipped between hit planes
//in the event.
Int_t AlgFitTrackSR::FindNumSkippedPlanes(Int_t currentPlane, Int_t prevPlane,
                                          KalmanPlaneSR *oldkp, 
                                          Int_t direction)
{

  UgliGeomHandle ugh(fVldContext);

  // need to check how many active planes were actually skipped
  Int_t nplaneskip=0;
  
  PlexPlaneId plnid(fDetector,prevPlane,false);
  PlexPlaneId plnidend(fDetector,currentPlane,false);
  plnid = plnid.GetAdjoinScint(direction);
  
  // setup kalmanplane to do swimming
  Double_t currentstate[5] = {oldkp->GetFilStateValue(kU,0),
                              oldkp->GetFilStateValue(kV,0),
                              oldkp->GetFilStateValue(kdUdZ,0),
                              oldkp->GetFilStateValue(kdVdZ,0),
                              oldkp->GetFilStateValue(kQoverP,0)};
  Double_t newstate[] = {0.,0.,0.,0.,0.};
  
  Double_t currentz = oldkp->GetTrackCluster()->GetZPos();
  
  bool isvalid = true;
  bool swimflag = true;
  
  while(isvalid && plnid != plnidend && nplaneskip<2*fParmNSkipActive){
    
    if(plnid.IsValid() && plnid.GetPlaneCoverage()!=PlaneCoverage::kNoActive){
      
      UgliScintPlnHandle scintpln = ugh.GetScintPlnHandle(plnid);
      if(scintpln.IsValid()){
        
        // plane is active, check if in coil
        swimflag = Swim(currentstate,newstate,currentz,scintpln.GetZ0(),0,
                            &fVldContext);
        
        if(!swimflag || TMath::Abs(newstate[kU])>10. || TMath::Abs(newstate[kV])>10.) {

          // if failed swim, set nplaneskip to very large number
          nplaneskip = 9999999;
          isvalid = 0;
        } 
        else{
          currentz = scintpln.GetZ0();
          currentstate[kU] = newstate[kU];
          currentstate[kV] = newstate[kV];
          currentstate[kdUdZ] = newstate[kdUdZ];
          currentstate[kdVdZ] = newstate[kdVdZ];
          currentstate[kQoverP] = newstate[kQoverP];
        }

        // check if not in coil (defined to have radius 30 cm)
        if(currentstate[kU]*currentstate[kU]+currentstate[kV]*currentstate[kV]>0.09) {
          ++nplaneskip;
        
        }


      }//end if scintillator plane is valid
    }  
    plnid = plnid.GetAdjoinScint(direction);
  }//end loop over planes
  return nplaneskip;
}

//----------------------------------------------------------------------
//this method marks those clusters to use in a track by setting the IsValid
//flag of the cluster to true. it returns a false if ther are fewewr than 
//3 hits in each view
Bool_t AlgFitTrackSR::MarkTrackClusters(const CandTrackHandle *track0,
                                        TObjArray &planeClusterList, 
                                        CandStripHandleItr &stripItr,
                                        CandFitTrackSRHandle &cfh, 
                                        Int_t &begPlane, Int_t &endPlane)
{

  bool goodTrack = true;

  Int_t oldplane=0;

  stripItr.Reset();

// add trackclusters that are part of track
  Int_t minstrip = 0;
  Int_t maxstrip = 0;
  map<TrackClusterSR*,Bool_t> isintrack;
  
  Int_t inTrackPlane[1000];
  for(Int_t i = 0; i<1000; ++i){
    inTrackPlane[i] = 0;
  }

  Int_t ntrku = 0;
  Int_t ntrkv = 0;

  Double_t oldu,oldv;
  Int_t prevplane=-1;
  begPlane = -1;
  endPlane = -1;
  Bool_t found = false;
  TrackClusterSR *tc = 0;
  CandStripHandle *strip = 0;
  CandStripHandle *tcstrip = 0;

  //set all the track clusters in the iterator to IsValid = false
  ResetTrackClusterList(planeClusterList);

  //loop over strips to find the clusters in the track and mark them as valid
  while( (strip = stripItr()) ){
    // check if strip outside detector
    if(prevplane!=strip->GetPlane()){
      MSG("FitTrackSR", Msg::kDebug) << "prev plane = " << prevplane 
                                   << " strip plane = "
                                    << strip->GetPlane() << " strip " 
                                    << strip->GetStrip()
                                    << endl;

     prevplane = strip->GetPlane();
     oldu = track0->GetU(prevplane);
     oldv = track0->GetV(prevplane);
     MSG("FitTrackSR",Msg::kDebug) << "PLANE " << prevplane 
                                   << " u,v = " << oldu << " " << oldv << endl; 
    }
    //if the current track point is out of the detector, ignore it
    //B.J.R. 3.24.2005: test the impact parameter for the point to make the call
    if(TMath::Sqrt(oldu*oldu + oldv*oldv) > fParmMaxImpactParameter){
      MSG("FitTrackSR",Msg::kDebug) << "PLANE " << prevplane 
                                    << " out of detector, u,v = " 
                                    << oldu << " " << oldv 
				    << " REMOVING from fittrack input" << endl;  
      continue;
    }

    //make sure you only take one cluster from each plane
    if(inTrackPlane[strip->GetPlane()]==0 
       && (strip->GetPlane()!=oldplane || strip->GetStrip()<minstrip 
           || strip->GetStrip()>maxstrip)){
      found = false;
      
      //loop over the clusters until you find the one containing the 
      //current strip

      TObjArray * planeClusters=(TObjArray *)planeClusterList.At(strip->GetPlane());
      TIter clusterItr(planeClusters);
      while(!found && (tc = (TrackClusterSR *)(clusterItr.Next()) )){
	if( tc->GetPlane()==strip->GetPlane()){
	  
	  MSG("AlgFitTrackSR", Msg::kDebug) << "cluster on plane " << tc->GetPlane()
					 << " min strip " << tc->GetMinStrip() 
                                       << " max strip " << tc->GetMaxStrip() 
                                       << " is valid " << (Int_t)tc->IsValid()
                                       << endl; 	  //if this strip is in this cluster
	  if(tc->IsContained(strip)){
	    found = true;	    
	    //the cluster is good so set the plane flag to 1 and the cluster
	    //valid flag to true
	    inTrackPlane[strip->GetPlane()] = 1;
	    tc->SetIsValid(true);
	    MSG("AlgFitTrackSR",Msg::kDebug) << "Adding trackcluster " 
					     << tc->GetPlane() 
					     << " " << tc->GetMinStrip() << " " 
					     << tc->GetMaxStrip() << " is valid " 
					     << (Int_t)tc->IsValid() 
					     << " to track list" 
					     << endl;
	    //see what view the cluster is in and if it is a good beg plane
	    if(tc->GetPlaneView()==PlaneView::kU) ++ntrku;
	    else if(tc->GetPlaneView()==PlaneView::kV) ++ntrkv;
	    if(begPlane<0) begPlane = strip->GetPlane();
	    endPlane = strip->GetPlane();

	    //loop over all strips in the cluster and add them to the CandFitTrack
	    for(int j=0; j<=tc->GetStripList()->GetLast(); ++j){
	      tcstrip = dynamic_cast<CandStripHandle*>(tc->GetStripList()->At(j));
	      cfh.SetInShower(tcstrip,track0->IsInShower(strip));
	    }
	    oldplane = tc->GetPlane();
	    minstrip = tc->GetMinStrip();
	    maxstrip = tc->GetMaxStrip();
	  }//end if this strip is in the current cluster
	}//end loop over clusters
      }

           
    }//end if strip is from a different plane/cluster
  }//end loop over strips

  stripItr.Reset();
  MSG("FitTrackSR",Msg::kDebug) << "# of u,v planes in track = " << ntrku << " " 
                                << ntrkv << endl;
 
 if(ntrku<3 || ntrkv<3){
    goodTrack = false;
    return goodTrack;
  } 

  return goodTrack;
}

//---------------------------------------------------------------
//this method sets the end point parameters for the fit track
void AlgFitTrackSR::SetTrackEndParameters(Int_t begPlane, Int_t endPlane,
                                          CandFitTrackSRHandle &cfh,
                                          const CandTrackHandle *track0)
{
  cfh.SetVtxPlane(begPlane);
  cfh.SetVtxZ(track0->GetZ(begPlane));

  if(begPlane!=cfh.GetVtxPlane() && track0->IsTPosValid(begPlane)) {
    cfh.SetVtxU(track0->GetU(begPlane));
    cfh.SetVtxV(track0->GetV(begPlane));
   
    cfh.SetVtxT(track0->GetT(begPlane));
    if(track0->InheritsFrom("CandTrackSRHandle")){
      // CandTrackSR calculates angles at each plane, use those
      cfh.SetDirCosU(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosU(begPlane));
      cfh.SetDirCosV(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosV(begPlane));
      cfh.SetDirCosZ(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosZ(begPlane));
    }
  }

  cfh.SetEndPlane(endPlane);
  cfh.SetEndZ(track0->GetZ(endPlane));
  if(endPlane!=cfh.GetEndPlane() && track0->IsTPosValid(endPlane)){
    cfh.SetEndU(track0->GetU(endPlane));
    cfh.SetEndV(track0->GetV(endPlane));
    cfh.SetEndT(track0->GetT(endPlane));
    if(track0->InheritsFrom("CandTrackSRHandle")){
      // CandTrackSR calculates angles at each plane, use those
      cfh.SetEndDirCosU(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosU(endPlane));
      cfh.SetEndDirCosV(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosV(endPlane));
      cfh.SetEndDirCosZ(dynamic_cast<const CandTrackSRHandle *>(track0)->GetDirCosZ(endPlane));
    }
  }
  return;
}

//--------------------------------------------------------------------
//method to reset the IsValid flag for all the clusters in a list to
//false
void AlgFitTrackSR::ResetTrackClusterList(TObjArray &planeClusterList)
{
  //  clusterItr.Reset(); 
  TrackClusterSR *tc = 0;
  //loop over all the track clusters and set their valid flags to false
  TIter planeItr(&planeClusterList);
  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (tc = (TrackClusterSR *)clusterItr.Next()) ){
      tc->SetIsValid(false);
      MSG("FitTrackSR", Msg::kDebug) << "cluster on plane " << tc->GetPlane()
				     << " is valid " << (Int_t)tc->IsValid()
				     << endl;
    }
  }
  //  clusterItr.Reset();
  return;
}

//--------------------------------------------------------------------
//method to reset the IsValid flag for all the clusters in a list to
//false except for those included in the fit
void AlgFitTrackSR::ResetTrackClusterList(TObjArray &planeClusterList,
                                          CandFitTrackSRHandle &cfh)
{
  //  clusterItr.Reset(); 
  TrackClusterSR *tc = 0;
  KalmanPlaneSR *kp = 0;

  //loop over the KalmanPlaneSR objects in the fit - if the cluster
  //for a given KalmanPlaneSR is the same as the current cluster,
  //set IsValid = true
  for(Int_t i = 0; i<=cfh.GetKalmanLast(); ++i){
    kp = cfh.GetKalmanPlane(i);
    
    TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kp->GetTrackCluster()->GetPlane());
    TIter clusterItr(planeClusters);   
    //loop over all the track clusters and set their valid flags to false
    while( (tc = (TrackClusterSR * )clusterItr.Next()) ){
      if(tc->GetPlane()==kp->GetTrackCluster()->GetPlane()){
	tc->SetIsValid(false);	
	if(tc->GetMinStrip() == kp->GetTrackCluster()->GetMinStrip()
	   && tc->GetMaxStrip() == kp->GetTrackCluster()->GetMaxStrip() )
	  tc->SetIsValid(true);
      }
    }
  }//end loop over KalmanPlaneSR's
  return;
}

//-------------------------------------------------------------------------
//method that handles the actual track fitting
Int_t AlgFitTrackSR::DoKalmanFit(TObjArray &planeClusterList, 
                                 CandFitTrackSRHandle &cfh,
                                 Int_t &istatus, Int_t direction, Int_t dosearch)
{
  Int_t nadd=1;
  Int_t iadd=0;
  istatus=0;

  Int_t nbadfit = 1;
  Int_t ibadfit = 0;

  Int_t nu = 0;
  Int_t nv = 0;

  TrackClusterSR *tc = 0;

  //make maps of fit planes in each view.  map the plane number
  //to an int - 0 for a bad fit and 1 for a good fit
  map<Int_t, Int_t> uFitPlanes;
  map<Int_t, Int_t> vFitPlanes;
  
  //loop over the clusters in the slice to fill the uFitPlanes and vFitPlanes
  //maps.  initialize all plane values to zero, and reset them in the for
  //loop below if the fit was good.  this way you can see how many hit planes
  //are being skipped by dropping poorly fit planes - otherwise you might
  //just keep removing planes but forgetting that you had the ones you dropped
  TIter planeItr(&planeClusterList);
  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (tc = (TrackClusterSR *)clusterItr.Next()) ){
      if(tc->GetPlaneView() == PlaneView::kU) uFitPlanes[tc->GetPlane()] = 0;
      else if(tc->GetPlaneView() == PlaneView::kV) vFitPlanes[tc->GetPlane()] = 0;
    }
  }


  //loop as long as the fit flag is 0, you have added some points to the 
  //track, and you havent gone over the max number of iterations
  while(!istatus && nadd && iadd<=fParmMaxIterate2){
    
    nbadfit=1;
    ibadfit=0;

    //loop as long as the fit flag is 0 and you have points to remove from the
    //track
    while(!istatus && nbadfit){
      
      //reset the number of bad points in the fit to zero for this loop
      nbadfit = 0;
      
      //iterate the Kalman fit forward and backwards until the fit q/p 
      //converges
      
      istatus = IterateKalmanFit(planeClusterList, cfh, nu, nv, direction, dosearch);
      
      //if all systems are still go, look for and remove any bad points in the 
      //track
      
      if (!istatus){
	MSG("FitTrackSR",Msg::kDebug) << "Removing iteration " << iadd << "/" 
				      << ibadfit << endl;
	
	nbadfit = RemoveBadPointsFromFit(cfh, planeClusterList, uFitPlanes, vFitPlanes,nu, nv, direction);
	
      }//end if tracking worked
      
      ++ibadfit;
    }//end loop while there are still bad points or tracking
    
    //now look for planes to add to the fit
    //reset the counter for the number of added points to the track
    nadd = 0;
    
    //make sure the fit is still ok
    if(!istatus && dosearch){
      
      cfh.ClearKalmanPlaneList();
      
      //add the remaining clusters after removal of bad points to the fit.  
      //use 999 as the value for the third parameter
      //to ensure that we just scale the previous covariance matrix
      nu = 0; 
      nv = 0;
      
      AddClustersToFit(planeClusterList, cfh, 999, nu, nv, direction);
      //make sure you have enough planes in both view to procede
      if(nu>=3 && nv>=3){
	
	// find additional planes downstream
	nadd += FindDownstreamPlanes(planeClusterList, cfh, direction);
	
	//reinitialize the fit and use 
	// 999 to ensure we simply scale previous covariance matrix
	InitializeFit(cfh,1,999); 
	
	//make an array to hold all the new kp's you find
	TObjArray *newkplist = new TObjArray(1,0);
	
	nadd += FindUpstreamPlanes(planeClusterList, cfh, newkplist, direction);
	
	//reset the track clusters in the clusterItr for the next iteration
	ResetTrackClusterList(planeClusterList);
	MSG("FitTrackSR",Msg::kDebug) << "Last iteration " << iadd << "/" << ibadfit 
				      << endl;
	//clear the cfh Kalman Plane list - those objects are all in the
	//newkplist and will get deleted once the clusters have all their 
	//IsValid flags reset
	
	cfh.ClearKalmanPlaneList(0); 
	const KalmanPlaneSR *currentkp = 0;
	Int_t sizeoflist = newkplist->GetLast();
	
	//loop over the newkplist in reverse to mark the clusters used in the fit
	KalmanPlaneSR *kp = 0;
	for(Int_t i=sizeoflist; i>=0; --i){
	  kp = dynamic_cast<KalmanPlaneSR*>(newkplist->At(i));
	  assert(kp);
	  
	  if(!currentkp) currentkp = cfh.AddKalmanPlaneAt(kp,sizeoflist-i);
	  else cfh.AddKalmanPlaneAt(kp,sizeoflist-i);
	 
	  TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kp->GetTrackCluster()->GetPlane());
	  TIter clusterItr(planeClusters);
	  while((tc = (TrackClusterSR *)(clusterItr.Next()) )){
	    if(tc->GetPlane()==kp->GetTrackCluster()->GetPlane()){
	      if(tc->GetMinStrip() == kp->GetTrackCluster()->GetMinStrip()
		 && tc->GetMaxStrip() == kp->GetTrackCluster()->GetMaxStrip() )
		tc->SetIsValid(true);
	    }
  	  }
	  MSG("FitTrackSR",Msg::kDebug) << "  TC " << kp->GetTrackCluster()->GetPlane() 
					  << " " << kp->GetTrackCluster()->GetMinStrip() 
					<< " " << kp->GetTrackCluster()->GetMaxStrip() 
					<< " " << kp->GetZDir() 
					<< endl;
	  delete kp;
	  
	}//end loop over new kplist
	
	MSG("FitTrackSR", Msg::kDebug) << "vertex direction = " 
				       << currentkp->GetZDir()
				       << endl;
	//set the vertex to the first kp
	cfh.SetVtx(currentkp);
	
	newkplist->Clear();
	delete newkplist;
	
      }//end if enough planes hit to start looking for more to add
    }//end if fit is still ok after iteration ste
    ++iadd;  
  }//end while !iadd
  
  //the iadd-1 is so that we can just put the returned value right into
  //cfh.SetNIterate()
  return iadd-1;
}

//-----------------------------------------------------------------------
//this method adds track clusters to the CandFitTrackSRHandle object
Int_t AlgFitTrackSR::AddClustersToFit(TObjArray &planeClusterList,
				      CandFitTrackSRHandle &cfh,
				      Int_t iterate, Int_t &nu, Int_t &nv, Int_t direction)
{
  MSG("FitTrackSR", Msg::kDebug) << "in AddClustersToFit" << endl;
  
  TrackClusterSR *tc = 0;
  Int_t istatus = 0;

  Bool_t dir = kIterForward;
  if(direction!=1) dir=kIterBackward;
  TIter planeItr(&planeClusterList,dir);

  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (tc = (TrackClusterSR *)clusterItr.Next()) && istatus>=0 ){
    
      MSG("FitTrackSR", Msg::kDebug) << "current cluster on plane " 
				     << tc->GetPlane() << " is valid "
				     << (Int_t)tc->IsValid() << endl;
      if(tc->IsValid()){
	MSG("FitTrackSR", Msg::kDebug) << "adding cluster on plane "
				       << tc->GetPlane() << " " 
				       << tc->GetMinStrip() << "-" << tc->GetMaxStrip()
				       << " to fit" << endl;
	
	istatus = AddToFit(cfh,tc, iterate);
	
	if(istatus>=0){
	  if(tc->GetPlaneView()==PlaneView::kU) ++nu;
	  else if(tc->GetPlaneView()==PlaneView::kV) ++nv;
	}
      }
    }
  }//end loop over clusters
  return istatus;
}

//---------------------------------------------------------------------
//this method iterates to do the Kalman filtering several times until
//the fit q/p converges
Int_t AlgFitTrackSR::IterateKalmanFit(TObjArray &planeClusterList,
                                      CandFitTrackSRHandle &cfh,
                                      Int_t &nu, Int_t &nv, Int_t direction, Int_t dosearch)
{
  MSG("AlgFitTrackSR", Msg::kDebug) << " in IterateKalmanFit" << endl;

  Int_t iterate=0;
  Double_t qp0=0.;
  Double_t qp1=0.;

  nu = 0;
  nv = 0;

  Int_t istatus = 0;
 
  // iterate until stable
 MSG("FitTrackSR",Msg::kDebug) << "Before iteration " << iterate << "/"
                               << fParmMaxIterate << " changed points " 
                               << cfh.GetNChangedFitPoint() << " qp0 "
                               << qp0 << " qp1 " << qp1 
                               << " istatus " << istatus 
                                << endl;
  Double_t qpDiff = fParmQPDiff+.01;
  while(iterate<fParmMaxIterate && !istatus 
        && (iterate<=1 
            || ((qp1!=0. && TMath::Abs(qpDiff)>fParmQPDiff) 
                && !(qp0==0. && qp1==0.)) 
            || cfh.GetNChangedFitPoint()) 
        ){
    
    //get the fit q/p for the forward fit
    if(iterate) qp0 = cfh.GetKalmanPlane(0)->GetFilStateValue(4,1);
    
    cfh.ClearKalmanPlaneList();
    nu=0;
    nv=0;
  
    //    clock_t dummyt;
    // struct tms t1;
    // struct tms t2;
    // struct tms t3;
    //static double ticksPerSecond = sysconf(_SC_CLK_TCK);
    // dummyt = times(&t1);

    istatus = AddClustersToFit(planeClusterList, cfh, iterate, nu, nv, direction); 
    // dummyt = times(&t2);
    //  cout << " forward fitter time " << (Double_t)(t2.tms_utime+t2.tms_stime-t1.tms_utime-t1.tms_stime)/ticksPerSecond << endl;
    // cout << " --------------- do reverse fit -------------------" << endl;


    //check to see if you have enough planes in both views to reverse the 
    //fit
    if(nu>=3 && nv>=3) istatus = ReverseFit(planeClusterList,cfh,iterate,dosearch);
    // dummyt = times(&t3);
    //  cout << " reverse fitter time " << (Double_t)(t3.tms_utime+t3.tms_stime-t2.tms_utime-t2.tms_stime)/ticksPerSecond << endl;
    //since new track clusters may have been chosen in the ReverseFit method, 
    //reset the cluster list
    ResetTrackClusterList(planeClusterList, cfh);
    
    qp1 = cfh.GetKalmanPlane(0)->GetFilStateValue(4,1);
    if(qp1==0)  qp1 = cfh.GetKalmanPlane(0)->GetFilStateValue(4,0);
    if(qp1 != 0.) qpDiff = 1.-qp0/qp1;
    else qpDiff = fParmQPDiff;
    MSG("FitTrackSR",Msg::kDebug) << "After iteration " << iterate << "/"
                                 << fParmMaxIterate << " changed points " 
                                 << cfh.GetNChangedFitPoint() << " qp0 "
                                 << qp0 << " qp1 " << qp1 
                                 << " istatus " << istatus 
                                 << " qpdiff " << TMath::Abs(qpDiff) 
                                 << "/" << fParmQPDiff << endl;

  
    ++iterate;
  }//end loop of iterations

  return istatus;
}

//----------------------------------------------------------------------
Int_t AlgFitTrackSR::ReverseFit(TObjArray &planeClusterList,
				CandFitTrackSRHandle & cfh,
				Int_t iterate, Bool_t dosearch)
{

  InitializeFit(cfh,1,iterate);
  TObjArray *KalmanPlaneList =cfh.GetPlaneList();
  
  TObjArray newlist;
  newlist.Clear();
  newlist.Compress();
  
  cfh.SetNChangedFitPoint(0);
  
  TObjArray *tclist = cfh.GetTrackClusterList();

  KalmanPlaneSR *kpold = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(KalmanPlaneList->GetLast()));
  KalmanPlaneSR *lastkp = kpold;
  
  Bool_t *kpswimfail = new Bool_t[KalmanPlaneList->GetLast()+1];
  
  for(Int_t i=0 ;i<=KalmanPlaneList->GetLast(); ++i){
    kpswimfail[i] = 0;
  }

  Int_t nu = 0;
  Int_t nv = 0;
  Bool_t didswimfail = false;
  
  Int_t nswimfail = 0;
  Int_t nswimfailnewer = 0;

  KalmanPlaneSR *kpnew = 0;
  KalmanPlaneSR *kpnewer = 0;
  TrackClusterSR *thistc = 0;

  Bool_t ready = true;

  for(Int_t i=KalmanPlaneList->GetLast()-1; i>=0; --i){
    kpnew = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
    MSG("FitTrackSR",Msg::kDebug) << "new TC, plane = " 
				  << kpnew->GetTrackCluster()->GetPlane() 
				  << " tpos = " 
				  << kpnew->GetTrackCluster()->GetTPos() 
				  << " minstrip = " 
				  << kpnew->GetTrackCluster()->GetMinStrip() 
				  << " maxstrip = " 
				  << kpnew->GetTrackCluster()->GetMaxStrip()
                                   << " q/p = " 
				  << kpnew->GetFilStateValue(4,0)
				  << endl;
    
  if(TMath::Abs(kpnew->GetFilStateValue(4,0))<=2.0){
    
    
    MSG("FitTrackSR", Msg::kDebug) << "current kp has sufficient momentum to do "
				   << "reverse fit" << endl;
    pRev++;
    nswimfail = FitFrom(kpold,kpnew,1,iterate);
    
    if(nswimfail<0){
      MSG("FitTrackSR",Msg::kDebug) << "failed swim" << endl;
      return nswimfail;
    }
    
    //if(i==KalmanPlaneList->GetLast()-1){
    if(ready){
      ready = false;
      cfh.SetPlaneChi2(kpold->GetTrackCluster()->GetPlane(),kpold->GetFilChi2(0));
      cfh.SetPlanePreChi2(kpold->GetTrackCluster()->GetPlane(),kpold->GetPreChi2(0));
      cfh.SetPlaneQP(kpold->GetTrackCluster()->GetPlane(),kpold->GetFilStateValue(4,1));
    }
    MSG("FitTrackSR",Msg::kDebug) << "prechi2 = " << kpnew->GetPreChi2(1) 
				  << "   filchi2 = " 
				  << max(kpnew->GetFilChi2(0),kpnew->GetFilChi2(1)) 
				  << endl; 
    
    if(dosearch && iterate<2){
      kpnewer = 0;
      nswimfailnewer = 0;
      
      MSG("FitTrackSR",Msg::kDebug) << "Searching for other track clusters in plane " 
				    << kpnew->GetTrackCluster()->GetPlane() 
				    << " original min,max strips = " 
				    << kpnew->GetTrackCluster()->GetMinStrip() 
				    << " " 
				    << kpnew->GetTrackCluster()->GetMaxStrip() 
				    << endl;             
      //tclist is the list of TrackClusterSR objects for this track
      TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kpnew->GetTrackCluster()->GetPlane());
      TIter clusterItr(planeClusters);
      while((thistc = dynamic_cast<TrackClusterSR *>(clusterItr.Next()) )){
	//is this a good tc and from the same plane but a different
	//cluster than the one currently used in the plane?
	if(!cfh.GetBadFit(thistc)
	   && thistc->GetMinStrip()!=kpnew->GetTrackCluster()->GetMinStrip()){
	  //make a new kp based on the alternate track cluster and set 
	  //its range and z direction
	  KalmanPlaneSR *thiskp = new KalmanPlaneSR(thistc);
	  thiskp->SetRange(cfh.GetRange(thiskp->GetTrackCluster()->GetPlane()));
	  thiskp->SetZDir(kpold->GetZDir());
	  
	  //try fitting from the previous kp to the alternate one in
	  //the reverse direction
	  pRev2++;
	  nswimfailnewer = FitFrom(kpold,thiskp, 1,iterate);
	  MSG("FitTrackSR",Msg::kDebug) << "considering track cluster: "
					<< tclist->GetLast() << " tpos = " 
					<< thistc->GetTPos() 
					<< " minstrip = " 
					<< thistc->GetMinStrip() << "/" 
					<< kpnew->GetTrackCluster()->GetMinStrip() 
					<< "  prechi2 = " 
					<< thiskp->GetPreChi2(1) << "  filchi2 = " 
					<< thiskp->GetFilChi2(1) << endl;         
	  if(nswimfailnewer>=0 
	     && ((kpnewer 
		  && (nswimfail<0 || thiskp->GetPreChi2(1)<kpnewer->GetPreChi2(1))) 
		 || (!kpnewer && thiskp->GetPreChi2(1)<kpnew->GetPreChi2(1)))){
	    MSG("FitTrackSR",Msg::kDebug) << "  better track cluster found\n";
	    
	    if(kpnewer) delete kpnewer;
	    kpnewer = thiskp;
	    nswimfail = nswimfailnewer;
	  }//end if nswimfailnewer>=0 etc 
	  else delete thiskp;
	  
	}//end if !GetBadFit() etc
      }//end loop over track clusters

      //put the better kp in the list
      if(kpnewer){
	KalmanPlaneList->AddAt(kpnewer,i);
	delete kpnew;
	kpnew = kpnewer;
	cfh.SetNChangedFitPoint(cfh.GetNChangedFitPoint()+1);
      }
      
    }//end if dosearch
   
    if(nswimfail<0){
      MSG("FitTrackSR",Msg::kDebug) << "failed swim" << endl;   
      return nswimfail;
    }
    
    if(nswimfail!=0 && nu>=3 && nv>=3){
      kpswimfail[i] = 1;
      didswimfail = 1;
      MSG("FitTrackSR",Msg::kDebug) << "Swim failed, skipping plane" << endl;
    } 
    else{
      //swim didnt fail, set the chi^2 and qp values for the plane
      cfh.SetPlaneChi2(kpnew->GetTrackCluster()->GetPlane(),
		       max(kpnew->GetFilChi2(1), kpnew->GetFilChi2(0)));
      cfh.SetPlanePreChi2(kpnew->GetTrackCluster()->GetPlane(),
			  max(kpnew->GetPreChi2(1), kpnew->GetPreChi2(0)));
      
      cfh.SetPlaneQP(kpnew->GetTrackCluster()->GetPlane(),kpnew->GetFilStateValue(4,1));
      cfh.SetCurrentKalmanPlane(kpnew);
      lastkp = kpnew;
      
      //set the covariance matrix array based on the filtered covariance
      //matrix of the current kp
      for(int i1=0; i1<5; ++i1){
	for(int i2=0; i2<5; ++i2){
	  fCov[i1][i2]=kpnew->GetFilCovarianceMatrixValue(1,i1,i2);
	}
      }
      
      if(kpnew->GetTrackCluster()->GetPlaneView()==PlaneView::kU) ++nu;
      if(kpnew->GetTrackCluster()->GetPlaneView()==PlaneView::kV) ++nv;
      
      kpold = kpnew;
    }//end else for if nswimfail!=0 && nu>=3 && nv>=3
    
    cfh.SetNSwimFail(cfh.GetNSwimFail()+TMath::Abs(nswimfail));
    
  }//end if enough momentum left to do fit
  //  else kpold = kpnew;
  }//end loop over kalman planes in reverse

  // remove any planes which failed swim
  if(didswimfail){
    
    Int_t nlast = KalmanPlaneList->GetLast();
    TObjArray newlist;
    newlist.Clear();
    newlist.Compress();

    KalmanPlaneSR *kp = 0;
    for(Int_t i=0; i<=nlast; ++i){
      kp = dynamic_cast<KalmanPlaneSR*>(KalmanPlaneList->At(i));
      if(kpswimfail[i]) delete kp;
      else{
        newlist.Add(kp);
	MSG("FitTrackSR",Msg::kDebug) << "adding thiskp  plane " 
                                      << kp->GetTrackCluster()->GetPlane() 
                                      << " min/max strip " 
                                      << kp->GetTrackCluster()->GetMinStrip() 
                                      << "/" 
                                      << kp->GetTrackCluster()->GetMaxStrip() 
                                      << " q/p " 
                                      << kp->GetFilStateValue(4,1) << endl;

      }//end else kp didnt fail swim
    }//end loop over kalman planes

    KalmanPlaneList->Clear();
    KalmanPlaneList->Compress();

    nlast = newlist.GetLast();
    MSG("FitTrackSR",Msg::kDebug) << "new KP list" << endl;
    
    for(Int_t i=0; i<=nlast; ++i){
      kp = dynamic_cast<KalmanPlaneSR*>(newlist.At(i));
      KalmanPlaneList->Add(kp);
      MSG("FitTrackSR",Msg::kDebug) << "  plane " 
                                    << kp->GetTrackCluster()->GetPlane() 
                                    << " min/max strip " 
                                    << kp->GetTrackCluster()->GetMinStrip()
                                    << "/" << kp->GetTrackCluster()->GetMaxStrip() 
                                    << " q/p " 
                                    << kp->GetFilStateValue(4,1) << endl;

    }//end loop over kalman planes to add to the list for the track
  }//end if didswimfail

  delete [] kpswimfail;
  
  if(lastkp) cfh.SetVtx(lastkp);
  
  return 0;
}

//-----------------------------------------------------------------------
//this method removes bad points from a fit.  it returns the number of
//points removed.  it takes ints for the number of planes fit in each view
//as well as a KalmanPlaneSR * that will be filled with the vertex kp
Int_t AlgFitTrackSR::RemoveBadPointsFromFit(CandFitTrackSRHandle &cfh,
                                            TObjArray &planeClusterList,
                                            std::map<Int_t,Int_t>& uFitPlanes,
                                            std::map<Int_t,Int_t>& vFitPlanes,
                                            Int_t &nfitu, Int_t &nfitv,
                                            Int_t direction)
{

  MSG("FitTrackSR", Msg::kDebug) << "in RemoveBadPointsFromFit" << endl; 

  TrackClusterSR *tc = 0;
  const KalmanPlaneSR *vtxkp = 0;
  Int_t nremoveitr = 0;
  Int_t nbadfit = 0;
  nfitu = 0;
  nfitv = 0;
  
  Double_t maxlocalchi2 = fParmMaxLocalChi2;
  Double_t maxanglecovariance = fParmMaxAngleCovariance;
  
  // if chi2 has to be increased due to too many points being removed, 
  //deltachi2 > 0
  fParmDeltaChi2 = -1.; 

  // if angle covariance has to be increased due to too many points 
  //being removed, deltacovariance > 0
  fParmDeltaCovariance = -1.; 
  
  Double_t mindchi2=0.;
  Double_t mindcovariance=0.;
  
  Int_t nPlanesSkippedU = 0;
  Int_t nPlanesSkippedV = 0;
  Int_t lastUPlane = -1;
  Int_t lastVPlane = -1;
  Int_t lastFitPlane = -1;

  MSG("FitTrackSR", Msg::kDebug) << "nfit u " << nfitu << " nfitv " << nfitv << endl;
  while(nfitu<3 || nfitv<3){
    
    vtxkp = 0;
    nfitu = 0;
    nfitv = 0;
    nbadfit = 0;
    
    //reset the cluster list
    ResetTrackClusterList(planeClusterList);
    
    mindchi2=0.;
    mindcovariance=0.;
    
    nPlanesSkippedU = 0;
    nPlanesSkippedV = 0;
    
    lastUPlane = -1;
    lastVPlane = -1;
    
    KalmanPlaneSR *kp = 0;
    
    //loop over the Kalman planes to fill the maps
    for(Int_t i=0; i<=cfh.GetKalmanLast(); ++i){
      kp = cfh.GetKalmanPlane(i);
      
      //see if the reset of the clusterList also reset the 
      //clusters in the kp's
      MSG("FitTrackSR", Msg::kDebug) << "cluster for kp on plane "
				     << kp->GetTrackCluster()->GetPlane()
				     << " is valid " 
				     << (Int_t)kp->GetTrackCluster()->IsValid()
				     << endl;
      
      MSG("FitTrackSR",Msg::kDebug) << "  TC " << kp->GetTrackCluster()->GetPlane() 
                                    << " " << kp->GetTrackCluster()->GetMinStrip() 
                                    << " " << kp->GetTrackCluster()->GetMaxStrip() 
                                    << " " << kp->GetFilChi2(0)
                                    << " " << kp->GetFilChi2(1) 
                                    << "/" << maxlocalchi2 << " " 
                                    << TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdUdZ, kdUdZ),
						  kp->GetFilCovarianceMatrixValue(1,kdUdZ, kdUdZ))
                                    << " " 
                                    << TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdVdZ, kdVdZ),
						  kp->GetFilCovarianceMatrixValue(1,kdVdZ, kdVdZ)) 
                                    << endl;
      
      //see if the fit for this plane was really good
      if(TMath::Max(kp->GetFilChi2(0),kp->GetFilChi2(1))<=maxlocalchi2 
         && TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdUdZ,kdUdZ),
		       kp->GetFilCovarianceMatrixValue(1,kdUdZ,kdUdZ))<maxanglecovariance 
         && TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdVdZ,kdVdZ),
		       kp->GetFilCovarianceMatrixValue(1,kdVdZ,kdVdZ))<maxanglecovariance)
        {
	  
	  MSG("FitTrackSR", Msg::kDebug) << "  this fit was good" << endl;
	  
	  kp->GetTrackCluster()->SetIsValid(true);
	  
	  MSG("FitTrackSR", Msg::kDebug) << "  plane " << kp->GetTrackCluster()->GetPlane()
					 << " IsValid " << kp->GetTrackCluster()->IsValid()
					 << endl;  
	  TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kp->GetTrackCluster()->GetPlane());
	  TIter clusterItr(planeClusters);
	  while((tc = (TrackClusterSR *)(clusterItr.Next()) )){
	    if(tc->GetPlane()==kp->GetTrackCluster()->GetPlane()){  // replacing slice
	      if(tc->GetMinStrip() == kp->GetTrackCluster()->GetMinStrip()
		 && tc->GetMaxStrip() == kp->GetTrackCluster()->GetMaxStrip() )
		tc->SetIsValid(true);
	    }
	  }
	  
	  if(kp->GetTrackCluster()->GetPlaneView() == PlaneView::kU)
	    uFitPlanes[kp->GetTrackCluster()->GetPlane()] = 1;
	  
	  else if(kp->GetTrackCluster()->GetPlaneView() == PlaneView::kV)
	    vFitPlanes[kp->GetTrackCluster()->GetPlane()] = 1;
	  
	}//end if good fit for kp
      else{
	
        MSG("FitTrackSR", Msg::kDebug) << "  plane " << kp->GetTrackCluster()->GetPlane()
				       << " IsValid " 
				       << (Int_t)kp->GetTrackCluster()->IsValid()
				       << endl;
	
        //no need to set the fTrackCluster to IsValid=false, as the 
        //reset call above did that.
        if(kp->GetTrackCluster()->GetPlaneView() == PlaneView::kU)
          uFitPlanes[kp->GetTrackCluster()->GetPlane()] = 0;
        else if(kp->GetTrackCluster()->GetPlaneView() == PlaneView::kV)
          vFitPlanes[kp->GetTrackCluster()->GetPlane()] = 0;
      }
      
    }//end loop to fill map of fit quality for kp's
    
    lastFitPlane = cfh.GetKalmanPlane(cfh.GetKalmanLast())->GetTrackCluster()->GetPlane();
    map<Int_t, Int_t>::iterator vfitIter = vFitPlanes.begin();
    map<Int_t, Int_t>::iterator ufitIter = uFitPlanes.begin();
    for(Int_t i=0; i<=cfh.GetKalmanLast(); ++i){
      kp = cfh.GetKalmanPlane(i);
      
      //look to see which view the kp comes from, and if it has a 
      //good fit, as found above
      if(kp->GetTrackCluster()->GetPlaneView()==PlaneView::kU){
        if(uFitPlanes[kp->GetTrackCluster()->GetPlane()]==1){
          ++nfitu;
          lastUPlane = kp->GetTrackCluster()->GetPlane();
          if(!vtxkp) vtxkp = kp;
        }
        else{
          //see how many planes were skipped in this view
          nPlanesSkippedU = FindNumSkippedPlanesInView(uFitPlanes,
						       ufitIter, 
						       kp->GetTrackCluster()->GetPlane(), 
						       lastUPlane,
						       lastFitPlane, direction);
          if(nPlanesSkippedU>=fParmNSkipView){
	    
            //dont remove this plane as there would be too many skipped planes
            //in the view if we did
	    
	    TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kp->GetTrackCluster()->GetPlane());
	    TIter clusterItr(planeClusters);
	    while((tc = (TrackClusterSR * )(clusterItr.Next()) )){
	      if(tc->GetPlane()==kp->GetTrackCluster()->GetPlane()){
		if(tc->GetMinStrip() == kp->GetTrackCluster()->GetMinStrip()
		   && tc->GetMaxStrip() == kp->GetTrackCluster()->GetMaxStrip() )
		  tc->SetIsValid(true);
	      }
	    }
	    MSG("FitTrackSR",Msg::kDebug) << "too many consecutive planes  on plane " 
					  << kp->GetTrackCluster()->GetPlane()<<  " # skipped  " << nPlanesSkippedU <<"/" << fParmNSkipView 
					  << "   keeping plane" 
					  << endl;
	    
            if (!vtxkp) vtxkp = kp;
            lastUPlane = kp->GetTrackCluster()->GetPlane();
            ++nfitu;	  
          }
          else{
            MSG("FitTrackSR",Msg::kDebug) << "REMOVING, plane " 
					  << kp->GetTrackCluster()->GetPlane() 
					  << " nskipped = " << nPlanesSkippedU 
					  << "/" << fParmNSkipView << endl;
	    
            ++nbadfit;
	    if(TMath::Max(kp->GetFilChi2(0),kp->GetFilChi2(1))>maxlocalchi2){
              if(mindchi2<=0. 
                 || TMath::Max(kp->GetFilChi2(0),kp->GetFilChi2(1))-maxlocalchi2<mindchi2){
                mindchi2 = TMath::Max(kp->GetFilChi2(0),kp->GetFilChi2(1))-maxlocalchi2;
                mindchi2 += 0.001; // precision
              }//end if the max chi^2 is bigger than allowed value
            } 
            else{
              Double_t thismaxcov = TMath::Max(TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdUdZ,kdUdZ),
							  kp->GetFilCovarianceMatrixValue(1,kdUdZ,kdUdZ)),
					       TMath::Min(kp->GetFilCovarianceMatrixValue(0,kdVdZ,kdVdZ),
							  kp->GetFilCovarianceMatrixValue(1,kdVdZ,kdVdZ)));
              if (mindcovariance<=0. 
                  || thismaxcov-maxanglecovariance<mindcovariance) {
                mindcovariance = thismaxcov-maxanglecovariance;
                mindcovariance += 0.001; // precision
              }//end if mindcovariance is too small
            }//end else to check covariance
          }//end else remove the current plane
	  
        }//end else the current kp is a bad fit
      }//end if in U view
      //check the other view
      else if(kp->GetTrackCluster()->GetPlaneView()==PlaneView::kV){
        if(vFitPlanes[kp->GetTrackCluster()->GetPlane()]==1){
	  
	  ++nfitv;
          lastVPlane = kp->GetTrackCluster()->GetPlane();
          if(!vtxkp) vtxkp = kp;
        }
        else{
          //see how many planes were skipped in this view
          nPlanesSkippedV = FindNumSkippedPlanesInView(vFitPlanes,
						       vfitIter, 
						       kp->GetTrackCluster()->GetPlane(), 
						       lastVPlane,
						       lastFitPlane, direction);
          if(nPlanesSkippedV>=fParmNSkipView){
	    
            //dont remove this plane as there would be too many skipped planes
            //in the view if we did
	    //	    clusterItr.GetSet()->Slice(kp->GetTrackCluster()->GetPlane());
	    //in the view if we did

	    MSG("FitTrackSR",Msg::kDebug) << "too many consecutive planes  on plane  " 
					  << kp->GetTrackCluster()->GetPlane()<<  " # skipped  " << nPlanesSkippedU <<"/" << fParmNSkipView 
					  << "   keeping plane" 
					  << endl;

	    TObjArray * planeClusters=(TObjArray *)planeClusterList.At(kp->GetTrackCluster()->GetPlane());
	    TIter clusterItr(planeClusters);
	    while((tc = (TrackClusterSR * )(clusterItr.Next()) )){
	      if(tc->GetPlane()==kp->GetTrackCluster()->GetPlane()){
		if(tc->GetMinStrip() == kp->GetTrackCluster()->GetMinStrip()
		   && tc->GetMaxStrip() == kp->GetTrackCluster()->GetMaxStrip() )
		  tc->SetIsValid(true);
	      }
	    }
	    //	    clusterItr.GetSet()->Slice();
	    
	    if (!vtxkp) vtxkp = kp;
 	    lastVPlane = kp->GetTrackCluster()->GetPlane();
	    ++nfitv;
          }
          else{
            MSG("FitTrackSR",Msg::kDebug) << "REMOVING, plane " 
					  << kp->GetTrackCluster()->GetPlane() 
					  << " nskipped = " << nPlanesSkippedV
					  << "/" << fParmNSkipView << endl;
            ++nbadfit;
            if(max(kp->GetFilChi2(0),kp->GetFilChi2(1))>maxlocalchi2){
              if(mindchi2<=0. 
                 || max(kp->GetFilChi2(0),kp->GetFilChi2(1))-maxlocalchi2<mindchi2){
                mindchi2 = max(kp->GetFilChi2(0),kp->GetFilChi2(1))-maxlocalchi2;
                mindchi2 += 0.001; // precision
              }//end if the max chi^2 is bigger than allowed value
            } 
            else{
              Double_t thismaxcov = max(min(kp->GetFilCovarianceMatrixValue(0,kdUdZ,kdUdZ),
					    kp->GetFilCovarianceMatrixValue(1,kdUdZ,kdUdZ)),
					min(kp->GetFilCovarianceMatrixValue(0,kdVdZ,kdVdZ),
					    kp->GetFilCovarianceMatrixValue(1,kdVdZ,kdVdZ)));
              if (mindcovariance<=0. 
                  || thismaxcov-maxanglecovariance<mindcovariance) {
                mindcovariance = thismaxcov-maxanglecovariance;
                mindcovariance += 0.001; // precision
              }//end if mindcovariance is too small
            }//end else to check covariance
          }//end else remove the current plane
	  
        }//end else the current kp is a bad fit
      }//end if in V view
    }//end loop over kp's to remove bad points
    
    if (nfitu<3 || nfitv<3) {      
      // not enough points, increase maximum chi2
      
      maxlocalchi2 += mindchi2;
      maxanglecovariance += mindcovariance;
      
      MSG("FitTrackSR",Msg::kDebug) << "TOO FEW POINTS LEFT, increasing chi2" 
				    << " threshold to " << maxlocalchi2 
				    << " and angle covariance threshold to " 
				    << maxanglecovariance << endl;
      
    }//end if less than 3 planes fit in either view
    fParmDeltaChi2 = mindchi2;
    fParmDeltaCovariance = mindcovariance;
    ++nremoveitr;
    assert(nremoveitr<=cfh.GetKalmanLast()+2);
    // in principle can never loop through more than # of planes in fit
  }//end while less than 3 fit planes in at least one view
  //make sure you found a vertex kp and set the vertex to it.
  assert(vtxkp);
  cfh.SetVtx(vtxkp);
    
  TIter planeItr(&planeClusterList);
  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (tc = (TrackClusterSR * )clusterItr.Next()) ){
      
      MSG("FitTrackSR", Msg::kDebug) << "current cluster on plane " 
				     << tc->GetPlane() << " is valid "
				     << (Int_t)tc->IsValid() << " " 
				     << tc->GetMinStrip() << "-" << tc->GetMaxStrip()
				     << endl;
    }
  }

  return nbadfit;
}

//---------------------------------------------------------------------
//this method looks for planes to add downstream of the last fit plane in the 
//event 
Int_t AlgFitTrackSR::FindDownstreamPlanes(TObjArray &planeClusterList,
                                          CandFitTrackSRHandle &cfh, 
                                          Int_t direction)
{
 

 MSG("FitTrackSR", Msg::kDebug) << "in FindDownstreamPlanes " 
                                 << direction << endl;
  TrackClusterSR *thistc = 0;
  TrackClusterSR *oldtc = 0;
  KalmanPlaneSR *thiskp = 0;
  KalmanPlaneSR *bestkp = 0;
  KalmanPlaneSR *oldkp = cfh.GetKalmanPlane(cfh.GetKalmanLast());
  Int_t nswimfail = 0;
  Int_t nplaneskip = 0;
  Bool_t isvalid = true;
  Int_t istatus2 = 0;
  Int_t nadd = 0;

  Int_t currentPlane = -1;
  Int_t oldKPPlane = oldkp->GetTrackCluster()->GetPlane();

  //loop over the clusters in the cluster list

  //save some time - slice the cluster iterator from the last plane
  //in the fit to the last one in the detector in the appropriate direction
  //neither detector has more than 500 planes, and both start with plane 0 being
  //the front steel (uninstrumented) plane

  Bool_t dir = kIterForward;
  if(direction!=1) dir=kIterBackward;
  TIter planeItr(&planeClusterList,dir);
  TObjArray * plnarray;
  while( (plnarray = (TObjArray * )planeItr.Next()) ){
    TIter clusterItr(plnarray);
    while( (thistc = (TrackClusterSR * )clusterItr.Next()) && isvalid ){
 	  //make sure you are looking at a valid track cluster, if not go on to the next one
	  if( !thistc->IsValid() ) continue; 	     
      currentPlane = thistc->GetPlane();
      
      MSG("FitTrackSR", Msg::kDebug) << "looking at plane " << currentPlane << endl;
      
      //make sure you are looking at a downstream plane
      if(direction*currentPlane>direction*oldKPPlane){
	
	//see if the current track cluster is on a new plane and that
	//you have a potential kp to add to the fit
	if(bestkp && oldtc && oldtc->GetPlane() != currentPlane ){
	  MSG("FitTrackSR", Msg::kDebug ) << "add best kp to fit" << endl;
	  if(AddForwardBestKPToFit(bestkp, cfh, nswimfail)){
	    ++nadd;
	    oldkp = bestkp;
	  }
	  bestkp = 0;
	}
	
	oldKPPlane = oldkp->GetTrackCluster()->GetPlane();
	MSG("FitTrackSR", Msg::kDebug) << " prev plane = " << oldKPPlane 
				       << " direction " << direction << endl;
	
	//on a new cluster, see if it is a good one
	MSG("FitTrackSR",Msg::kDebug) << "considering tc " << thistc->GetPlane() 
				      << " " << thistc->GetMinStrip() << "-" 
				      << thistc->GetMaxStrip() 
				      << " for addition to end" << endl;
	nplaneskip = 0;
	
	//check if the current plane is far from the last kp - need to
	//see how many active planes were skipped if it is
	if(TMath::Abs(currentPlane-oldKPPlane)>=2*fParmNSkipActive)
	  nplaneskip = FindNumSkippedPlanes(currentPlane,oldKPPlane,oldkp, direction);
	
	//make sure you didnt skipp too many active planes
	if(nplaneskip<2*fParmNSkipActive){
	  
	  //make a new kp for this cluster to test it out
	  thiskp = new KalmanPlaneSR(thistc);
	  thiskp->SetRange(cfh.GetRange(thiskp->GetTrackCluster()->GetPlane()));
	  thiskp->SetZDir(direction);
	  istatus2 = FitFrom(oldkp,thiskp,0,-1);
	  MSG("FitTrackSR",Msg::kDebug) << "  chi2 = " << thiskp->GetPreChi2(0)
					<< " direction " << direction
					<< " nswimfail = " << istatus2 << endl;
	  //look to see if the kp you just made is better than the previous
	  //best kp
	  if(!istatus2 && (!bestkp || thiskp->GetPreChi2(0)<bestkp->GetPreChi2(0))){
	    MSG("FitTrackSR",Msg::kDebug) << "best chi2, keeping" << endl;   
	    if (bestkp) delete bestkp;
	    bestkp = thiskp;
	    
	    nswimfail = istatus2;
	  }
	  else delete thiskp;
	  
	  if(istatus2<0) isvalid = 0;
	} 
	else {
	  // any planes beyond this one will not satisfy NSkipView, stop search
	  isvalid = 0;
	}
	MSG("FitTrackSR",Msg::kDebug) << "set oldtc" << endl;
	oldtc = thistc;
      }//end if current plane is down stream of the last fit plane
    }//end loop over cluster list
  }

  //may still have a best kp, so make sure to add it to the fit
  if(bestkp){
    MSG("FitTrackSR", Msg::kDebug) << "add bestkp outside of loop over tc" << endl;
    nadd += AddForwardBestKPToFit(bestkp, cfh, nswimfail);
    
    //delete bestkp so as to not have a memory leek
    bestkp = 0;
  }

  return nadd;
}

//--------------------------------------------------------------------
//this method looks for planes upstream of the first plane in the fit to 
//add to the fit
Int_t AlgFitTrackSR::FindUpstreamPlanes(TObjArray &planeClusterList,
                                        CandFitTrackSRHandle &cfh,
                                        TObjArray *newkplist,
                                        Int_t direction)
{
  MSG("FitTrackSR", Msg::kDebug) << "in FindUpstreamPlanes " 
                                 << direction << endl;
  TrackClusterSR *thistc = 0;
  KalmanPlaneSR *thiskp = 0;
  KalmanPlaneSR *bestkp = 0;
  KalmanPlaneSR *oldkp = cfh.GetKalmanPlane(cfh.GetKalmanLast());
  Int_t nswimfail = 0;
  Int_t nplaneskip = 0;
  Bool_t isvalid(1);
  Int_t istatus2 = 0;
  
  Int_t nadd = 0;

  //loop over the clusters in the cluster list

  // identify planes which already are fit
  map<Int_t,KalmanPlaneSR *> fitplane;

  KalmanPlaneSR *kp = 0;
  for(int i=0; i<=cfh.GetKalmanLast(); ++i){
    kp = cfh.GetKalmanPlane(i);
    MSG("FitTrackSR",Msg::kDebug) << "  plane " << kp->GetTrackCluster()->GetPlane() 
                                  << "  strips " << kp->GetTrackCluster()->GetMinStrip() 
                                  << "-" << kp->GetTrackCluster()->GetMaxStrip() 
                                  << " " << kp->GetZDir() << endl;
    

    fitplane[kp->GetTrackCluster()->GetPlane()] = kp;
  }
  MSG("FitTrackSR",Msg::kDebug) << "After finding end hits" << endl;
 
  Int_t currentPlane = oldkp->GetTrackCluster()->GetPlane();
  Int_t oldKPPlane = oldkp->GetTrackCluster()->GetPlane();

  Int_t prevPlane = -1;
  Int_t firstPlane = cfh.GetKalmanPlane(0)->GetTrackCluster()->GetPlane();
  MSG("FitTrackSR",Msg::kDebug) << "REVERSING FIT, looking for good track clusters" 
                               << endl;

  newkplist->AddLast(oldkp);

  nswimfail = 0;
  isvalid = 1;
  map<Int_t,KalmanPlaneSR *>::iterator planeiter;

  Bool_t dir = kIterBackward;
  if(direction!=1) dir=kIterForward;
  TIter planeItr(&planeClusterList,dir);

  TObjArray * plnarray;
  while(isvalid &&  (plnarray = (TObjArray * )planeItr.Next()) ){

    TIter clusterItr(plnarray);
    thistc = (TrackClusterSR *)clusterItr.Next();
    if( (direction==1 && thistc->GetPlane()<oldKPPlane) ||
	(direction!=1 && thistc->GetPlane()>oldKPPlane)){
      clusterItr.Reset();
      while(isvalid &&  (thistc = (TrackClusterSR *)clusterItr.Next()) ){
	  //if this isnt a valid track cluster, go on to the next one
	  if(!thistc->IsValid()) continue;
	currentPlane = thistc->GetPlane();
	oldKPPlane = oldkp->GetTrackCluster()->GetPlane();

	planeiter = fitplane.find(thistc->GetPlane());
	MSG("FitTrackSR",Msg::kDebug) << "considering trackcluster, plane=" 
				      << thistc->GetPlane() << " strips=" 
				      << thistc->GetMinStrip() << "-" 
				      << thistc->GetMaxStrip() << endl;
	//if you are on a new plane and have a bestkp , try adding it to the fit
	if(bestkp && currentPlane != prevPlane){
	  nadd += AddReverseBestKPToFit(bestkp, oldkp, cfh, newkplist,
					nswimfail, direction);
	  MSG("FitTrackSR" , Msg::kDebug) << "bestkp has direction " 
					  << bestkp->GetZDir() << endl; 
	  bestkp = 0;
	}
          
	//see if this plane contains a fit track cluster
	if(planeiter!=fitplane.end() ){
	  
	  //this plane contains a fit track cluster, so add it to the 
	  //newkplist
	  if(currentPlane != prevPlane ){
	    kp = dynamic_cast<KalmanPlaneSR *>(planeiter->second);
	    MSG("FitTrackSR", Msg::kDebug) << oldkp->GetTrackCluster()->GetPlane() << " "
					   << kp->GetTrackCluster()->GetPlane() << " " 
					   << kp->GetZDir() << endl;
	    
	    nswimfail = FitFrom(oldkp,kp,1,-1);
	    MSG("FitTrackSR", Msg::kDebug) << "adding previously fit plane " 
					   << thistc->GetPlane() << " to list" << " " 
					   << kp->GetZDir() << endl;
	    
	    newkplist->AddLast(kp);
	    MSG("FitTrackSR", Msg::kDebug) << "added" << endl;
	    cfh.SetCurrentKalmanPlane(kp);
	    oldkp = kp;
	  }
	}//end if plane contains a fit track cluster 
	else{   
 
	  MSG("FitTrackSR",Msg::kDebug) << "this plane not fit, considering tc " 
					<< thistc->GetPlane() << " " 
					<< thistc->GetMinStrip() << "-" 
					<< thistc->GetMaxStrip() 
					<< " for addition to end" << endl;
	  // need to check how many active planes were actually skipped
	  // only do check if beyond first fit plane
	  nplaneskip=0;
	  
	  //only worry about checking how many planes were skipped
	  //if the currentPlane is upstream of the first plane
	  // - the gaps are of acceptable size between fit planes already
	  if(currentPlane*direction<firstPlane*direction 
	     && TMath::Abs(currentPlane-oldKPPlane)>=2*fParmNSkipActive)
	    nplaneskip = FindNumSkippedPlanes(currentPlane, oldKPPlane, oldkp, -direction);
	  
	  //not too many planes skipped, give this cluster a whirl
	  if(nplaneskip<2*fParmNSkipActive){
	    thiskp = new KalmanPlaneSR(thistc);
	    thiskp->SetRange(cfh.GetRange(thiskp->GetTrackCluster()->GetPlane()));
	    thiskp->SetZDir(oldkp->GetZDir());
	    MSG("FitTrackSR",Msg::kDebug) << "fitting from track cluster, plane " 
					  << oldkp->GetTrackCluster()->GetPlane() 
					  << " strips " 
					  << oldkp->GetTrackCluster()->GetMinStrip() 
					  << "-" << oldkp->GetTrackCluster()->GetMaxStrip() 
					  << " direction " << direction << "/" 
					  << thiskp->GetZDir()
					  << endl;
	    istatus2 = FitFrom(oldkp,thiskp,1,-1);
	    //if the fit was good and this kp has a better chi^2 than the previous
	    //best one for the plane, make it bestkp
	    MSG("FitTrackSR",Msg::kDebug) << "  chi2 = " << thiskp->GetPreChi2(1) 
					  << " nswimfail = " << istatus2 << endl;
	    
	    if(!istatus2 && (!bestkp || thiskp->GetPreChi2(1)<bestkp->GetPreChi2(1))){
	      if (bestkp) delete bestkp;
	      MSG("FitTrackSR",Msg::kDebug) << "best chi2, keeping" << endl;
	      bestkp = thiskp;
	      nswimfail = istatus2;
	    }
	    else delete thiskp;
	    
	  }//end not too many active planes skipped
	  else{
	    
	    // any planes beyond this one will not satisfy NSkipView, stop search
	    isvalid = 0;
	  }
	}//end current plane does not have a fit clustser
	prevPlane = currentPlane;
      }//end loop in reverse over clusters
    }
  }
  //outside of the loop, but there could still be an extra best kp to add
  if(bestkp){
    nadd += AddReverseBestKPToFit(bestkp, oldkp, cfh, newkplist,
				  nswimfail, direction);
    MSG("FitTrackSR" , Msg::kDebug) << "bestkp has direction " 
                                    << bestkp->GetZDir() << endl;
    bestkp = 0;
  }
  
  return nadd;
}

//---------------------------------------------------------------------
//method to find the number of planes skipped in a view between planes with 
//a good fit
Int_t AlgFitTrackSR::FindNumSkippedPlanesInView(std::map<Int_t,Int_t>& fitPlanes, std::map<Int_t,Int_t>::iterator& fitIter,
                                                Int_t currentPlane, Int_t prevPlane,
                                                Int_t lastPlane, Int_t direction)
{
 
  //assume you drop the current plane, so start off with numSkipped=1
  Int_t numSkipped = 1;
  Int_t plane = 0;

  if(prevPlane<=0) return 0;
  fitIter = fitPlanes.find(currentPlane);
  //only do the check if there was a plane previous to this one
  if(prevPlane>0){
    if(direction == 1) --fitIter;
    else ++fitIter;

    plane = (*fitIter).first;
  
    //loop the iterator in reverse until you find the previously fit plane
    while( plane*direction > prevPlane*direction && numSkipped<fParmNSkipView){
      if(direction == 1) --fitIter;
      else ++fitIter;
      plane = (*fitIter).first;

      //went through the loop, increment the number of skipped planes
  
    ++numSkipped;

      //stop looking once you find a kp with a good fit
      if( (*fitIter).second != 0 ) break;
    }
  }//end if not the first plane in view
  
  //now look downstream
  if(numSkipped<fParmNSkipView){
    
    //didnt skip too many before the current plane, what about after it?
    fitIter = fitPlanes.find(currentPlane);
    if(direction == 1) ++fitIter;
    else --fitIter;

    plane = (*fitIter).first;
    
    while( plane*direction < lastPlane*direction && numSkipped<fParmNSkipView ){
      if(direction == 1) ++fitIter;
      else --fitIter;
      plane = (*fitIter).first;
      
      //stop looking once you find a kp with a good fit
      //the break point is before the count increase in 
      //the forward loop so that you
      //dont double count the current plane as being skipped.  you have
      //set the iterator such that it should be one 
      //plane beyond the current plane
      //and if that plane is a good fit, you didnt skip 2 planes, just one

      if( (*fitIter).second != 0) break;
      
      //went through the loop, increment the number of skipped planes
      ++numSkipped;
      
    }
    
  }//end check for second view
  
  return numSkipped;
}

//----------------------------------------------------------------------
//this method checks to see if the passed kp should be added to the fit
//it returns 1 if the kp is added
Int_t AlgFitTrackSR::AddForwardBestKPToFit(KalmanPlaneSR *bestkp, 
                                           CandFitTrackSRHandle &cfh, 
                                           Int_t &nswimfail)
{  

  MSG("FitTrackSR", Msg::kDebug) << "in AddForwardBestKPToFit" << endl;
  Int_t nadd = 0;
    
  MSG("FitTrackSR",Msg::kDebug) << "considering best tc " 
                                << bestkp->GetTrackCluster()->GetPlane() 
                                << " " << bestkp->GetTrackCluster()->GetMinStrip() 
                                << "-" << bestkp->GetTrackCluster()->GetMaxStrip() 
                                << ", chi2 = " << bestkp->GetPreChi2(0) << "/" 
                                << fParmMaxLocalPreChi2 << " " 
                                << bestkp->GetPreCovarianceMatrixValue(0,kdUdZ,kdUdZ) 
                                << "/"
                                << fParmMaxAngleCovariance << " " 
                                << bestkp->GetPreCovarianceMatrixValue(0,kdVdZ,kdVdZ) 
                                << "/"
                                << fParmMaxAngleCovariance << endl;
                        
  if(bestkp->GetPreChi2(0)<fParmMaxLocalPreChi2 
     && bestkp->GetPreCovarianceMatrixValue(0,kdUdZ,kdUdZ)<fParmMaxAngleCovariance 
     && bestkp->GetPreCovarianceMatrixValue(0,kdVdZ,kdVdZ)<fParmMaxAngleCovariance){
    
    KalmanPlaneSR *kpnewer = cfh.AddKalmanPlaneAt(bestkp,cfh.GetKalmanLast()+1);
    Int_t istatus2 = AddToFit(cfh,kpnewer,-1,0);
    if (istatus2>=0) {
      cfh.SetNSwimFail(cfh.GetNSwimFail()+nswimfail);
      ++nadd;
    } 
    else {
 MSG("FitTrackSR",Msg::kDebug) << "AddToFit failed, do not add to fit" 
                                    << endl;

      // need to remove from KalmanPlaneList
      cfh.RemoveKalmanPlane(-1,0);
      if(bestkp){
        delete bestkp;