KalmanTrack_module.cc

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////
// \file    KalmanTrack_module.cc
// \brief   A Kalman filter based track finding module
// \version $Id: KalmanTrack_module.cc,v 1.45 2012-11-16 16:57:10 raddatz Exp $
// \author  Nick Raddatz
////////////////////////////////////////////////////////////////////////////

#include <string>
#include <vector>
#include <cmath>

// ROOT includes
#include "TVector3.h"

// NOvA includes
#include "GeometryObjects/Geo.h"
#include "GeometryObjects/CellGeo.h"
#include "ChannelInfo/BadChanList.h"
#include "RecoBase/Track.h"
#include "RecoBase/FilterList.h"
#include "TrackFit/LocatedChan.h"
#include "TrackFit/KalmanGeoHelper.h"
#include "Utilities/AssociationUtil.h"
#include "NovaDAQConventions/DAQConventions.h"

// Framework includes
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"

/// Tracking algorithms
namespace trk {
  class KalmanTrack : public art::EDProducer {
  public:
    explicit KalmanTrack(fhicl::ParameterSet const& pset);
    virtual ~KalmanTrack();

    void produce(art::Event& evt);
    void reconfigure(const fhicl::ParameterSet& p);

  private:    
    double                                 fMaxSliceHits;      // Maximum fraction of number of hits active in any one time slice in which a track will be searched for
    double                                 fMaxHitCut;         // Maximum number of hits in slice that the tracker will try to reconstruct 
    int                                    fWhoaNelly;         // Maximum number of seeds to make in one slice
    std::string                            fClusterInput;      // Input folder from cluster reco
    double                                 fDeltaChiAllowed;   // Maximum change in chi squared by the addition of one hit
    unsigned int                           fMinHits;           // Minimum number of hits for a track to exist, in each view 
    int                                    fGapAllowed;        // Maximum gap in z allowed for a hit to be added to a track, in planes
    int                                    fLargeSliceHits;    // Start by finding long track first
    bool                                   fBadChannels;       // Allows tracking through bad channels
    bool                                   fLongTrackLoop;     // Decides if should look for longer tracks first
    double                                 fMinGapProb;        // Minimum acceptable probability of gap between track hits
    bool                                   fObeyPreselection;  // Check rb::IsFiltered?

    art::ServiceHandle<geo::Geometry>      fgeom;
    art::ServiceHandle<chaninfo::BadChanList> fbadc;
    std::vector<double> avgXPos;
    std::vector<std::vector<double> > avgXPosZRange;
    std::vector<double> avgYPos;
    std::vector<std::vector<double> > avgYPosZRange;
    double avgX;
    double avgY;
    double fSysVariance = 0.0001;         // Hard coding the system variance
    int NCells[2];                        // Maximum number of detector cells in each view NCell[0] = x view, NCell[1] = y view
    int fNChans;  // number of hits in slice available to make tracks out of
    std::vector<trk::LocatedChan>  fSliceChans;  // vector of hits in a view of a given slice sorted by plane (low to high)
    std::vector<trk::LocatedChan>  fSliceChansCellSort;  // same as fSliceChans except sorted by cell (low to high)
    std::vector<int>   fPlaneToCell;  // vector whose index corresponds to the position of a hit in fSliceChans and value of the index corresponds to the position in the fSliceChansCellSort
    geo::View_t  fView; // currentview of the hits in fSliceChans
    double fCellW; // cell width
    double fCellL; // cell length
    bool   fTrySingle; // try adding all/most of all the hits to a single track?

    // This function takes in hits on a given slice in a single and returns a vector of reconstructed tracks. 
    std::vector<rb::Track>                 FindTracks(KalmanGeoHelper& kgeo, std::vector<trk::LocatedChan> sliceChans);

    // This function forms small track segements that will be tracked through the detector using the available
    // hits in the slice
    int                                    SegmentFinder(KalmanGeoHelper& kgeo,
                                                         std::vector< std::vector<double> > &iP,
                                                         std::vector< bool> &propDir,
                                                         std::vector< std::vector<bool> > &trkHits,
                                                         std::vector< std::vector<std::array<double, 2> > > &trkTraj,
                                                         std::vector< std::vector<std::array<double, 2> > > &trkDirs );

    // This function combines all the hits after firsthit into a single straight line track segment
    double                                 SingleSegment(std::vector< std::vector<double> > &iP,
                                                         std::vector< bool> &propDir,
                                                         std::vector< std::vector<bool> > &trkHits,
                                                         std::vector< std::vector<std::array<double, 2> > > &trkTraj,
                                                         std::vector< std::vector<std::array<double, 2> > > &trkDirs,
                                                         int firsthit);

    // This function does most of the tracking work. It takes the input track and propogates it through the detector
    // plane by plane adding more hits when it comes accross ones that are consistent with the given track
    double                                 FilterTracker(KalmanGeoHelper& kgeo,
                                                         int zDir, bool zProp, std::vector<bool> &trkHits,
                                                         std::vector<std::array<double, 2> > &trkTrajs,
                                                         std::vector<std::array<double, 2> > &trkDirs,
                                                         std::vector<double> &iP, int &outnhits, int firsthit = -1);

    // This function takes in a vector of tracks defined by which hits are associated to a track and checks for duplicate
    // tracks. If a duplicate is found the one with the best fit is kept.
    void                                   RemoveSameTracks(std::vector< std::vector<bool> > &trkHits, 
                                                            std::vector< std::vector<std::array<double, 2> > > &trkTrajs,
                                                            std::vector< std::vector<std::array<double, 2> > >&trkDirs,
                                                            std::vector< double > &chi2,
                                                            std::vector< std::vector<double> > &pfiltState, 
                                                            std::vector<bool> &zProp,
                                                            std::vector<int> &ntrkHits,
                                                            std::vector<bool> &ignoreTrk);

    // Give the input track, the function removes the hits associated with the track from the list of hits available
    // for track finding in the slice
    void                                   RemoveHitsFromSignal(const rb::Track & track);

    // Given the input track hits, this function fits the track and also finds the hit that is the largest outlier
    // from the fit
    double                                 FilterOnly(const std::vector<bool> & trkHits, 
                                                      std::vector<std::array<double, 2> > &trkTrajs,
                                                      std::vector<std::array<double, 2> > &trkDirs,
                                                      std::vector<double> &initialP, int zDir, bool zProp, 
                                                      int &newOutlierPos);

    // This checks that the given track fits the criteria defined by the fcl parameters as an acceptable track.
    // If too large a gap exists between hits on the track the input track is split into two pieces at the gap.
    void                                   CheckTrack(KalmanGeoHelper& kgeo,
                                                      std::vector<bool> &trkHits, 
                                                      std::vector<std::array<double, 2> > trkTrajs,
                                                      bool zProp, std::vector<bool> &newTrkHits);

    void                                   PlaneToCellSortHits(std::vector<bool> &planeSortedHits);
    void                                   CellToPlaneSortHits(std::vector<bool> &cellSortedHits);
    void                                   PlaneToCellSort(std::vector<std::array<double, 2> > &planeSortedHits);
    void                                   CellToPlaneSort(std::vector<std::array<double, 2> > &cellSortedHits);
    void                                   CreateHitMaps();
    int                                    CountHits(const std::vector<bool> & trkHits);
    bool                                   IsSinglePlane(const std::vector<bool> & trkHits);
    int                                    GetFirstHit(const std::vector<bool> & trkHits);
    int                                    GetLastHit(const std::vector<bool> & trkHits);
  };

}

namespace trk{

  //......................................................................
  KalmanTrack::KalmanTrack(fhicl::ParameterSet const& pset)
  {
    reconfigure(pset);
    produces< std::vector<rb::Track>   >();
    produces< art::Assns<rb::Track, rb::Cluster> >();
  }
  
  //......................................................................
  KalmanTrack::~KalmanTrack()
  {
  }


  //......................................................................
  void KalmanTrack::reconfigure(const fhicl::ParameterSet& pset)
  {
    fMaxSliceHits      = pset.get< double      >("MaxSliceHits")    ;
    fMaxHitCut         = pset.get< double      >("MaxHitCut")       ;
    fWhoaNelly         = pset.get< int         >("WhoaNelly")       ;
    fClusterInput      = pset.get< std::string >("ClusterInput")    ;
    fDeltaChiAllowed   = pset.get< double      >("DeltaChiAllowed") ;
    fMinHits           = pset.get< int         >("MinHits")         ;
    fGapAllowed        = pset.get< int         >("GapAllowed")      ;
    fLargeSliceHits    = pset.get< int         >("LargeSliceHits")  ;
    fBadChannels       = pset.get< bool        >("BadChannels")     ;
    fLongTrackLoop     = pset.get< bool        >("LongTrackLoop")   ;
    fMinGapProb        = pset.get< double      >("MinGapProb")      ;
    fObeyPreselection  = pset.get< bool        >("ObeyPreselection");
  }

  //......................................................................
  /// Reconstruction method for this module. Provides read-write access
  /// to the event data.

  void KalmanTrack::produce(art::Event& evt)
  {

    // Declare a container for Track objects to be stored in the art::event
    std::unique_ptr< std::vector<rb::Track> > trackcol(new std::vector<rb::Track>);
    std::unique_ptr< art::Assns<rb::Track, rb::Cluster> > assns(new art::Assns<rb::Track, rb::Cluster>);

    // Make the helper geo function
    KalmanGeoHelper kgeo(fBadChannels);

    // define a vector holding slices 
    art::Handle<std::vector<rb::Cluster> > slicecol;
    evt.getByLabel(fClusterInput,slicecol);
    //Now make it an art::PtrVector
    art::PtrVector<rb::Cluster> slicelist;
    for(unsigned int i = 0; i < slicecol->size(); ++i){
      if(fObeyPreselection && rb::IsFiltered(evt,slicecol,i)){ continue; }
      art::Ptr<rb::Cluster>slice(slicecol,i);
      slicelist.push_back(slice);
    }

    // get the total number of non masked off channels by looping over all planes/cells in the detector
    int numGoodChannels = 0;
    bool foundmaxcellx = false;
    bool foundmaxcelly = false;
    for(unsigned int iPlane = 0; iPlane<fgeom->NPlanes();++iPlane){
      const geo::PlaneGeo* p = fgeom->Plane(iPlane);
      if(!foundmaxcellx && p->View() == geo::kX){
        NCells[0] = p->Ncells()-1;
        foundmaxcellx = true;
      }
      else if(!foundmaxcelly && p->View() == geo::kY){
        NCells[1] = p->Ncells()-1;
        foundmaxcelly = true;
      }
      for(unsigned int iCell = 0;iCell<p->Ncells();++iCell){
        if(!(fbadc->IsBad(iPlane,iCell))){ ++numGoodChannels; }
      }
    }

    double xCellW = 0.0;
    double xCellL = 0.0;
    double yCellW = 0.0;
    double yCellL = 0.0;

    // Loop over slices to try to fit tracks in each individual slice
    // ignoring the noise slice
    for(unsigned int iSlice = 0; iSlice < slicelist.size(); iSlice++){
      // skip noise slices and big slices 
      if(slicelist[iSlice]->IsNoise() || slicelist[iSlice]->NCell() > fMaxHitCut){ continue; }

      // Check if this slice is a likely air shower. If so, do not attempt reco
      if(slicelist[iSlice]->NCell() < numGoodChannels*fMaxSliceHits){
        avgXPosZRange.resize(0);
        avgXPos.resize(0);
        avgYPosZRange.resize(0);
        avgYPos.resize(0);
        avgX = slicelist[iSlice]->MeanX();
        avgY = slicelist[iSlice]->MeanY();
        
        int minplanex = 10000;
        int maxplanex = 0;

        // Get the x cell hits out of the slice
        std::vector<trk::LocatedChan> xcellchans;
        xcellchans.reserve(slicelist[iSlice]->NXCell());
        double zxmin = 100000;
        double zxmax = 0;
        for(unsigned int nx = 0; nx < slicelist[iSlice]->NXCell(); ++nx){
          art::Ptr<rb::CellHit> xhit = slicelist[iSlice]->XCell(nx);
          LocatedChan xchan(xhit->Plane(),xhit->Cell());
          xchan.SetHit(xhit);
          double xyz[3];
          const geo::CellGeo* c = fgeom->Plane(xchan.Plane())->Cell(xchan.Cell());
          c->GetCenter(xyz);
          xchan.SetCenter(xyz[0],xyz[1],xyz[2]);
          xchan.SetHalfWidths(c->HalfW(),c->HalfL(),c->HalfD());
          xcellchans.push_back(xchan);
          if(xchan.Plane() < minplanex){ minplanex = xchan.Plane(); }
          if(xchan.Plane() > maxplanex){ maxplanex = xchan.Plane(); }
          if(nx == 0){
            xCellW = c->HalfW();
            xCellL = c->HalfD();
          }
          if(xyz[2]>zxmax){ zxmax = xyz[2]; }
          if(xyz[2]<zxmin){ zxmin = xyz[2]; }
        }
        
        // get a vector that holds the average x position for every 100 cm in z
        trk::SortByPlane(xcellchans);  
        // how many break points in z
        unsigned int nxzbreak = zxmax-zxmin > 0? (int)floor((zxmax-zxmin)/100) : 0;
        // decide where to break the z positions up
        double dz = (zxmax-zxmin)/((double)nxzbreak);
        double xzbreaks[nxzbreak+2];
        xzbreaks[0] = zxmin;
        for(unsigned int ibrk = 1; ibrk<=nxzbreak; ++ibrk){
          xzbreaks[ibrk] = zxmin+((double)ibrk)*dz;
        }
        xzbreaks[nxzbreak+1] = zxmax;
        for(unsigned int ibrk = 1; ibrk < nxzbreak+2; ++ibrk){
          double xtot = 0;
          double ntot = 0;
          for(unsigned int xchan = 0;xchan<xcellchans.size();++xchan){
            double xyz[3];
            xcellchans[xchan].GetCenter(xyz);
            if(xyz[2] >= xzbreaks[ibrk-1] && xyz[2] <=xzbreaks[ibrk]){
              xtot+=xyz[0];
              ntot+=1.0;
            }
          }
          std::vector<double> zrng;
          zrng.push_back(xzbreaks[ibrk-1]);
          zrng.push_back(xzbreaks[ibrk]);
          avgXPosZRange.push_back(zrng);
          if(ntot>0){ avgXPos.push_back(xtot/ntot); }
          else{ avgXPos.push_back(avgX); }
        }

        int minplaney = 10000;
        int maxplaney = 0;

        // Get the y cell hits out of the slice
        std::vector<trk::LocatedChan> ycellchans;
        double zymin = 100000;
        double zymax = 0;
        for(unsigned int ny = 0; ny < slicelist[iSlice]->NYCell(); ++ny){
          art::Ptr<rb::CellHit> yhit = slicelist[iSlice]->YCell(ny);
          LocatedChan ychan(yhit->Plane(),yhit->Cell());
          ychan.SetHit(yhit);
          double xyz[3];
          const geo::CellGeo* c = fgeom->Plane(ychan.Plane())->Cell(ychan.Cell());
          c->GetCenter(xyz);
          ychan.SetCenter(xyz[0],xyz[1],xyz[2]);
          ychan.SetHalfWidths(c->HalfL(),c->HalfW(),c->HalfD());
          ycellchans.push_back(ychan);
          if(ychan.Plane() < minplaney){ minplaney = ychan.Plane(); }
          if(ychan.Plane() > maxplaney){ maxplaney = ychan.Plane(); }
          if(ny == 0){
            yCellW = c->HalfW();
            yCellL = c->HalfD();
          }
          if(xyz[2]>zymax){ zymax = xyz[2]; }
          if(xyz[2]<zymin){ zymin = xyz[2]; }
        }

        // how many break points in yz
        int nyzbreak = zymax-zymin > 0? (int)floor((zymax-zymin)/100) : 0;
        // decide where to break the z positions up
        dz = (zymax-zymin)/((double)nyzbreak);
        std::vector<double> yzbreaks;
        yzbreaks.push_back(zymin);
        for(int ibrk = 1; ibrk<=nyzbreak; ++ibrk){
          yzbreaks.push_back(zymin+((double)ibrk)*dz);
        }
        yzbreaks.push_back(zymax);
        for(unsigned int ibrk = 1; ibrk < yzbreaks.size(); ++ibrk){
          double ytot = 0;
          double ntot = 0;
          for(unsigned int ychan = 0;ychan<ycellchans.size();++ychan){
            double xyz[3];
            ycellchans[ychan].GetCenter(xyz);
            if(xyz[2] >= yzbreaks[ibrk-1] && xyz[2] <=yzbreaks[ibrk]){
              ytot+=xyz[1];
              ntot+=1.0;
            }
          }
          std::vector<double> zrng;
          zrng.push_back(yzbreaks[ibrk-1]);
          zrng.push_back(yzbreaks[ibrk]);
          avgYPosZRange.push_back(zrng);
          if(ntot>0){ avgYPos.push_back(ytot/ntot); }
          else{ avgYPos.push_back(avgY); }
        }

        // Define the vectors to hold the track info from the new FindTracks function
        std::vector<rb::Track> xTracks;
        fSliceChans = xcellchans;
        fNChans = fSliceChans.size();
        fView = geo::kX;
        fCellW = xCellW;
        fCellL = xCellL;
        
        if(xcellchans.size() >= fMinHits){ xTracks = FindTracks(kgeo, xcellchans); }

        // Define the vectors to hold the track info from the new FindTracks function
        std::vector<rb::Track> yTracks;
        fSliceChans = ycellchans;
        fNChans = fSliceChans.size();
        fView = geo::kY;
        fCellW = yCellW;
        fCellL = yCellL;

        if(ycellchans.size() >= fMinHits){yTracks = FindTracks(kgeo, ycellchans); }

        std::vector<rb::Track> alltracks;
        for(unsigned int i = 0; i<xTracks.size();++i){
          xTracks[i].SetID(iSlice);
          alltracks.push_back(xTracks[i]);
        }
        for(unsigned int i = 0; i<yTracks.size();++i){
          yTracks[i].SetID(iSlice);
          alltracks.push_back(yTracks[i]);
        }
        for(std::vector<rb::Track>::const_iterator it = alltracks.begin(); it != alltracks.end(); ++it){
          trackcol->push_back(*it);
          util::CreateAssn(*this,evt,*trackcol,slicelist[iSlice],*assns);
        }
      } // end of if statement on requirement on the  number of hits in slice
    } // end of loop over slices

    evt.put(std::move(trackcol));
    evt.put(std::move(assns));

  }

  /////////////////////////////////////////////////////////
  std::vector<rb::Track>  KalmanTrack::FindTracks(KalmanGeoHelper& kgeo, std::vector<trk::LocatedChan> sliceChans)
  {
    std::vector<rb::Track> outtracks;
    // set a boolean to tell when we are done finding tracks
    bool Done = false;
    // keep track of times in the loop so only the first time look for long tracks only - SL
    int nTimes = 0;
    fTrySingle = true;
    // Loop over the slice hits until no more acceptable tracks are found
    // Now that we have cell hits lets sort them from lowest plane to highest

    trk::SortByPlane(fSliceChans);
    while(!Done){
      fSliceChansCellSort = fSliceChans;
      trk::SortByCell(fSliceChansCellSort);
      // (re)create hit map between plane and cell sorted channels
      CreateHitMaps();

      // Make segments which consist of two points that form the start of a track       
      std::vector<rb::Track> trackSegs;    
      std::vector< std::vector<double> > iP;
      std::vector<bool> zProps;
      std::vector< std::vector<bool> > trkHits;
      std::vector< std::vector<std::array<double, 2> > > trkTrajs;
      std::vector< std::vector<std::array<double, 2> > > trkDirs;
      bool initTrack = true;
      if(fNChans < fLargeSliceHits){ fTrySingle = false; }
      
      int ntracks = SegmentFinder(kgeo,iP,zProps,trkHits,trkTrajs,trkDirs);
     
      //create some vectors to hold track information in
      std::vector< double >  Chi2(trkHits.size(),0.0);   // chi squared value of the track;
      std::vector< int > nTrkHits(trkHits.size());   // number of hits in the track
      std::vector< bool > ignoreTrk(trkHits.size(),false);  // ignore this track?
            
      unsigned int propDirIters = 3; // maximum number of iterations to do tracking/filtering
      int propDir = -1;
      for(unsigned int iprop = 0; iprop < propDirIters; ++iprop){
        
        // Do the track finding 
        int largestTrack = 0;
        for(unsigned int it = 0; it< trkHits.size(); ++it){
          if(!ignoreTrk[it]){
            int n = CountHits(trkHits[it]);
            nTrkHits[it] = n;
            if(n > 0 && n < fNChans){
              if(n > 2 && fTrySingle){
                Chi2[it] = FilterTracker(kgeo,propDir,zProps[it],trkHits[it],trkTrajs[it],trkDirs[it],iP[it],nTrkHits[it]);
              }
              else{
                Chi2[it] = FilterTracker(kgeo,propDir,zProps[it],trkHits[it],trkTrajs[it],trkDirs[it],iP[it],nTrkHits[it]);
              }
              if(nTrkHits[it] > largestTrack){ largestTrack = nTrkHits[it]; }
            }
          }
        }
        if(fTrySingle && ntracks > 0 && nTrkHits[0] == fNChans){ initTrack = false; }
        
        // Remove tracks that have exactly the same hits in them as another track, keeping the one with the lowest chi square.
        RemoveSameTracks(trkHits,trkTrajs,trkDirs,Chi2,iP,zProps,nTrkHits,ignoreTrk);

        propDir = (-1*propDir); // Set filter direction to opposite of that in the track finding stage
        int minFiltHits = 2; // Need at least two hits to make a track

        // If we're on the last propogation stage, then minimum filtered hits 
        // is the minimum number of hits for a track
        if(iprop == propDirIters-1 && fMinHits>2){ minFiltHits = (fMinHits-1); }

        // Iterate over the filtering
        for(unsigned int i = 0; i< trkHits.size(); i++){

          // no need to filter tracks that we don't care about
          if(nTrkHits[i] >= minFiltHits && !ignoreTrk[i]){

            // check that this is being propogated in the correct manner
            if(zProps[i]){
              if(IsSinglePlane(trkHits[i])){
                zProps[i] = false;
                PlaneToCellSortHits(trkHits[i]);
                PlaneToCellSort(trkTrajs[i]);
                PlaneToCellSort(trkDirs[i]);
              }
            }
            else{
              std::vector<bool> sortedhits = trkHits[i];
              CellToPlaneSortHits(sortedhits);
              if(!IsSinglePlane(sortedhits)){
                zProps[i] = true;
                trkHits[i] = sortedhits;
                CellToPlaneSort(trkTrajs[i]);
                CellToPlaneSort(trkDirs[i]);
              }
            }

            // define a boolean to determine whether or not to continue iterating
            bool filter = true;
            // save the number of hits on the track
            int nhitsold = nTrkHits[i];

            // keep track of the number of iterations needed for filtering
            int filterIter = 0;
            int maxFilter = 2; // maximum number of iterations over filtering
            
            while(filter && filterIter < maxFilter){

              ++filterIter; // update the counter

              // Filter the track
              int trkOutlier = -1;
              Chi2[i] = FilterOnly(trkHits[i],trkTrajs[i],trkDirs[i],iP[i],propDir,zProps[i],trkOutlier);
              
              // remove outlier if it exists, give the fitter at least one chance to get it right first
              if(filterIter>0){
                if(trkOutlier != -1){
                  trkHits[i][trkOutlier] = false;
                  --nTrkHits[i];
                  --filterIter;
                }
              }
              
              // if number of hits is less than the minimum than stop filtering
              if(nTrkHits[i] < minFiltHits){
                filter = false;
                break; // exit while loop of filtering
              }
                      
              // May need to re-sort hits if any where removed
              if(nTrkHits[i] != nhitsold){
                if(zProps[i]){
                  if(IsSinglePlane(trkHits[i])){
                    zProps[i] = false;
                    PlaneToCellSortHits(trkHits[i]);
                    PlaneToCellSort(trkTrajs[i]);
                    PlaneToCellSort(trkDirs[i]);
                  }
                }
              }
              
              // If we didn't remove any hits and filtered more than once we can stop
              if(nhitsold == nTrkHits[i] && filterIter > 0){

                // If this is the last propogation
                // check that this track meets the tracking criteria
                if(iprop == propDirIters-1 || !initTrack){
                  
                  // check the track
                  std::vector<bool> newTrkHits;
                  CheckTrack(kgeo,trkHits[i],trkTrajs[i],zProps[i],newTrkHits);

                  // update hit counters
                  nTrkHits[i] = CountHits(trkHits[i]);
                  int nhitsnew = CountHits(newTrkHits);

                  if(nhitsnew > 0 && nTrkHits[i] > 0){
                    --filterIter;

                    // if track now has less than desired number of hits, stop filtering
                    if(nTrkHits[i] < minFiltHits){ filter = false;  }

                    // may need to re-sort hits if this track had hits removed
                    if(filter && zProps[i]){
                      if(IsSinglePlane(trkHits[i])){
                        zProps[i] = false;
                        PlaneToCellSortHits(trkHits[i]);
                        PlaneToCellSort(trkTrajs[i]);
                        PlaneToCellSort(trkDirs[i]);
                      }
                    }

                    // add the second piece of the track onto the track list if its large enough
                    if(nhitsnew >= minFiltHits){ 
                      iP.push_back(iP[i]);
                      zProps.push_back(zProps[i]);
                      Chi2.push_back(0);
                      trkHits.push_back(newTrkHits);
                      trkTrajs.push_back(trkTrajs[i]);
                      trkDirs.push_back(trkDirs[i]);
                      nTrkHits.push_back(nhitsnew);
                      ignoreTrk.push_back(false);
                    }

                  }
                  else{ 
                    // if track passes check stop filtering
                    filter = false;
                    break;
                  }
                  // nhitsold = nTrkHits[i];
                } // if last propogation
                else{ 
                  // not the last propogation step stop filtering now
                  filter = false;
                  break;
                }
              } // end of if number of hits is the same and filter counter is greater than 0

              // update the nhits counter
              nhitsold = nTrkHits[i];

            } // end of while loop over filtering
          } // end if this is a track we care to filter
        } // end of loop over tracks
        
        if(!initTrack){ break; }
        
      } // end of loop over iprop
      
      unsigned int loopMinHits = fMinHits;
      if((nTimes == 0) && (fLongTrackLoop)) {  //Addition by Susan Lein
        for (unsigned int s=0; s<trkHits.size(); ++s){
          if ((.85*nTrkHits[s]) > loopMinHits && !ignoreTrk[s]) {
            unsigned int trkMinHits = .85*nTrkHits[s];
            loopMinHits = trkMinHits;
          }
        }
        nTimes = 1;
      }
      else { nTimes = 2; }
      
      if(trkHits.size() > 0){
        // Find best track
        // use the chi2/(number of hits in track) as measure of goodness of track. Track with min of this is best
        double chiOverLength = 99999;   
        int bestTrack = -1;
        for(unsigned int i = 0; i<trkHits.size();i++){
          double nhits = nTrkHits[i];
          if(nhits < loopMinHits || ignoreTrk[i]){ continue; }
          double chiOverLengthnew = Chi2[i]/nhits;
          if(chiOverLengthnew < chiOverLength){
            bestTrack = i;
            chiOverLength = chiOverLengthnew;
          }
        }
        if(bestTrack >= 0){

          // make a track out of the best one
          rb::Cluster clust(fView);
          int firstHit = GetFirstHit(trkHits[bestTrack]);
          rb::Track tracknew = rb::Track(clust,trkTrajs[bestTrack][firstHit][0],
                                      trkTrajs[bestTrack][firstHit][1],
                                      trkDirs[bestTrack][firstHit][0],
                                      trkDirs[bestTrack][firstHit][1]);


          // add the hits and trajectory points to the track
          for(int ihit = 0; ihit < fNChans; ++ihit){
            if(trkHits[bestTrack][ihit]){
              if(zProps[bestTrack]){ tracknew.Add(fSliceChans[ihit].GetHit()); }
              else{ tracknew.Add(fSliceChansCellSort[ihit].GetHit()); }
              if(ihit != firstHit){ tracknew.AppendTrajectoryPoint(trkTrajs[bestTrack][ihit][0],trkTrajs[bestTrack][ihit][1]); }
            }
          }

          std::vector<int> extratrpts;
          for(unsigned int itraj = 1; itraj<tracknew.NTrajectoryPoints();++itraj){
            TVector3 oldtrpt = tracknew.TrajectoryPoint(itraj-1);
            if(oldtrpt.X() == tracknew.TrajectoryPoint(itraj).X() &&
               oldtrpt.Y() == tracknew.TrajectoryPoint(itraj).Y() &&
               oldtrpt.Z() == tracknew.TrajectoryPoint(itraj).Z()){ extratrpts.push_back(itraj);}
          }
          std::sort(extratrpts.begin(),extratrpts.end());
          for(int rmpt = ((int)extratrpts.size()-1);rmpt>=0;--rmpt){
            tracknew.RemoveTrajectoryPoint(extratrpts[rmpt]);
          }
          if(zProps[bestTrack]){
            // extend the track ends to the distance of closest approach of the end hits on the track.
            // this should only be neccessary for tracks that were propagated in z, i.e. are more than one plane long
            // get the start position and direction
            TVector3 start = tracknew.Start();
            TVector3 dir = tracknew.Dir();
            double startslope = dir.X()/dir.Z();
            if(fView == 1){startslope = dir.Y()/dir.Z(); }
            // get the extent of the track in the start plane
            if(startslope != 0){ // if slope is zero the fit should be correct already
              int startCell = 10000;
              if(startslope < 0){ startCell = -1; }
              for(unsigned int iHit = 0; iHit<tracknew.NCell(); ++iHit){
                if(tracknew.Cell(iHit)->Plane() == tracknew.MinPlane()){
                  if(startslope<0 && tracknew.Cell(iHit)->Cell()>startCell){startCell = tracknew.Cell(iHit)->Cell();}
                  if(startslope>0 && tracknew.Cell(iHit)->Cell()<startCell){startCell = tracknew.Cell(iHit)->Cell();}
                }
              }
              if(startCell != 10000){
                std::array<double, 2> bestStart;
                double cpos[3];
                const geo::CellGeo* scell = fgeom->Plane(tracknew.MinPlane())->Cell(startCell);
                scell->GetCenter(cpos);
                double deltaz = scell->HalfD();
                double deltav = scell->HalfW();
                // find the cell "best position" i.e. min dist to track or midpoint track makes through cell
                if(fView == 0){ kgeo.BestTrackPoint(cpos[2],cpos[0],deltaz,deltav,bestStart,start.Z(),start.X(),dir.Z(),dir.X()); }
                else if(fView == 1){ kgeo.BestTrackPoint(cpos[2],cpos[1],deltaz,deltav,bestStart,start.Z(),start.Y(),dir.Z(),dir.Y()); }
                tracknew.SetStart(bestStart[1],bestStart[0]);
              }
            }
            // get the stop position and relative direction
            TVector3 stop = tracknew.Stop();
            if(tracknew.NTrajectoryPoints()>1){
              TVector3 traj = tracknew.TrajectoryPoint(tracknew.NTrajectoryPoints()-2);
              dir = stop-traj;
            }
            double stopslope = dir.X()/dir.Z();
            if(fView == 1){stopslope = dir.Y()/dir.Z(); }
            if(stopslope != 0){ // if slope is zero the fit should be correct already
              // get the extent of the track in the stop plane
              int stopCell = 10000;
              if(stopslope > 0){ stopCell = -1; }
              for(unsigned int iHit = 0; iHit<tracknew.NCell(); ++iHit){
                if(tracknew.Cell(iHit)->Plane() == tracknew.MaxPlane()){
                  if(stopslope<0 && tracknew.Cell(iHit)->Cell()<stopCell){stopCell = tracknew.Cell(iHit)->Cell();}
                  if(stopslope>0 && tracknew.Cell(iHit)->Cell()>stopCell){stopCell = tracknew.Cell(iHit)->Cell();}
                }
              }
              if(stopCell!= 10000){
                std::array<double, 2> bestStop;
                double cpos[3];
                const geo::CellGeo* stcell = fgeom->Plane(tracknew.MaxPlane())->Cell(stopCell);
                stcell->GetCenter(cpos);
                double deltaz = stcell->HalfD();
                double deltav = stcell->HalfW();
                // find the cell "best position" i.e. min dist to track or midpoint track makes through cell
                if(fView == 0){ kgeo.BestTrackPoint(cpos[2],cpos[0],deltaz,deltav,bestStop,stop.Z(),stop.X(),dir.Z(),dir.X()); }
                else if(fView == 1){ kgeo.BestTrackPoint(cpos[2],cpos[1],deltaz,deltav,bestStop,stop.Z(),stop.Y(),dir.Z(),dir.Y()); }
                tracknew.RemoveTrajectoryPoint(tracknew.NTrajectoryPoints()-1);
                tracknew.AppendTrajectoryPoint(bestStop[1],bestStop[0]);
              }
            }
          }
          
          // if the start and stop planes are the same i.e. vertical track, pick direction so that the start is nearest to the avg pos
          if(tracknew.MinPlane() == tracknew.MaxPlane() && tracknew.NTrajectoryPoints() > 0){
            if(fView == 0){
              if(abs(tracknew.Start().X() - avgX) > abs(tracknew.Stop().X() - avgX)){
                // reverse order of trajectory points
                std::vector<TVector3> trjpoints;
                for(unsigned int i = 0; i<tracknew.NTrajectoryPoints(); ++i){
                  trjpoints.push_back(tracknew.TrajectoryPoint(i));
                }
                tracknew.ClearTrajectoryPoints();
                reverse(trjpoints.begin(),trjpoints.end());
                tracknew.SetStart(trjpoints[0].X(), trjpoints[0].Z());
                for(unsigned int i = 1; i<trjpoints.size(); ++i){
                  tracknew.AppendTrajectoryPoint(trjpoints[i].X(),trjpoints[0].Z());
                }
              }
            }
            else{
              if(abs(tracknew.Start().Y() - avgY) > abs(tracknew.Stop().Y() - avgY)){
                // reverse order of trajectory points
                std::vector<TVector3> trjpoints;
                for(unsigned int i = 0; i<tracknew.NTrajectoryPoints(); ++i){
                  trjpoints.push_back(tracknew.TrajectoryPoint(i));
                }
                tracknew.ClearTrajectoryPoints();
                reverse(trjpoints.begin(),trjpoints.end());
                tracknew.SetStart(trjpoints[0].Y(), trjpoints[0].Z());
                for(unsigned int i = 1; i<trjpoints.size(); ++i){
                  tracknew.AppendTrajectoryPoint(trjpoints[i].Y(),trjpoints[0].Z());
                }
              }
            }
          }

          // Store best track
          // filter best track to get the correct dir vector at vertex
          outtracks.push_back(tracknew);
          nTimes = 0;
          
          // Remove track hits from signal
          RemoveHitsFromSignal(tracknew);         
          if(fNChans < (int)fMinHits){ Done = true; }
          fTrySingle = true;
        }
        else if(fTrySingle){ fTrySingle = false; }
        else{
          if(nTimes == 2){ Done = true; }
        }
      }
      else if(fTrySingle){ fTrySingle = false; }
      else{
        if(nTimes == 2){ Done = true; }
      }
    }      //end over while loop
    
    return outtracks;
    
  }  // End of FindTracks function

  /////////////////////////////////////////////////////////
  int KalmanTrack::SegmentFinder(KalmanGeoHelper& kgeo,
                                  std::vector< std::vector<double> > &iP,
                                  std::vector< bool> &zProp,
                                  std::vector< std::vector<bool> > &trkHits,
                                  std::vector< std::vector<std::array<double, 2> > > &trkTrajs,
                                  std::vector< std::vector<std::array<double, 2> > > &trkDirs)
  {
    int ntracks = 0;
    int maxgap = fGapAllowed;
    std::vector<bool> hitsTemplate(fNChans,false);
    std::vector<std::array<double, 2> > trajs(fNChans);
    std::vector<std::array<double, 2> > dirs(fNChans);
    std::vector<bool> usedVert(fNChans,false);
    std::vector<bool> usedHor(fNChans,false);

    int firstiHitPlane = fSliceChans[fNChans-1].Plane();

    if(fTrySingle){
      int firsthitplane = 0;
      SingleSegment(iP,zProp,trkHits,trkTrajs,trkDirs,firsthitplane);
      if(iP.size() > 0){ 
        firstiHitPlane = 0;
        ++ntracks;
      }
      
      int planeextent = fSliceChans[fNChans-1].Plane() - fSliceChans[0].Plane();

      // Check if also grouping just the last half of the cells works better 
      firsthitplane = planeextent/2+fSliceChans[0].Plane();
      SingleSegment(iP,zProp,trkHits,trkTrajs,trkDirs,firsthitplane);
      if((int)iP.size() > ntracks){
        ++ntracks;
        // if we didn't find a usable track using all the hits update firstiHitPlane here so we can consider
        if(ntracks < 2){ firstiHitPlane = firsthitplane; }
        
      }
      if(ntracks < 2){
        // try another division of hits
        firsthitplane = firsthitplane + planeextent/4;
        SingleSegment(iP,zProp,trkHits,trkTrajs,trkDirs,firsthitplane);
        if((int)iP.size() > ntracks){
          ++ntracks;
          // if we didn't find a usable track using all the hits update firstiHitPlane here so we can consider
          if(ntracks < 2){ firstiHitPlane = firsthitplane; }
        }
      }
      
      
      if(ntracks > 1){ firstiHitPlane = 0; }

    }

    // If we didn't find any tracks this way don't try it anymore
    if(ntracks == 0){ fTrySingle = false; }

    // loop over all points cell hits and consider them as the starting point
    for(int iHit = fNChans-1; iHit > 0; --iHit){
      // get this hits location
      int iPlane = fSliceChans[iHit].Plane();
      if(fSliceChans[iHit].Plane() >= firstiHitPlane){ continue; }
      int iCell = fSliceChans[iHit].Cell();
      // loop over all other cell hits and consider them as a compatible point 
      for(int fHit = iHit-1; fHit >= 0; --fHit){

        // check that number of seed is not less than maximum;
        if(ntracks >= fWhoaNelly){ return ntracks; }

        //make loose cut on number of planes between the hits
        int fPlane = fSliceChans[fHit].Plane();
        int deltap = (iPlane-fPlane-2)/2;

        // if too large a gap in planes skip
        if(deltap > maxgap){ continue; }

        // get information about the cell hits
        int fCell = fSliceChans[fHit].Cell();
        //make vectors to hold position information.
        double ixyz[3]; // xyz[0] = x, xyz[1] = y, xyz[2] = z
        fSliceChans[iHit].GetCenter(ixyz);
        double fxyz[3];
        fSliceChans[fHit].GetCenter(fxyz);

        // check if we can make a track out of these two hits
        // Break up into 3 cases, completely veritcal in view, completely horizontal in z, and else
        // First two cases can take some short cuts
        if(iPlane == fPlane && !usedVert[iHit]){
          // hits on the same plane check number of cells between hits
          if(TMath::Abs(iCell-fCell)<=1){
            // make a track segment out of these hits
            // hits on same plane want to propogate by cell/not plane
            zProp.push_back(false);
            double delta = TMath::Abs(ixyz[fView] - fxyz[fView]);
            std::vector<bool> hits = hitsTemplate;
            int celliHitid = fPlaneToCell[iHit];
            int cellfHitid = fPlaneToCell[fHit];
            hits[celliHitid] = true;
            hits[cellfHitid] = true;
            trkHits.push_back(hits);
            trajs[celliHitid][0] = ixyz[fView];
            trajs[celliHitid][1] = ixyz[2];
            trajs[cellfHitid][0] = fxyz[fView];
            trajs[cellfHitid][1] = fxyz[2];
            trkTrajs.push_back(trajs);
            dirs[celliHitid][0] = 1.0;
            dirs[celliHitid][1] = 0.0;
            dirs[cellfHitid] = dirs[celliHitid];
            trkDirs.push_back(dirs);
            double Pconst = fCellL*fCellL/3.0;
            std::vector<double> P(3);
            P[0] = Pconst;
            P[1] = Pconst/delta;
            P[2] = 2*P[1]/delta;
            iP.push_back(P);
            usedVert[iHit] = true;
            ++ntracks;
          }
        }
        else if(iCell == fCell && !usedHor[iHit]){
          // hits in same cell number different plane
          // Already checked distance between plane, no more checks to make a track segment
          // hits exactly horizontal to each other in view so propogate by plane
          zProp.push_back(true);
          double deltaZ = (ixyz[2]-fCellL)-(fxyz[2]+fCellL);
          std::vector<double> P(3);
          double Pconst = fCellW*fCellW/3.0;
          P[0] = Pconst;
          P[1] = Pconst/deltaZ;
          P[2] = 2*P[1]/deltaZ;
          iP.push_back(P);
          usedHor[iHit] = true;
          std::vector<bool> hits = hitsTemplate;
          hits[iHit] = true;
          hits[fHit] = true;
          trkHits.push_back(hits);
          trajs[iHit][0] = ixyz[fView];
          trajs[iHit][1] = ixyz[2];
          trajs[fHit][0] = fxyz[fView];
          trajs[fHit][1] = fxyz[2]; 
          trkTrajs.push_back(trajs);
          dirs[iHit][0] = 0.0;
          dirs[iHit][1] = 1.0;
          dirs[fHit] = dirs[iHit];
          trkDirs.push_back(dirs);
          ++ntracks;
        }
        else if(iPlane != fPlane && iCell != fCell){

          double xyz[3] = {fxyz[0], fxyz[1], fxyz[2]+fCellL-0.001};
          double xyz1[3] = {ixyz[0], ixyz[1], ixyz[2]-fCellL-0.001};            
          double deltaZ = ixyz[2]-fxyz[2]-2.0*fCellL;
          double delta = ixyz[fView]-fxyz[fView];
          double m = (1.0/(deltaZ+4*fCellL));
          // from these potentially matched hits calculate how many hits lie between them
          if(iCell>fCell){
            xyz[fView]+=(fCellW+0.001);
            xyz1[fView]-=(fCellW+0.001);
            delta-=(2.0*fCellW);
            m*=TMath::Abs((delta-4*fCellW));
          }
          else if(iCell<fCell){
            xyz[fView]-=(fCellW+0.001);
            xyz1[fView]+=(fCellW+0.001);
            delta+=(2.0*fCellW);
            m*=TMath::Abs((delta+4*fCellW));
          }

          // Check how many cells the track passes through between iHit and fHit
          // By construction adjacent plane hits miss zero times
          int numMissed = 0;
          if(iPlane>(2+fPlane)){
            numMissed = kgeo.CountMissedCellsOnLine(fView,xyz[fView],xyz[2],xyz1[fView],xyz1[2],maxgap);
          }
          if(numMissed <= maxgap){
            double slope = delta/deltaZ;
            double dxyz[3] = {0, 0, 1/sqrt(1+slope*slope)};
            dxyz[fView] = slope/sqrt(1+slope*slope);
                            
            std::vector<double> P(3);
            double Pconst = (fCellL*m+fCellW)*(fCellL*m+fCellW)/3;
            P[0] = Pconst;
            P[1] = Pconst/deltaZ;
            P[2] = 2*P[1]/deltaZ;
            std::vector<bool> hits = hitsTemplate;
            hits[iHit] = true;
            hits[fHit] = true;
            trajs[iHit][0] = ixyz[fView];
            trajs[iHit][1] = ixyz[2];
            trajs[fHit][0] = fxyz[fView];
            trajs[fHit][1] = fxyz[2]; 
            dirs[iHit][0] = dxyz[fView];
            dirs[iHit][1] = dxyz[2];
            dirs[fHit] = dirs[iHit];

            trkHits.push_back(hits);
            zProp.push_back(true);
            iP.push_back(P);
            trkTrajs.push_back(trajs);
            trkDirs.push_back(dirs);
            ++ntracks;
          }
        }
      } // end of loop over fHit
    } // end of loop over iHit
    return ntracks;

  } // end of SegmentFinder


  /////////////////////////////////////////////////////////
  double KalmanTrack::SingleSegment(std::vector< std::vector<double> > &iP,
                                  std::vector< bool> &zProp,
                                  std::vector< std::vector<bool> > &trkHits,
                                  std::vector< std::vector<std::array<double, 2> > > &trkTrajs,
                                  std::vector< std::vector<std::array<double, 2> > > &trkDirs,
                                  int firsthitplane)
  {

    std::vector<trk::LocatedChan> channels = fSliceChans;

    // loop over all the hits and mark the indicated ones as hits to include on the track
    // also grab the view and z positions as well as the pe to get an initial fit
    std::vector<double> vv;
    std::vector<double> zz;
    std::vector<double> w;
    double petot = 0.0;
    std::vector<bool> hits(fNChans,false);
    // get the max/min view and z while we're at it
    double minz = 100000;
    double maxz = 0;
    double minv = 10000;
    double maxv = -10000;
    for(int ihit = 0; ihit < fNChans; ++ihit){
      if(channels[ihit].Plane() >= firsthitplane){
        hits[ihit] = true;
        double hitxyz[3];
        channels[ihit].GetCenter(hitxyz);
        vv.push_back(hitxyz[fView]);
        zz.push_back(hitxyz[2]);
        w.push_back(channels[ihit].GetHit()->PE());
        petot+=channels[ihit].GetHit()->PE();
        if(hitxyz[fView] > maxv){ maxv = hitxyz[fView]; }
        else if (hitxyz[fView] < minv){ minv = hitxyz[fView]; }
        if(hitxyz[2] > maxz){ maxz = hitxyz[2]; }
        else if(hitxyz[2] < minz){ minz = hitxyz[2]; }
      }
    }

    // do the fit
    double linfitchi2 = 0.0;
    int nhits = vv.size();
    if(nhits < 3){ return 0.0; }
    for(int i = 0; i < nhits; ++i){
      w[i] = w[i]/petot;
    }
    double z1[1],v1[1],z2[1],v2[1];
    linfitchi2 =geo::LinFitMinDperp(zz,vv,w,z1,v1,z2,v2);

    double chindf = linfitchi2/(nhits-2);
    // is this fit good enough to consider this a track segement?
    if(chindf > 3){ return chindf; }

    // Are all the hits on a single plane?
    bool prop = IsSinglePlane(hits);
    if(!prop){ 
      // sort the hits by cells
      channels = fSliceChansCellSort;
      PlaneToCellSortHits(hits);
    }
    
    double deltav = v2[0] - v1[0];
    double deltaz = z2[0] - z1[0];
    double dirv = deltav/(deltav*deltav+deltaz*deltaz);
    double dirz = deltaz/(deltav*deltav+deltaz*deltaz);

    // set the trajectory points and directions of the track    
    std::vector<std::array<double, 2> > trajs(fNChans);
    std::vector<std::array<double, 2> > dirs(fNChans);

    for(int ihit = 0; ihit < fNChans; ++ihit){
      
      if(hits[ihit]){
        double hitxyz[3];
        channels[ihit].GetCenter(hitxyz);
        if(prop){
          trajs[ihit][1] = hitxyz[2];
          trajs[ihit][0] = (dirv/dirz)*(hitxyz[2]-z1[0])+v1[0];
        }
        else{
          trajs[ihit][0] = hitxyz[fView];
          trajs[ihit][1] = (dirz/dirv)*(hitxyz[fView]-v1[0])+z1[0];
        }
        dirs[ihit][0] = dirv;
        dirs[ihit][1] = dirz;
      }

    }

    double vdelta = maxv-minv;
    double zdelta = maxz-minz;
    // set the initial covariance matrix
    double Pconst = vdelta*vdelta/12.0;
    std::vector<double> P(3);
    P[0] = Pconst;
    P[1] = Pconst/zdelta;
    P[2] = 2*P[1]/zdelta;

    trkHits.push_back(hits);
    trkTrajs.push_back(trajs);
    trkDirs.push_back(dirs);
    zProp.push_back(prop);
    iP.push_back(P);
    return chindf;
  } // end of SingleSegments


  ////////////////////////////////////////////////////////
  double KalmanTrack::FilterTracker(KalmanGeoHelper& kgeo,
                                    int zDir,bool zProp, std::vector<bool> &trkHits,
                                    std::vector<std::array<double, 2> > &trkTrajs,
                                    std::vector<std::array<double, 2> > &trkDirs,
                                    std::vector<double> &errorCov, int &outnhits,
                                    int firsthit)
  {
    std::vector<trk::LocatedChan> & channels = zProp? fSliceChans : fSliceChansCellSort;
    double chi = 0.0; // return value
    // get which 2d view we are in from the slice hits
    double maxDeltaChi = fDeltaChiAllowed;
    int maxMisses = 1.5*fGapAllowed;
    double cellVarW = fCellW*fCellW;
    double cellVarL = fCellL*fCellL;
    int misses = 0;
    bool done = false;
    int nHits = CountHits(trkHits);
    if(nHits == 0){ return 0.0; }

    // find the first hit attached to this track
    int iFirstHit = 0;

    if(zDir < 0){ iFirstHit = fNChans-1; }
    if(firsthit < 0){
      bool firstHitfound = false;
      while(!firstHitfound && iFirstHit<fNChans && iFirstHit >= 0){
        if(zProp){
          if(trkHits[iFirstHit]){ firstHitfound = true; }
          else{iFirstHit+=zDir;}
        }
        else{
          if(trkHits[iFirstHit]){ firstHitfound = true; }
          else{ iFirstHit+=zDir; }
        }
      }
    }
    else{ iFirstHit = firsthit; }
    
    unsigned short iPlane = channels[iFirstHit].Plane();
    unsigned short iCell = channels[iFirstHit].Cell();;

    // Find the first slice hit that belongs to the same plane/cell as the first hit attached to the track
    int iCurrentHit = 0; 
    if(zDir < 0){ iCurrentHit = fNChans-1; } 
    bool currentHitfound = false;
    while(!currentHitfound && iCurrentHit<fNChans && iCurrentHit >= 0){
      if(zProp){
        if(channels[iCurrentHit].Plane() == iPlane){ currentHitfound = true; }
        else{iCurrentHit+=zDir;}
      }
      else{
        if(channels[iCurrentHit].Cell() == iCell){ currentHitfound = true; }
        else{ iCurrentHit+=zDir; }
      }
    }

    // get the starting trajectory point of the track (trajectory point of the first hit)
    const std::array<double, 2> & start = trkTrajs[iFirstHit];
    std::array<double, 2> dir = trkDirs[iFirstHit];
    int iCurrentTrackHit = iFirstHit; // keep track of which index corresponds to the currentHitPlane, currentHitCell

    double position;
    double slope;
    double otherpos;
    if(zProp){
      position = start[0];
      otherpos = start[1];
      if(dir[1] == 0){ dir[1] = 0.0001; }
      slope = dir[0]/dir[1];
    }
    else{
      position = start[1];
      otherpos = start[0];
      if(dir[0] == 0){ dir[0] = 0.0001; }
      slope = dir[1]/dir[0];
    }

    double R = cellVarW/3.0;
    if(!zProp){ R = cellVarL/3.0; };
    double Q[3] = {0, 0, fSysVariance};
    // state[0] = filtered position, state[1] = position of measurement in other view, state[2] = slope;
    double state[3] = {position, otherpos, slope}; 

    int iNextHit = iCurrentHit;

    // forget about all current track hits
    if(firsthit < 0){
      for(int i = 0; i < fNChans; ++i){
        trkHits[i] = false;
      }
      outnhits = 0;
    }
    else{ outnhits = nHits; }

    int nTrackHits = outnhits; // running total of number of hits added to track
    // Create variable for projecting states into the next plane of the detector
    // initialize the estimates state and error covairance
    double xMinus[3];
    double pMinus[3];
    double K[2];
    double xPredict[3];
    double cPredict[3];

    double delta;
    short currentHitPlane = iPlane;
    int expectedHitPlane = currentHitPlane;
    short currentHitCell = iCell;
    short expectedHitCell = currentHitCell;

    std::array<double, 2> bestpos;

    while(((iCurrentHit < (fNChans-1) && zDir > 0) || (iCurrentHit > 0 && zDir < 0)) && !done){
     
      short nextHitPlane = channels[iNextHit].Plane();
      short nextHitCell = channels[iNextHit].Cell();

      // update current hit plane and cell
      if(nTrackHits>0){
        currentHitPlane = channels[iCurrentTrackHit].Plane();
        currentHitCell = channels[iCurrentTrackHit].Cell();
      }
      // Define a boolean to check if the next hit is in the layer(plane/cell) that we expect it to be in;
      bool sameLayerTest;
      if(zProp){sameLayerTest = ((nextHitPlane-currentHitPlane) == (expectedHitPlane-currentHitPlane)); }
      else{ sameLayerTest = (((nextHitCell-currentHitCell) == (expectedHitCell-currentHitCell))); }

      // Check if the next slice hit is in the plane/cell where the expected hit
      if(sameLayerTest){
        // keep track of information about hits added to the track
        double peWeight = 0;
        double layerState[3] = {state[0], state[1], state[2]};
        double layerP[3] = {errorCov[0], errorCov[1], errorCov[2]};

        int k = iNextHit;
        int firstLayerHit = iNextHit; 

        if(zProp){ while(k < fNChans && k >= 0 && channels[k].Plane() == nextHitPlane){ k+=zDir; } }
        else{ while(k < fNChans && k >= 0 && channels[k].Cell() == nextHitCell){ k+=zDir; } }
        int nadd = zDir*(k-iNextHit);
        bool layerTrackHits[nadd];
        int iLayerHit = firstLayerHit;
        int fLayerHit = k-zDir;
        for(int i = 0; i < nadd; ++i){
          layerTrackHits[i] = false;
          if(trkHits[iNextHit]){
            trkHits[iNextHit] = false;
            --outnhits;
          }
          iCurrentHit = iNextHit;
          iNextHit = iCurrentHit + zDir;
        }
        bool doneAdding = false;
        if(nadd == 0){ doneAdding = true;}

        int nhits = 0;
        int layerWindowlow = 0;
        int layerWindowhigh = 999999;
        if(!zProp){
          if(((state[2] <= 0 && zDir < 0) || (state[2] >= 0 && zDir  > 0))){
            if(nTrackHits>0){
              layerWindowlow = channels[iCurrentTrackHit].Plane();
              if(abs(nextHitCell-currentHitCell)>1){
                layerWindowlow = channels[iCurrentTrackHit].Plane()-2; 
                layerWindowhigh = channels[iCurrentTrackHit].Plane()+6;
              }
            }
          }
          else{
            if(nTrackHits>0){
              layerWindowhigh = channels[iCurrentTrackHit].Plane();
              if(abs(nextHitCell-currentHitCell)>1){
                layerWindowhigh = channels[iCurrentTrackHit].Plane()+2;  
                layerWindowlow = channels[iCurrentTrackHit].Plane()-6;  
              }
            }
          }
        }
        while(!doneAdding){
          double minDistBest = -1;
          int bestHitIndex = 9999;
          if(nadd == 1 && !layerTrackHits[0]){ bestHitIndex = 0; }
          else{
            for(int i = 0; i<nadd;++i){
              // do nothing if this hit has already been added to the track
              if(!layerTrackHits[i]){
                int currChanIdx = firstLayerHit+i*zDir;

                // check if this hit falls within the track window
                if(zProp && (channels[currChanIdx].Cell()<layerWindowlow || channels[currChanIdx].Cell()>layerWindowhigh)){ continue; }
                else if(!zProp && (channels[currChanIdx].Plane()<layerWindowlow || channels[currChanIdx].Plane()>layerWindowhigh)){ continue; }

                //Get the bestpoint of this track
                double xyzHit[3];
                channels[currChanIdx].GetCenter(xyzHit);
                if(zProp){kgeo.BestTrackPoint(xyzHit[2],xyzHit[fView],fCellL,fCellW,bestpos,layerState[1],layerState[0],dir[1],dir[0]);}
                else{kgeo.BestTrackPoint(xyzHit[2],xyzHit[fView],fCellL,fCellW,bestpos,layerState[0],layerState[1],dir[1],dir[0]);}
                // Get the minimum distance to the projected track
                double minHitDist = 0.0;
                if(zProp){ minHitDist = abs(bestpos[1]-(layerState[0]+layerState[2]*(bestpos[0]-layerState[1])));}
                else{ minHitDist = abs(bestpos[0]-(layerState[0]+layerState[2]*(bestpos[1]-layerState[1]))); }
                if((minHitDist < minDistBest) || minDistBest < 0){
                  bestHitIndex = i*zDir;
                  minDistBest = minHitDist;
                }
              }
            } // end of loop over nadd
          } // end of else nadd == 1     
          if(bestHitIndex != 9999){
            int bestChanIdx = firstLayerHit+bestHitIndex;
            double xyzHit[3];
            channels[bestChanIdx].GetCenter(xyzHit);

            // Update the prediction and error matrices
            if(zProp){ delta = xyzHit[2] - layerState[1]; }
            else{ delta = xyzHit[fView] - layerState[1]; }
            Q[0] = delta*delta*Q[2] + layerP[0]; 
            Q[1] = delta*Q[2];
            
            //Predict next state using a linear model
            xMinus[0] = layerState[0] + delta*layerState[2];
            xMinus[2] = layerState[2];
            pMinus[0] = layerP[0] + 2.0*delta*layerP[1] + delta*delta*layerP[2] + Q[0];
            pMinus[1] = layerP[1] + delta*layerP[2] + Q[1];
            pMinus[2] = layerP[2] + Q[2];
            
            //calculate filter at this layer
            double denom = (pMinus[0]+R);
            K[0] = pMinus[0]/denom;
            K[1] = pMinus[1]/denom;
            
            // get the measurements from the hit
            double candidatePos;
            double ipos;
            if(zProp){
              candidatePos = xyzHit[fView];
              ipos = xyzHit[2]; 
              xPredict[1] = ipos;
            }
            else{
              candidatePos = xyzHit[2];
              ipos = xyzHit[fView];
              xPredict[1] = ipos;
            }

            //calculate corrected states with inclusion of this hit
            xPredict[0] = xMinus[0]+K[0]*(candidatePos-xMinus[0]);
            xPredict[2] = xMinus[2]+K[1]*(candidatePos-xMinus[0]);
            cPredict[0] = pMinus[0]*R/denom;
            cPredict[1] = pMinus[1]*R/denom;
            cPredict[2] = (pMinus[2]*pMinus[0]-pMinus[1]*pMinus[1]+pMinus[2]*R)/denom;
            
            double r2 = (candidatePos - xPredict[0])*(candidatePos - xPredict[0]);
            double sl2 = xPredict[2]*xPredict[2];
            double deltaChi = 0.0;
            if(zProp){
              deltaChi = (r2*(1+sl2)*(1+sl2))/
                (((1+sl2)*(1+sl2)*(cellVarW/3+sl2*cellVarL/3+cPredict[0]))+r2*sl2*cPredict[2]/2);
            }
            else{
              deltaChi = (r2*(1+sl2)*(1+sl2))/
              (((1+sl2)*(1+sl2)*(cellVarL/3+sl2*cellVarW/3+cPredict[0]))+r2*sl2*cPredict[2]/2);
            }

            if(deltaChi <= maxDeltaChi){
              ++nhits;
              chi+=deltaChi;
              trkHits[bestChanIdx] = true;
              ++outnhits;
              ++nTrackHits;
              layerTrackHits[(bestHitIndex/zDir)] = true;
              currentHitPlane = channels[bestChanIdx].Plane();
              currentHitCell = channels[bestChanIdx].Cell();
              float currentPE = channels[bestChanIdx].GetHit()->PE();
              iCurrentTrackHit = bestChanIdx;
              if(nhits == 1){
                layerState[0] = xPredict[0];
                layerState[1] = xPredict[1];
                layerState[2] = xPredict[2];
                layerP[0] = cPredict[0];
                layerP[1] = cPredict[1];
                layerP[2] = cPredict[2];
                peWeight = currentPE;
              }
              else{
                layerState[0] = (layerState[0]*peWeight+xPredict[0]*currentPE)/(peWeight+currentPE);
                layerState[1] = (layerState[1]*peWeight+xPredict[1]*currentPE)/(peWeight+currentPE);
                layerState[2] = (layerState[2]*peWeight+xPredict[2]*currentPE)/(peWeight+currentPE);
                layerP[0] = (layerP[0]*peWeight+cPredict[0]*currentPE)/(peWeight+currentPE);
                layerP[1] = (layerP[1]*peWeight+cPredict[1]*currentPE)/(peWeight+currentPE);
                layerP[2] = (layerP[2]*peWeight+cPredict[2]*currentPE)/(peWeight+currentPE);
                peWeight+=currentPE;
              }
              if(zProp){
                // update dir
                dir[0] = layerState[2]/sqrt(1.0+layerState[2]*layerState[2]);
                dir[1] = 1.0/sqrt(1.0+layerState[2]*layerState[2]);
                if(nhits == 1){
                  int newLayerWindowlow = currentHitCell-fGapAllowed;
                  int newLayerWindowhigh = currentHitCell+fGapAllowed;
                  if(fBadChannels){
                    while(newLayerWindowlow > 0 &&
                          (fbadc->IsBad(currentHitPlane,newLayerWindowlow)
                           || fbadc->IsLowEfficiency(currentHitPlane,newLayerWindowlow))){
                      --newLayerWindowlow;
                    }
                    while(newLayerWindowhigh < NCells[fView] && 
                          (fbadc->IsBad(currentHitPlane,newLayerWindowhigh)
                           || fbadc->IsLowEfficiency(currentHitPlane,newLayerWindowhigh))){
                      ++newLayerWindowhigh;
                    }
                  }
                  layerWindowlow = newLayerWindowlow;
                  layerWindowhigh = newLayerWindowhigh;
                }
                else{
                  if((currentHitCell - layerWindowlow) < (layerWindowhigh - currentHitCell)){
                    if(!fBadChannels){layerWindowlow = currentHitCell-fGapAllowed;}
                    else{                 
                      // increase layer window by 1 cell until no more bad channels found
                      int newLayerWindowlow = currentHitCell-fGapAllowed;
                      while(newLayerWindowlow > 0 &&
                            (fbadc->IsBad(currentHitPlane,newLayerWindowlow) 
                             || fbadc->IsLowEfficiency(currentHitPlane,newLayerWindowlow)) ){
                        --newLayerWindowlow;
                      }
                      layerWindowlow = newLayerWindowlow;
                    }
                  }
                  else{
                    if(!fBadChannels){layerWindowhigh = currentHitCell+fGapAllowed;}
                    else{
                      // increase layer window by 1 cell until no more bad channels found
                      int newLayerWindowhigh = currentHitCell+fGapAllowed;
                      while(newLayerWindowhigh < NCells[fView] &&
                            (fbadc->IsBad(currentHitPlane,newLayerWindowhigh)
                             || fbadc->IsLowEfficiency(currentHitPlane,newLayerWindowhigh)) ){
                        ++newLayerWindowhigh;
                      }
                      layerWindowhigh = newLayerWindowhigh;
                    }
                  }
                }
              } // end of if zProp
              else{
                // update dir
                dir[0] = 1.0/sqrt(1.0+layerState[2]*layerState[2]);
                dir[1] = layerState[2]/sqrt(1.0+layerState[2]*layerState[2]);
                if(nhits == 1){
                  if(((state[2] <= 0 && zDir < 0) || (state[2] >= 0 && zDir  > 0))){
                    int newLayerWindowlow = currentHitPlane;
                    int newLayerWindowhigh = fgeom->NextPlaneInView(currentHitPlane,+1);
                    layerWindowlow = newLayerWindowlow;
                    if(newLayerWindowhigh == geo::kPLANE_NOT_FOUND){newLayerWindowhigh = currentHitPlane;}
                    else{
                      if(fBadChannels){
                        int newlayerWindowhightest = newLayerWindowhigh;
                        while(newlayerWindowhightest != geo::kPLANE_NOT_FOUND &&
                              (fbadc->IsBad(newlayerWindowhightest,currentHitCell)
                               || fbadc->IsLowEfficiency(newlayerWindowhightest,currentHitCell)) ){
                          newlayerWindowhightest = fgeom->NextPlaneInView(newLayerWindowhigh,+1);
                          if(newlayerWindowhightest != geo::kPLANE_NOT_FOUND){newLayerWindowhigh = newlayerWindowhightest;}
                        }
                      }
                    }
                    layerWindowhigh = newLayerWindowhigh;
                  }
                  else{
                    int newLayerWindowlow = fgeom->NextPlaneInView(currentHitPlane,-1);
                    int newLayerWindowhigh = currentHitPlane;
                    layerWindowhigh = newLayerWindowhigh;
                    if(newLayerWindowlow == geo::kPLANE_NOT_FOUND){newLayerWindowlow = currentHitPlane;}
                    else{
                      if(fBadChannels){
                        int newlayerWindowlowtest = newLayerWindowlow;
                        while(newlayerWindowlowtest != geo::kPLANE_NOT_FOUND &&
                              (fbadc->IsBad(newlayerWindowlowtest,currentHitCell)
                               || fbadc->IsLowEfficiency(newlayerWindowlowtest,currentHitCell)) ){
                          newlayerWindowlowtest = fgeom->NextPlaneInView(newLayerWindowlow,-1);
                          if(newlayerWindowlowtest != geo::kPLANE_NOT_FOUND){newLayerWindowlow = newlayerWindowlowtest;}
                        }
                      }
                    }
                    layerWindowlow = newLayerWindowlow;
                  }
                }
                else{
                  if(((state[2] <= 0 && zDir < 0) || (state[2] >= 0 && zDir  > 0))){
                    layerWindowlow = currentHitPlane;
                    if(!fBadChannels){
                      layerWindowhigh = fgeom->NextPlaneInView(currentHitPlane,1);
                      if(layerWindowhigh == geo::kPLANE_NOT_FOUND){layerWindowhigh = currentHitPlane;}
                    }
                    else{
                      int newLayerWindowhigh = fgeom->NextPlaneInView(currentHitPlane,1);
                      if(newLayerWindowhigh == geo::kPLANE_NOT_FOUND){newLayerWindowhigh = currentHitPlane;}
                      else{
                        int newLayerWindowhightest = newLayerWindowhigh; 
                        while(newLayerWindowhightest != geo::kPLANE_NOT_FOUND && 
                              (fbadc->IsBad(newLayerWindowhightest,currentHitCell)
                               || fbadc->IsLowEfficiency(newLayerWindowhightest,currentHitCell)) ){
                          newLayerWindowhightest = fgeom->NextPlaneInView(newLayerWindowhigh,+1);
                          if(newLayerWindowhightest != geo::kPLANE_NOT_FOUND){newLayerWindowhigh = newLayerWindowhightest;}
                        }
                      }
                      layerWindowhigh = newLayerWindowhigh;
                    }
                  }
                  else{
                    layerWindowhigh = currentHitPlane;
                    if(!fBadChannels){
                      layerWindowlow = fgeom->NextPlaneInView(currentHitPlane,-1);
                      if(layerWindowlow == geo::kPLANE_NOT_FOUND){layerWindowlow = currentHitPlane;}
                    }
                    else{
                      int newLayerWindowlow = fgeom->NextPlaneInView(currentHitPlane,-1);
                      if(newLayerWindowlow == geo::kPLANE_NOT_FOUND){newLayerWindowlow = currentHitPlane;}
                      else{
                        int newLayerWindowlowtest = newLayerWindowlow; 
                        while(newLayerWindowlowtest != geo::kPLANE_NOT_FOUND && 
                              (fbadc->IsBad(newLayerWindowlowtest,currentHitCell)
                               || fbadc->IsLowEfficiency(newLayerWindowlowtest,currentHitCell)) ){
                          newLayerWindowlowtest = fgeom->NextPlaneInView(newLayerWindowlow,-1);
                          if(newLayerWindowlowtest != geo::kPLANE_NOT_FOUND){newLayerWindowlow = newLayerWindowlowtest;}
                        }
                      }
                      layerWindowlow = newLayerWindowlow;
                    }
                  }
                }
              } // end of else of if(zProp) else
            } // end of if(deltaChi <= maxDeltaChi)
            else{ layerTrackHits[(bestHitIndex/zDir)] = true; }
            // now we know everything about this hit delete it from our knowledge bank
          } // if(bestHitIndex != 9999)
          else{ doneAdding = true;}
        } // while(!doneAdding)

        // update the filtered state and covariance matrix based on added hits
        if(nhits>0){
          // Add track information to output variables
          state[0] = layerState[0];
          state[1] = layerState[1];
          state[2] = layerState[2];
          errorCov[0] = layerP[0];
          errorCov[1] = layerP[1];
          errorCov[2] = layerP[2];
          
          // update track and find next expected hit layer
          if(zProp){
            if(fLayerHit < iLayerHit){ std::swap(iLayerHit,fLayerHit); }
            for(int ihit = iLayerHit; ihit<=fLayerHit; ++ihit){
              if(trkHits[ihit]){
                trkTrajs[ihit][0] = state[0];
                trkTrajs[ihit][1] = state[1];
                trkDirs[ihit][0] = dir[0];
                trkDirs[ihit][1] = dir[1];
              }
            }
            expectedHitPlane = fgeom->NextPlaneInView(currentHitPlane,zDir);
            if(expectedHitPlane == geo::kPLANE_NOT_FOUND){ done = true; }
          }
          else{
            if(fLayerHit < iLayerHit){ std::swap(iLayerHit,fLayerHit); }
            for(int ihit = iLayerHit; ihit<=fLayerHit; ++ihit){
              if(trkHits[ihit]){
                trkTrajs[ihit][0] = state[1];
                trkTrajs[ihit][1] = state[0];
                trkDirs[ihit][0] = dir[0];
                trkDirs[ihit][1] = dir[1];
              }
            }
            expectedHitCell = currentHitCell+zDir;
            if(expectedHitCell < 0 || expectedHitCell > NCells[fView] ){ done = true; }
          }
          if(iNextHit > (fNChans-1) || iNextHit < 0){ done = true; }
          // reset number of misses to zero
          misses = 0;
        } // if(nhits > 0)
        else{
          // check if there could possibly be anymore hits to add to the track
          if(iNextHit > (fNChans-1) || iNextHit < 0){ done = true; }
          else{
            short nextHitPlane = channels[iNextHit].Plane();
            short nextHitCell = channels[iNextHit].Cell();
            // project the last trajectory point to the beginning of the expected hit plane
            double currtraj[2]; // [0] = v, [1] = z
            //point on next cell that track will hit
            double nextpt[3]; // [0] = x, [1] = y, [2] = z
            channels[iNextHit].GetCenter(nextpt); 
            if(zProp){
              // change current trajectory to just outside of the current hit plane
              double offset = zDir*(fCellL+2.0);
              currtraj[1] = state[1] + offset;
              currtraj[0] = state[2]*offset + state[0];
              // change next z position to the z location just outside of the next hit plane
              nextpt[2]-=offset;
              nextpt[fView] = state[2]*(nextpt[2]-currtraj[1])+currtraj[0];
              // check if this point is out of the detector
              if(fView == 0 && abs(nextpt[fView]) > fgeom->DetHalfWidth()){ done = true; }
              else if(fView == 1 && abs(nextpt[fView]) > fgeom->DetHalfHeight()){ done = true; }
            }
            else{
              // change current trajectory to just outside of the current hit cell
              double offset = zDir*(fCellW+2.0);
              currtraj[0] = state[1] + offset;
              currtraj[1] = state[2]*offset + state[0];
              // change next hit position to just outside of the next hit cell
              nextpt[fView]-=offset;
              nextpt[2] = state[2]*(nextpt[fView]-currtraj[0])+currtraj[1];
              if(nextpt[2] < 0 || nextpt[2] > fgeom->DetLength()){ done = true; }
            }
            misses+=kgeo.CountMissedCellsOnLine(fView,currtraj[0],currtraj[1],nextpt[fView],nextpt[2],fGapAllowed);
            if(misses > maxMisses){done = true;}
            else{
              expectedHitCell = nextHitCell;
              expectedHitPlane = nextHitPlane;
            }
          }
        } // end of else for checking if more hits exist in slice past this one
      } // end if in samelayertest
      else{
        // check how many cells this track goes through before getting to the next hit plane
        double currtraj[2]; // [0] = v, [1] = z
        //point on next cell that track will hit
        double nextpt[3]; // [0] = x, [1] = y, [2] = z
        channels[iNextHit].GetCenter(nextpt);
        if(zProp){
          // change current trajectory to just outside of the current hit plane
          double offset = zDir*(fCellL+2.0);
          currtraj[1] = state[1] + offset;
          currtraj[0] = state[2]*offset + state[0];
          // change next z position to the z location just outside of the next hit plane
          nextpt[2]-=offset;
          nextpt[fView] = state[2]*(nextpt[2]-currtraj[1])+currtraj[0];
          // check if this point is out of the detector
          if(fView == 0 && abs(nextpt[fView]) > fgeom->DetHalfWidth()){ done = true; }
          else if(fView == 1 && abs(nextpt[fView]) > fgeom->DetHalfHeight()){ done = true; }
        }
        else{
          // change current trajectory to just outside of the current hit cell
          double offset = zDir*(fCellW+2.0);
          currtraj[0] = state[1] + offset;
          currtraj[1] = state[2]*offset + state[0];
          // change next hit position to just outside of the next hit cell
          nextpt[fView]-=offset;
          nextpt[2] = state[2]*(nextpt[fView]-currtraj[0])+currtraj[1];
          if(nextpt[2] < 0 || nextpt[2] > fgeom->DetLength()){ done = true; }
        }
        misses+=kgeo.CountMissedCellsOnLine(fView,currtraj[0],currtraj[1],nextpt[fView],nextpt[2],fGapAllowed);
        if(misses > maxMisses){done = true;}
        else{
          expectedHitPlane = nextHitPlane;
          expectedHitCell = nextHitCell;
        }
        
      } //end of else in samelayertest
      
    } // end of while loop over all slicehits

    return chi;
  }
  
  ///////////////////////////////////////////////////////////////////
  void KalmanTrack::RemoveSameTracks(std::vector< std::vector<bool> > &trkHits,
                                     std::vector< std::vector<std::array<double, 2> > > &trkTrajs,
                                     std::vector< std::vector<std::array<double, 2> > >&trkDirs,std::vector<double> &chi2,
                                     std::vector< std::vector<double> > &pfiltState, std::vector<bool> &zProps,
                                     std::vector<int> &nTrkHits, std::vector<bool> &ignoreTrk)
  {
    int nTracks = trkHits.size();
    if(nTracks == 0) return;
    // set up a vector to track whether or not an input track is the same as another track.
    bool sameTrack[nTracks];
    for(int st = 0; st < nTracks; ++st) sameTrack[st] = false;

    int nSame = 0;
    for(int iTrack = 0; iTrack < nTracks; iTrack++){
      //define a variable to track whether this track is the same as another or not.
      bool inTrackDone = false;
      if(sameTrack[iTrack] || ignoreTrk[iTrack]){inTrackDone = true; }
      // set a counter of the track to compare the input track to.
      int cTrack = iTrack+1;
      // loop over all other tracks
      while(cTrack < nTracks && !inTrackDone){
        if(!sameTrack[cTrack] && !ignoreTrk[cTrack]){
          // assume these are the same tracks until proven otherwise
          bool same = true;
          // first check if these are both vertical or horizontally propogated
          if(zProps[iTrack] == zProps[cTrack] && nTrkHits[iTrack] == nTrkHits[cTrack]){
            // compare if the same trkHits
            for(int ihit = 0; ihit < fNChans; ++ihit){
              if(trkHits[iTrack][ihit] != trkHits[cTrack][ihit]){
                same = false;
                break;
              }
            } 
          }
          else{ same = false; }
          // if the same track keep the one with the smaller chi2
          if(same){
            if(chi2[iTrack] > chi2[cTrack]){
              sameTrack[iTrack] = true;
              ignoreTrk[iTrack] = true;
              // move onto next input track if this track is to be deleted
              inTrackDone = true;
            }
            else{ 
              sameTrack[cTrack] = true;
              ignoreTrk[cTrack] = true;
            }
          }

        }       
        cTrack++; // move on to next track for comparison
        //end loop over the comparison tracks
      }
      if(sameTrack[iTrack]){ ++nSame; }
      //end loop over track to remove
    }
    
  }

  ///////////////////////////////////////////////////////////////////
  void    KalmanTrack::RemoveHitsFromSignal(const rb::Track & track)
  {

    /// Following is added for new hit storage method
    std::vector<trk::LocatedChan> newfSignalChan;   
    // loop over all the signal hits
    for(int i = 0; i<fNChans; ++i){
      // get current hit
      art::Ptr<rb::CellHit> chanhit = fSliceChans[i].GetHit();
      // define a boolean to track if signal hit is in track hit
      bool inTrack = false;
      for(unsigned int j = 0; j < track.NCell(); ++j){
        // get track hit
        art::Ptr<rb::CellHit> thit = track.Cell(j);
        if(chanhit == thit){
          inTrack = true;
          break;
        }
      }
      if(!inTrack){ newfSignalChan.push_back(fSliceChans[i]); }
    }
    fNChans = newfSignalChan.size();
    fSliceChans.resize(fNChans);
    fSliceChans = newfSignalChan;
    
  }
  
  //////////////////////////////////////////////////////////////////
  double KalmanTrack::FilterOnly(const std::vector<bool> & trkHits, std::vector<std::array<double, 2> > &trkTrajs,
                                 std::vector<std::array<double, 2> > &trkDirs, std::vector<double> &pEstimate,
                                 int zDir, bool zProp, int &newOutlierPos)
  {
    std::vector<trk::LocatedChan> & channels = zProp? fSliceChans: fSliceChansCellSort;
    double chi = 0.0;
    double cellVarW = fCellW*fCellW;
    double cellVarL = fCellL*fCellL;
    double xMinus[3];
    double pMinus[3];
    double K[2];
    int nhits = CountHits(trkHits);
    // find the first hit in the track
    int firstHit = GetFirstHit(trkHits);
    // get the track direction and starting point
    const std::array<double, 2> & start = trkTrajs[firstHit];
    std::array<double, 2> dir = trkDirs[firstHit];

    // make a vector to store the order hits in (each entry holds a vector of track hits for the given layer)
    double trajPointsFor[nhits][3];
    double pFor[nhits][4]; 
    double trajPointsBack[nhits][3]; 
    double pBack[nhits][4]; 
    double pos;
    double slope;
    double otherpos;
    // sort all the hits into the order to be added to the track
    if(zProp){
      pos = start[0];
      otherpos = start[1];
      if(dir[1] == 0){ dir[1] = 0.0001; }
      slope = dir[0]/dir[1];
    }
    else{
      pos = start[1];
      otherpos = start[0];
      if(dir[0] == 0){ dir[0] = 0.0001; }
      slope = dir[1]/dir[0];
    }

    std::vector<const rb::CellHit * > trackHitsNew(nhits);
    double hitCentersNew[nhits][3];
    int ihit = 0;
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){
        trackHitsNew[ihit] = channels[i].GetHit().get();
        channels[i].GetCenter(hitCentersNew[ihit]);
        ++ihit;
      }
    }

    // The track is only one plane, no need to filter just choose the cell centers.
    if(!zProp && (trackHitsNew[0]->Plane() == trackHitsNew[nhits-1]->Plane())){
      // Loop over the trkHits and set trajectories and dirs
      int ihit = 0;
      for(int i = 0; i < fNChans; ++i){
        if(trkHits[i]){
          if(zDir > 0){ trkDirs[i][0] = 1.0; }
          else{ trkDirs[i][0] = -1.0; }
          trkDirs[i][1] = 0.0;
          trkTrajs[i][0] = hitCentersNew[ihit][fView];
          trkTrajs[i][1] = hitCentersNew[ihit][2];
          ++ihit;
        }
      }
      return chi; 
    }

    // define the measurement and system covariance matrices;
    double R = cellVarW/3.0; 
    if(!zProp){ R = cellVarL/3.0; }
    double Q[3];
    Q[2] = fSysVariance;
    
    // filter forward first
    int iCurrentHit = 0;
    int iNextHit = 0;
    int iTraj = 0;
    double delta;
    // second layer(plane/cell) of hits is at the plane of iNextHit
    while(iCurrentHit<nhits && iNextHit<nhits){
      int searchPlane = trackHitsNew[iCurrentHit]->Plane();
      int searchCell = trackHitsNew[iCurrentHit]->Cell();
      int nhitsAdded = 0;
      double peTot = 0.0;
      if(zProp){
        for(int i = iCurrentHit; i<nhits;++i){
          if(trackHitsNew[i]->Plane() == searchPlane){ 
            ++nhitsAdded; 
            ++iNextHit;
            peTot+= trackHitsNew[i]->PE();
          } 
          else{ break; }
        }
      }
      else{
        for(int i = iCurrentHit; i<nhits; ++i){
          if(trackHitsNew[i]->Cell() == searchCell){
            ++nhitsAdded;
            ++iNextHit;
            peTot+= trackHitsNew[i]->PE();
          }
          else{ break; }
        }
      }
      // loop over all the hits in the layer to find the state estimate of this state.
      double otherposOld = otherpos;
      double posOld = pos;
      double slopeOld = slope;
      pos = 0;
      otherpos = 0;
      slope = 0;
      const double pEstimateOld[3] = { pEstimate[0], pEstimate[1], pEstimate[2] };
      pEstimate[0] = 0;
      pEstimate[1] = 0;
      pEstimate[2] = 0;
      double z[2];
      while(iCurrentHit<iNextHit){
        // Get the best hit position from the track hit
        if(zProp){
          z[0] = hitCentersNew[iCurrentHit][fView];
          z[1] = hitCentersNew[iCurrentHit][2]; 
        }
        else{
          z[0] = hitCentersNew[iCurrentHit][2];
          z[1] = hitCentersNew[iCurrentHit][fView];
        }
        delta = z[1] - otherposOld;

        // update Q and R with new delta
        Q[0] = delta*delta*Q[2];
        Q[1] = delta*Q[2];
        
        // Predict next state
        xMinus[0] = posOld + delta*slopeOld;
        xMinus[2] = slopeOld;
        pMinus[0] = pEstimateOld[0] + 2.0*delta*pEstimateOld[1] + delta*delta*pEstimateOld[2] + Q[0];
        pMinus[1] = pEstimateOld[1] + delta*pEstimateOld[2] + Q[1];
        pMinus[2] = pEstimateOld[2] + Q[2];
        
        //calculate filter at this layer.
        double denom = (pMinus[0]+R);
        K[0] = pMinus[0]/denom;
        K[1] = pMinus[1]/denom;
        
        const double weight = trackHitsNew[iCurrentHit]->PE()/peTot;
        pos+= weight*(xMinus[0] + K[0]*(z[0]-xMinus[0]));
        otherpos+= weight*z[1];
        otherpos = z[1];
        slope+= weight*(xMinus[2] + K[1]*(z[0]-xMinus[0]));
        pEstimate[0]+= weight*(pMinus[0]*R/denom);
        pEstimate[1]+= weight*(pMinus[1]*R/denom);
        pEstimate[2]+= weight*((pMinus[2]*pMinus[0]-pMinus[1]*pMinus[1]+pMinus[2]*R)/denom);
        ++iCurrentHit;
      } // end of while loop over iCurrentHit
      while(iTraj<iNextHit){
        // add the trajectory point and set the new direction at this layer
        if(zProp){
          trajPointsFor[iTraj][0] = pos;
          trajPointsFor[iTraj][1] = otherpos;
        }
        else{
          trajPointsFor[iTraj][0] = otherpos;
          trajPointsFor[iTraj][1] = pos;
        }
        trajPointsFor[iTraj][2] = slope;
        pFor[iTraj][0] = pEstimate[0];
        pFor[iTraj][3] = pEstimate[2];
        ++iTraj;
      }
      iCurrentHit = iNextHit;
      iTraj = iNextHit;
    } //end of while loop over hits in the track

    // filter backward
    iCurrentHit = nhits-1;
    iNextHit = nhits-1;
    iTraj = nhits-1; 
    // second layer(plane/cell) of hits is at the plane of iNextHit
    while(iCurrentHit>=0 && iNextHit>=0){
      int searchPlane = trackHitsNew[iCurrentHit]->Plane();
      int searchCell = trackHitsNew[iCurrentHit]->Cell();
      int nhitsAdded = 0;
      double peTot = 0.0;
      if(zProp){
        for(int i = iCurrentHit; i>=0;--i){
          if(trackHitsNew[i]->Plane() == searchPlane){
            ++nhitsAdded; 
            --iNextHit;
            peTot+= trackHitsNew[i]->PE();
          }
          else{ break; } 
        }
      }
      else{
        for(int i = iCurrentHit; i>=0; --i){
          if(trackHitsNew[i]->Cell() == searchCell){
            ++nhitsAdded;
            --iNextHit;
            peTot+= trackHitsNew[i]->PE();
          }
          else{ break; }
        }
      }
      // loop over all the hits in the layer to find the state estimate of this state.
      double otherposOld = otherpos;
      double posOld = pos;
      double slopeOld = slope;
      pos = 0;
      otherpos = 0;
      slope = 0;
      const double pEstimateOld[3] = { pEstimate[0], pEstimate[1], pEstimate[2] };
      pEstimate[0] = 0;
      pEstimate[1] = 0;
      pEstimate[2] = 0;
      double z[2];
      double weight = 1.0/((double)nhitsAdded);
      while(iCurrentHit>iNextHit){
        // Get the best hit position from the track hit
        if(zProp){
          z[0] = hitCentersNew[iCurrentHit][fView];
          z[1] = hitCentersNew[iCurrentHit][2]; 
        }
        else{
          z[0] = hitCentersNew[iCurrentHit][2];
          z[1] = hitCentersNew[iCurrentHit][fView];
        }
        delta = z[1] - otherposOld;

        // update Q and R with new delta
        Q[0] = delta*delta*Q[2]; 
        Q[1] = delta*Q[2];
        
        // Predict next state
        xMinus[0] = posOld + delta*slopeOld;
        xMinus[2] = slopeOld;
        pMinus[0] = pEstimateOld[0] + 2.0*delta*pEstimateOld[1] + delta*delta*pEstimateOld[2] + Q[0];
        pMinus[1] = pEstimateOld[1] + delta*pEstimateOld[2] + Q[1];
        pMinus[2] = pEstimateOld[2] + Q[2];
        
        //calculate filter at this layer.
        double denom = (pMinus[0]+R);
        K[0] = pMinus[0]/denom;
        K[1] = pMinus[1]/denom;
        
        weight = trackHitsNew[iCurrentHit]->PE()/peTot;
        pos+= weight*(xMinus[0] + K[0]*(z[0]-xMinus[0]));
        otherpos+= weight*z[1];
        otherpos = z[1];
        slope+= weight*(xMinus[2] + K[1]*(z[0]-xMinus[0]));
        pEstimate[0]+= weight*(pMinus[0]*R/denom);
        pEstimate[1]+= weight*(pMinus[1]*R/denom);
        pEstimate[2]+= weight*((pMinus[2]*pMinus[0]-pMinus[1]*pMinus[1]+pMinus[2]*R)/denom);
        --iCurrentHit;
      } // end of while loop over iCurrentHit
      // add the trajectory points
      while(iTraj>iNextHit){
        if(zProp){
          trajPointsBack[iTraj][0] = pos;
          trajPointsBack[iTraj][1] = otherpos;
        }
        else{
          trajPointsBack[iTraj][0] = otherpos;
          trajPointsBack[iTraj][1] = pos;
        }
        trajPointsBack[iTraj][2] = slope;
        pBack[iTraj][0] = pEstimate[0];
        pBack[iTraj][3] = pEstimate[2];
        --iTraj;
      }
      iCurrentHit = iNextHit;
      iTraj = iNextHit;
    } //end of while loop over hits in the track

    // set trajectory points and calculate the chi^2 of each hit and keep track of the worst outlier
    double worstchi = 0;
    ihit = 0;
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){
        double trajWeight[2] = {0.5, 0.5};
        double dirWeight[2] = {0.5, 0.5};
        double p0;
        double p3;
        double deltaChi = 0.0;
        if(zProp){
          if(trackHitsNew[ihit]->Plane() == trackHitsNew[0]->Plane()){
            trajWeight[0] = 0.0, trajWeight[1] = 1.0;
            dirWeight[0] = 0.0, dirWeight[1] = 1.0;
            p0 = pBack[ihit][0];
            p3 = pBack[ihit][3];
          }
          else if(trackHitsNew[ihit]->Plane() == trackHitsNew[(nhits-1)]->Plane()){ 
            trajWeight[0] = 1.0, trajWeight[1] = 0.0;
            dirWeight[0] = 1.0, dirWeight[1] = 0.0;
            p0 = pFor[ihit][0];
            p3 = pFor[ihit][3];
          }
          else{ 
            double trajdenom = 1.0/sqrt(pFor[ihit][0]) + 1.0/sqrt(pBack[ihit][0]);
            trajWeight[0] = (1.0/sqrt(pFor[ihit][0]))/trajdenom, trajWeight[1] = (1.0/sqrt(pBack[ihit][0]))/trajdenom;
            double dirdenom = 1.0/sqrt(pFor[ihit][3]) + 1.0/sqrt(pBack[ihit][3]);
            dirWeight[0] = (1.0/sqrt(pFor[ihit][3]))/dirdenom, dirWeight[1] = (1.0/sqrt(pBack[ihit][3]))/dirdenom;
            p0 = 2.0/(trajdenom*trajdenom);
            p3 = 2.0/(dirdenom*dirdenom);
          }
          trkTrajs[i][0] = trajWeight[0]*trajPointsFor[ihit][0] + trajWeight[1]*trajPointsBack[ihit][0];
          trkTrajs[i][1] = 0.5*trajPointsFor[ihit][1] + 0.5*trajPointsBack[ihit][1];
          slope = dirWeight[0]*trajPointsFor[ihit][2] + dirWeight[1]*trajPointsBack[ihit][2];
          double sl2 = slope*slope;
          trkDirs[i][0] = slope/sqrt(1.0+sl2);
          trkDirs[i][1] = 1.0/sqrt(1.0+sl2);
          double r2 = (hitCentersNew[ihit][fView]-trkTrajs[i][0])*(hitCentersNew[ihit][fView]-trkTrajs[i][0]);
          deltaChi+= (r2*(1.0+sl2)*(1.0+sl2))/(((1.0+sl2)*(1.0+sl2)*(cellVarW/3.0+sl2*cellVarL/3.0+p0))+r2*sl2*p3/2.0);
        }
        else{
          if(trackHitsNew[ihit]->Cell() == trackHitsNew[0]->Cell()){
            trajWeight[0] = 0.0, trajWeight[1] = 1.0;
            dirWeight[0] = 0.0, dirWeight[1] = 1.0;
            p0 = pBack[ihit][0];
            p3 = pBack[ihit][3];
          }
          else if(trackHitsNew[ihit]->Cell() == trackHitsNew[(nhits-1)]->Cell()){ 
            trajWeight[0] = 1.0, trajWeight[1] = 0.0;
            dirWeight[0] = 1.0, dirWeight[1] = 0.0;
            p0 = pFor[ihit][0];
            p3 = pFor[ihit][3];
          }
          else{
            double trajdenom = 1.0/sqrt(pFor[ihit][0]) + 1.0/sqrt(pBack[ihit][0]);
            trajWeight[0] = (1.0/sqrt(pFor[ihit][0]))/trajdenom, trajWeight[1] = (1.0/sqrt(pBack[ihit][0]))/trajdenom;
            double dirdenom = 1.0/sqrt(pFor[ihit][3]) + 1.0/sqrt(pBack[ihit][3]);
            dirWeight[0] = (1.0/sqrt(pFor[ihit][3]))/dirdenom, dirWeight[1] = (1.0/sqrt(pBack[ihit][3]))/dirdenom;
            p0 = 2.0/(trajdenom*trajdenom);
            p3 = 2.0/(dirdenom*dirdenom);
          }
          trkTrajs[i][0] = 0.5*trajPointsFor[ihit][0] + 0.5*trajPointsBack[ihit][0];
          trkTrajs[i][1] = trajWeight[0]*trajPointsFor[ihit][1] + trajWeight[1]*trajPointsBack[ihit][1];
          slope = dirWeight[0]*trajPointsFor[ihit][2] + dirWeight[1]*trajPointsBack[ihit][2];
          double sl2 = slope*slope;
          trkDirs[i][0] = 1.0/sqrt(1.0+sl2);
          trkDirs[i][1] = slope/sqrt(1.0+sl2);
          double r2 = (hitCentersNew[ihit][2]-trkTrajs[i][1])*(hitCentersNew[ihit][2]-trkTrajs[i][1]);
          deltaChi+= (r2*(1+sl2)*(1+sl2))/(((1+sl2)*(1+sl2)*(cellVarL/3.0+sl2*cellVarW/3.0+p0))+r2*sl2*p3/2.0);
        }
        chi+=deltaChi;
        // check if this is an outlier
        if(deltaChi>fDeltaChiAllowed){
          if(deltaChi>worstchi){
            worstchi = deltaChi;
            newOutlierPos = i;
          }
        }
        chi+=deltaChi;
        ++ihit;
      } // if hit is on track, ihit
    } // end of loop over channels, i

    return chi;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  void   KalmanTrack::CheckTrack(KalmanGeoHelper& kgeo,
                                 std::vector<bool> &trkHits, std::vector<std::array<double, 2> > trkTrajs,
                                 bool zProp, std::vector<bool> &newTrkHits)
  {
    newTrkHits.resize(fNChans);
    std::vector<trk::LocatedChan> & channels = zProp? fSliceChans : fSliceChansCellSort;
    // step through this track and make sure that there aren't any gaps larger than allowed
    // If gaps exist break track into two seperate pieces
    double minProb = fMinGapProb;    

    // first get a list of hits that the track has in it
    int nhits = CountHits(trkHits);

    std::vector<std::array<double, 2> > trjPointsNew(nhits);
    std::vector<int> trkHitsIdx(nhits);
    int ihit = 0;
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){
        trjPointsNew[ihit] = trkTrajs[i];
        trkHitsIdx[ihit] = i;
        ++ihit;
      }
    }

    unsigned int lastPlane = 10000; // last plane of a missed hit
    unsigned int lastCell = 10000; // last cell of a missed hit
    unsigned int lastHitPlane = 10000; // last plane with a hit in it
    unsigned int lastHitCell = 10000; // last cell with a hit in it
    bool posSlope = true; // does this track have a positive slope, need this for sorting
    bool spillover = false;  // does a gap exist between last trajectory point to this point that needs to be added
    int spillovergap = 0; // last gap in cells that spills over into current gap
    double spilloverprob = 1.0; // last gap in probability that spills over into current gap
    int allMissed = 0; // total number of missed cells
    int biggestGap = 0; // biggest gap in the track

    // loop over all trajectory points
    for(int itraj = 0; itraj < (nhits-1); ++itraj){

      // check if this trajectory point is the same as the next, can't possibly have gap in these cases
      if(trjPointsNew[itraj][0] == trjPointsNew[itraj+1][0] && trjPointsNew[itraj][1] == trjPointsNew[itraj+1][1]){ continue; } 

      bool sort = false;
      std::vector<geo::OfflineChan> missedCells; // missed cells
      std::vector<double> distance; // 2d distance in missed cells

      // total missed cells between this and next trajectory point
      int totMissed = kgeo.CountMissedCellsOnLine(fView,trjPointsNew[itraj][0],trjPointsNew[itraj][1],
                                                trjPointsNew[itraj+1][0],trjPointsNew[itraj+1][1],missedCells,distance);
      // If didn't go through any cells, no gaps to check and nothing to update
      if(totMissed == 0){ continue; }
      
      // order the hits in the order they appear along the track path, only neccessary if slope is negative
      if(trjPointsNew[itraj][1] != trjPointsNew[itraj+1][1] && 
         ((trjPointsNew[itraj+1][0]-trjPointsNew[itraj][0])/(trjPointsNew[itraj+1][1]-trjPointsNew[itraj][1])) < 0){ sort = true; }

      if(sort){
        // already ordered by plane, now do it by cell within the plane
        posSlope = false;
        std::vector<geo::OfflineChan> orderedMissedCells(totMissed);
        std::vector<double> orderedDistance(totMissed);
        unsigned int firstCellIndex = 0;
        unsigned int lastCellIndex = 0;
        unsigned int fillCellIndex = 0;
        unsigned int plane = missedCells[0].Plane();
        for(int iCell = 0; iCell<totMissed;++iCell){
          if(missedCells[iCell].Plane() != (int)plane || (int)iCell == totMissed-1){
            if(iCell == totMissed-1 && missedCells[iCell].Plane() == (int)plane){lastCellIndex = iCell;}
            // first fill the ordered cells vector
            for(int fillCell = (int) lastCellIndex; fillCell>= (int) firstCellIndex;--fillCell){
              orderedMissedCells[fillCellIndex] = missedCells[fillCell];
              orderedDistance[fillCellIndex] = distance[fillCell];
              ++fillCellIndex;
            }
            if(iCell == totMissed-1 && missedCells[iCell].Plane() != (int)plane){
              orderedMissedCells[totMissed-1] = missedCells[totMissed-1];
              orderedDistance[totMissed-1] = distance[totMissed-1];
              lastCellIndex = iCell;
              if(missedCells[iCell].Plane() != plane){firstCellIndex = iCell;}
            }
            // update the counters and plane
            firstCellIndex = iCell;
            lastCellIndex = iCell;
            plane = missedCells[iCell].Plane();
          }
          else{ lastCellIndex = iCell; }
        }
        missedCells = orderedMissedCells;
        distance = orderedDistance;
      }

      int currentgap = 0; // current gap in cells between this trajectory point and the next
      double currprob = 1.0; // current probability of cell gap between this and next trajectory point

      // if there is a spillover add it to the current gap
      if(spillover){
        currentgap+=spillovergap;
        currprob = spilloverprob;
      }

      // now the cells are ordered so we can look for gaps
      for(int iCell = 0; iCell < totMissed; ++iCell){

        bool ontrack = false; // Is this cell a hit on the track?
        bool inbadc = false; // Is this cell a bad channel?

        // check if the track has a hit in this cell
        for(int ihit = 0; ihit < nhits; ++ihit){
          if(channels[trkHitsIdx[ihit]].Plane() == missedCells[iCell].Plane() &&
             channels[trkHitsIdx[ihit]].Cell() == missedCells[iCell].Cell()){
            ontrack = true;
            lastHitPlane = missedCells[iCell].Plane();
            lastHitCell = missedCells[iCell].Cell();
            break;
          }
        }
        
        // check if this cell was a bad channel, if so don't count against the track, but also don't reset gaps
        if(fBadChannels && !ontrack){
          if(fbadc->IsBad(missedCells[iCell].Plane(),missedCells[iCell].Cell()) ||
             fbadc->IsLowEfficiency(missedCells[iCell].Plane(),missedCells[iCell].Cell()) ){ inbadc = true; }
        }

        // if hit is on track and not in a bad channel reset current and spillover gaps to zero
        if(ontrack){
          currentgap = 0;
          currprob = 1.0;
          spillovergap = 0;
          spilloverprob = 1.0;
          spillover = false;
        }
        else if(!ontrack && !inbadc){
          // if cell is not on the track and its not a bad channel calculate the gap probabilities
          double lenInCell = distance[iCell];     
          double zave = 0.5*trjPointsNew[itraj][1]+0.5*trjPointsNew[itraj+1][1];

          // find the approximate w of this cell
          double w = 0;
          if(fView == geo::kX){
            // loop over all of the breakpoints and find what w to use
            bool foundrng = false;
            if(zave < avgYPosZRange[0][0]){
              // use the first range
              w = avgYPos[0];
              foundrng = true;
            }
            else if(zave > avgYPosZRange[avgYPosZRange.size()-1][1]){
              // use the last range
              w = avgYPos[avgYPos.size()-1];
              foundrng = true;
            }
            else{
              // loop over all the ranges and find the one that encloses this z
              for(unsigned int i = 0; i < avgYPosZRange.size();++i){
                if(zave >= avgYPosZRange[i][0] && i <= avgYPosZRange[i][1]){
                  w = avgYPos[i];
                  foundrng = true;
                  break;
                }         
              }
            }
            if(!foundrng){ w = avgYPos[0]; }
          }
          else if(fView == geo::kY){
            // loop over all of the breakpoints and find what w to use
            bool foundrng = false;
            if(zave < avgXPosZRange[0][0]){
              // use the first range
              w = avgXPos[0];
              foundrng = true;
            }
            else if(zave > avgXPosZRange[avgXPosZRange.size()-1][1]){
              // use the last range
              w = avgXPos[avgXPos.size()-1];
              foundrng = true;
            }
            else{
              // loop over all the ranges and find the one that encloses this z
              for(unsigned int i = 0; i < avgXPosZRange.size();++i){
                if(zave >= avgXPosZRange[i][0] && i<= avgXPosZRange[i][1]){
                  w = avgXPos[i];
                  foundrng = true;
                  break;
                }         
              }
            }
            if(!foundrng){ w = avgXPos[0]; }
          }

          double probMiss = kgeo.CalcMissFrac(lenInCell,w,fView);

          if(probMiss < 1.0){
            if(!spillover){
              ++currentgap;
              currprob*=probMiss;
              ++allMissed;
            }
            else{
              // Only add if this doesn't correspond to the last plane/cell of the spillover (don't double count)
              if(missedCells[iCell].Plane() != lastPlane ||
                 missedCells[iCell].Cell() != lastCell){
                ++currentgap;
                currprob*=probMiss;
                ++allMissed;
              }
            }
          }

          //update the gap that spills over into the next track section if it is the last cell
          if(iCell == (totMissed-1)){
            spillovergap = currentgap;
            spilloverprob = currprob;
            lastPlane = missedCells[iCell].Plane();
            lastCell = missedCells[iCell].Cell();
            spillover = true;
          }

        } // end of if(!ontrack && !inbadc)

        // if we have crossed the threshold of the gap probability stop, break the track here   
        if(currprob < minProb && currentgap > 1){
          if(lastHitPlane == 10000){
            lastHitPlane = missedCells[iCell].Plane();
            lastHitCell = missedCells[iCell].Cell();
          }
          break;
        }

      } // end of loop over iCell

      if(currentgap > biggestGap){ biggestGap = currentgap; }

      if(currprob < minProb && currentgap > 1){
 
        // find all the hits to take off this track
        newTrkHits = trkHits;
        int nhitsnew = 0;
        for(int ihit = 0; ihit < fNChans; ++ihit){
          // don't need to do anything if this hit isn't on the track
          if(!trkHits[ihit]){ continue; }
          // if hit after the last hit plane, it shouldn't be anymore. Add it to new track hits
          if(channels[ihit].Plane() > lastHitPlane){
            trkHits[ihit] = false;
            ++nhitsnew;
          }
          // if hit is before last hit plane, it should only be on the old track
          if(channels[ihit].Plane() < lastHitPlane){ newTrkHits[ihit] = false; }
          // if hit is on the last hit plane it should only be on the new track if after the last hit cell
          if(channels[ihit].Plane() == lastHitPlane){
            if(posSlope && channels[ihit].Cell() > lastHitCell){
              trkHits[ihit] = false;
              ++nhitsnew;
            }
            else if(posSlope && channels[ihit].Cell() <= lastHitCell){ newTrkHits[ihit] = false; }
            else if(!posSlope && channels[ihit].Cell() < lastHitCell){
              trkHits[ihit] = false;
              ++nhitsnew;
            }
            else if(!posSlope && channels[ihit].Cell() >= lastHitCell){ newTrkHits[ihit] = false; }
          }
        }

        // stop after a gap is found
        if(nhitsnew > 0){ return; }
      }

    } // end of loop over trajectory points
    
    
  } // end of CheckTracks function

  ///////////////////////////////////////////////////////////////////////////////////
  void               KalmanTrack::CreateHitMaps()
  {
    fPlaneToCell.resize(fNChans);
    // loop over all the plane sorted hits
    for(int ipl = 0; ipl < fNChans; ++ipl){
      art::Ptr<rb::CellHit> plhit = fSliceChans[ipl].GetHit();
      // loop over all the cell sorted hits and compare to this hit
      for(unsigned int icl = 0; icl < fSliceChansCellSort.size(); ++icl){
        if(plhit == fSliceChansCellSort[icl].GetHit()){
          fPlaneToCell[ipl] = icl;
          break;
        }
      }
    }
  }  // end of CreateHitMaps()

  ///////////////////////////////////////////////////////////////////////////////////
  void                KalmanTrack::PlaneToCellSortHits(std::vector<bool> &planeSortedHits)
  {
    std::vector<bool> cellSortedHits(planeSortedHits.size());
    // loop over the planeSortedHits and set the corresponding cellSortedHit to true
    for(unsigned int i = 0; i < planeSortedHits.size(); ++i){
      // get the corresponding cell sorted hit location
      int cellsort = fPlaneToCell[i];
      cellSortedHits[cellsort] = planeSortedHits[i];
    }
    planeSortedHits = cellSortedHits;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  void                KalmanTrack::CellToPlaneSortHits(std::vector<bool> &cellSortedHits)
  {
    std::vector<bool> planeSortedHits(cellSortedHits.size());
    // loop over the cellSortedHits and set the corresponding planeSortedHit to true
    for(unsigned int i = 0; i < cellSortedHits.size(); ++i){
      int planesort = 0;
      // get the corresponding plane sorted hit location
      for(unsigned int j = 0; j < fPlaneToCell.size(); ++j){
        if(fPlaneToCell[j] == (int)i){ 
          planesort = j;
          break;
        }
      }
      planeSortedHits[planesort] = cellSortedHits[i];
    }
    cellSortedHits = planeSortedHits;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  void                KalmanTrack::PlaneToCellSort(std::vector<std::array<double, 2> > &planeSortedVec)
  {
    std::vector<std::array<double, 2>> cellSortedVec(planeSortedVec.size());
    // loop over the planeSortedVec and set the corresponding cellSortedHit to true
    for(unsigned int i = 0; i < planeSortedVec.size(); ++i){
      // get the corresponding cell sorted hit location
      int cellsort = fPlaneToCell[i];
      cellSortedVec[cellsort] = planeSortedVec[i];
    }
    planeSortedVec = cellSortedVec;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  void                KalmanTrack::CellToPlaneSort(std::vector<std::array<double, 2> > &cellSortedVec)
  {
    std::vector<std::array<double, 2>> planeSortedVec(cellSortedVec.size());
    // loop over the cellSortedVec and set the corresponding planeSortedHit to true
    for(unsigned int i = 0; i < cellSortedVec.size(); ++i){
      int planesort = 0;
      // get the corresponding plane sorted hit location
      for(unsigned int j = 0; j < fPlaneToCell.size(); ++j){
        if(fPlaneToCell[j] == (int)i){ 
          planesort = j;
          break;
        }
      }
      planeSortedVec[planesort] = cellSortedVec[i];
    }
    cellSortedVec = planeSortedVec;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  int                KalmanTrack::CountHits(const std::vector<bool> & trkHits)
  {
    int nhits = 0;
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){ ++nhits; }
    }
    return nhits;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  bool               KalmanTrack::IsSinglePlane(const std::vector<bool> & trkHits)
  {

    int minpl = -1;
    int maxpl = -1;
    // find the first plane in this track
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){
        minpl = fSliceChans[i].Plane();
        break;
      }
    }
    // find the last plane in this track
    for(int i = fNChans-1; i >=0; --i){
      if(trkHits[i]){
        maxpl = fSliceChans[i].Plane();
        break;
      }
    }
    if(minpl == maxpl){ return true; }
    else{ return false; }

  }

  ///////////////////////////////////////////////////////////////////////////////////
  int                KalmanTrack::GetFirstHit(const std::vector<bool> & trkHits)
  {
    int firstHit = -1;
    for(int i = 0; i < fNChans; ++i){
      if(trkHits[i]){
        firstHit = i;
        break;
      }
    }
    return firstHit;
  }

  ///////////////////////////////////////////////////////////////////////////////////
  int                KalmanTrack::GetLastHit(const std::vector<bool> & trkHits)
  {
    int lastHit = -1;
    for(int i = fNChans-1; i >= 0; --i){
      if(trkHits[i]){
        lastHit = i;
        break;
      }
    }
    return lastHit;
  }
  
}

 
namespace trk{

  DEFINE_ART_MODULE(KalmanTrack)

}