KalmanTrackMerge_module.cc

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////
// \file    KalmanTrackMerge_module.cc
// \brief   A module for merging 2d tracks into 3d tracks
// \version $Id: KalmanTrackMerge_module.cc,v 1.8 2012-11-28 17:16:59 raddatz Exp $
// \author  Nick Raddatz
////////////////////////////////////////////////////////////////////////////

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

// ROOT includes
#include "TMath.h"
#include "TVector3.h"


// NOvA includes
#include "Geometry/Geometry.h"
#include "GeometryObjects/Geo.h"
#include "GeometryObjects/CellGeo.h"
#include "RecoBase/Track.h"
#include "RecoBase/FilterList.h"
#include "Utilities/AssociationUtil.h"
#include "TrackFit/LocatedChan.h"
#include "TrackFit/KalmanGeoHelper.h"


// Framework includes
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "canvas/Persistency/Common/FindManyP.h"


//tracking algorithms
namespace trk {
  class KalmanTrackMerge : public art::EDProducer {
  public:
    explicit KalmanTrackMerge(fhicl::ParameterSet const& pset);
    virtual ~KalmanTrackMerge();
    void produce(art::Event& evt);
    void reconfigure(const fhicl::ParameterSet& p);
    static bool sort_traj(const TVector3& a, const TVector3& b);
    
  private:    
    void                                   MatchTracks(KalmanGeoHelper& kgeo,
                                                       std::vector<rb::Track> xtracks, 
                                                       std::vector<rb::Track> ytracks,
                                                       std::vector<rb::Track>&matchedtracls, 
                                                       std::vector<rb::Track>&unmatchedtracks);
    bool                                   CanJoinTracks(KalmanGeoHelper& kgeo, rb::Track tracka, rb::Track trackb);
    rb::Track                              JoinTracks(KalmanGeoHelper& kgeo, rb::Track tracka, rb::Track trackb);
    double                                 CalcMatchScore(KalmanGeoHelper& kgeo, rb::Track tracka, rb::Track trackb);
    rb::Track                              ViewMergeTracks(KalmanGeoHelper& kgeo, rb::Track xTrack, rb::Track yTrack);
    void                                   ShiftInterpolationPoints(TVector3 endtraj, TVector3 nextraj, 
                                                                    unsigned int plane, rb::Track track, 
                                                                    TVector3 &lowztraj, TVector3 &highztraj);
    std::string                            fTrackInput;       ///< Input folder from track reco
    std::string                            fSliceInput;       ///< Input folder of slices
    bool                                   fBadChannels;      ///< Account for bad channels in gaps ?
    bool                                   fObeyPreselection; ///< Check rb::IsFiltered?
    art::ServiceHandle<geo::Geometry>      fgeom;             ///< Detector geometry
    std::vector<double> avgXPos;
    std::vector<std::vector<double> > avgXPosZRange;
    std::vector<double> avgYPos;
    std::vector<std::vector<double> > avgYPosZRange;
    double avgX;
    double avgY;

  };
}

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

  //......................................................................
  void KalmanTrackMerge::reconfigure(const fhicl::ParameterSet& pset)
  {
    fTrackInput        = pset.get< std::string >("TrackInput")      ;
    fSliceInput        = pset.get< std::string >("SliceInput")      ;
    fBadChannels       = pset.get< bool        >("BadChannels")     ;
    fObeyPreselection  = pset.get< bool        >("ObeyPreselection");
  }


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

  void KalmanTrackMerge::produce(art::Event& evt)
  {
    // Declare a container for Track objects to be stored in the art::event
    std::unique_ptr< std::vector<rb::Track> > mergetrackcol(new std::vector<rb::Track>);
    std::unique_ptr< art::Assns<rb::Track, rb::Cluster> > assns(new art::Assns<rb::Track, rb::Cluster>);

    KalmanGeoHelper kgeo(fBadChannels);

    // define a vector holding slices
    art::Handle<std::vector<rb::Cluster> > slicecol;
    evt.getByLabel(fSliceInput,slicecol);

    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> clust(slicecol,i);
      slicelist.push_back(clust);
    }

    // loop over the slices and merge tracks
    art::FindManyP<rb::Track> fmt(slicelist, evt, fTrackInput);

    for(unsigned int iSlice = 0; iSlice<slicelist.size(); ++iSlice){
      // do nothing for noise slices
      if(slicelist[iSlice]->IsNoise()){ continue; }
      // Get all of the tracks in the slice
      const std::vector< art::Ptr<rb::Track> > slTracks = fmt.at(iSlice);
      // if no associated tracks do nothing
      if(slTracks.empty()){ continue; }
      avgXPosZRange.resize(0);
      avgXPos.resize(0);
      avgYPosZRange.resize(0);
      avgYPos.resize(0);
      avgX = slicelist[iSlice]->MeanX();
      avgY = slicelist[iSlice]->MeanY();
      std::vector<trk::LocatedChan> xcellchans;
        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(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
        int nxzbreak = (int)floor((zxmax-zxmin)/100);
        // decide where to break the z positions up
        double dz = (zxmax-zxmin)/((double)nxzbreak);
        std::vector<double> xzbreaks;
        xzbreaks.push_back(zxmin);
        for(int ibrk = 1; ibrk<=nxzbreak; ++ibrk){
          xzbreaks.push_back(zxmin+((double)ibrk)*dz);
        }
        xzbreaks.push_back(zxmax);
        for(unsigned int ibrk = 1; ibrk < xzbreaks.size(); ++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); }
        }
        
        
        // 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(xyz[2]>zymax){ zymax = xyz[2]; }
          if(xyz[2]<zymin){ zymin = xyz[2]; }
        }
        
        // how many break points in yz
        int nyzbreak = (int)floor((zymax-zymin)/100);
        // 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); }
        }

      // seperate tracks by view
      std::vector<rb::Track> xTracks;
      std::vector<rb::Track> yTracks;
      for(unsigned int j = 0;j<slTracks.size();++j){
        if(slTracks[j]->View() == geo::kX){
          xTracks.push_back(*slTracks[j]);
        }
        else if(slTracks[j]->View() == geo::kY){
          yTracks.push_back(*slTracks[j]);
        }
      }


      std::vector<rb::Track> matchedTracks;
      std::vector<rb::Track> unmatchedTracks;
      MatchTracks(kgeo, xTracks,yTracks,matchedTracks,unmatchedTracks);
      for(unsigned int j = 0; j<matchedTracks.size();j++){
        mergetrackcol->push_back(matchedTracks[j]);
        util::CreateAssn(*this,evt,*mergetrackcol,slicelist[iSlice],*assns);
      }
      for(unsigned int j = 0; j<unmatchedTracks.size();j++){
        mergetrackcol->push_back(unmatchedTracks[j]);
        util::CreateAssn(*this,evt,*mergetrackcol,slicelist[iSlice],*assns);
      }
    }


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

  }

  //......................................................................

  void KalmanTrackMerge::MatchTracks(KalmanGeoHelper& kgeo,
                                     std::vector<rb::Track > xTracks, std::vector<rb::Track>  yTracks,
                                     std::vector<rb::Track> &matchedtracks, std::vector<rb::Track> &unmatchedtracks)
  {

    std::vector<rb::Track> allXTracks;
    std::vector<std::vector< int > > jointxtrkIDs;
    // Do a first order joining of all tracks in each view
    // First add all of the individual tracks
    for(unsigned int ix = 0; ix<xTracks.size();++ix){
      allXTracks.push_back(xTracks[ix]);
      std::vector<int> xid;
      xid.push_back(ix);
      jointxtrkIDs.push_back(xid);
    }
    // now add all of the possible joint x tracks
    unsigned int ixmaxold = 0;
    unsigned int ixmax = allXTracks.size();
    unsigned int ixmin = 0;
    while(ixmaxold != ixmax && allXTracks.size()<100){
      ixmaxold = ixmax;
      for(unsigned int ix = 0; ix<ixmaxold; ++ix){
        // get the list of tracks that contributed to this track
        std::vector<int> ixids = jointxtrkIDs[ix];
        std::vector<int> jntids = ixids;
        for(unsigned int jx = ixmin; jx<ixmaxold; ++jx){
          // get the list of tracks that contributed to this track
          std::vector<int> jxids = jointxtrkIDs[jx];
          // check if any of these ids overlap, if so don't try to join
          bool idoverlap = false;
          for(unsigned int i = 0; i<ixids.size(); ++i){
            for(unsigned int j = 0; j<jxids.size(); ++j){
              if(ixids[i] == jxids[j]){
                idoverlap = true;
                break;
              }
            }
            if(idoverlap){ break; }
          }
          if(idoverlap){ continue; }
          // check if this combination has been used yet
          for(unsigned int j = 0; j<jxids.size(); ++j){
            jntids.push_back(jxids[j]);
          }
          std::sort(jntids.begin(),jntids.end());
          bool used = false;
          for(unsigned int i = 0; i<jointxtrkIDs.size(); ++i){
            if(jntids.size() != jointxtrkIDs[i].size()){ continue; }
            bool same = true;
            std::vector<int> jntidscheck = jointxtrkIDs[i];
            std::sort(jntidscheck.begin(),jntidscheck.end());
            for(unsigned int j = 0; j<jointxtrkIDs[i].size(); ++j){
              if(jntids[j] != jntidscheck[j]){
                same = false;
                break;
              }
            }
            if(same){
              used = true;
              break;
            }
          }
          if(used){ continue; }
          bool join = CanJoinTracks(kgeo,allXTracks[ix],allXTracks[jx]);
          if(join){
            rb::Track JointTrack = JoinTracks(kgeo,allXTracks[ix],allXTracks[jx]);
            allXTracks.push_back(JointTrack);
            std::vector<int> xjointid;
            for(unsigned int i = 0; i<ixids.size(); ++i){
              xjointid.push_back(ixids[i]);
            }
            for(unsigned int j = 0; j<jxids.size(); ++j){
              xjointid.push_back(jxids[j]);
            }
            jointxtrkIDs.push_back(xjointid);
          }
        }
        ixmax = allXTracks.size();
      }
      ixmax = allXTracks.size();
      ixmin = ixmaxold;
    }

    std::vector<rb::Track> allYTracks;
    std::vector<std::vector< int > > jointytrkIDs;
    // Do a first order joining of all tracks in each view
    // First add all of the individual tracks
    for(unsigned int iy = 0; iy<yTracks.size();++iy){
      allYTracks.push_back(yTracks[iy]);
      std::vector<int> yid;
      yid.push_back(iy);
      jointytrkIDs.push_back(yid);
    }
    // now add all of the possible joint y tracks
    unsigned int iymaxold = 0;
    unsigned int iymax = allYTracks.size();
    unsigned int iymin = 0;
    while(iymaxold != iymax && allYTracks.size()<100){
      iymaxold = iymax;
      for(unsigned int iy = 0; iy<iymaxold; ++iy){
        // get the list of tracks that contributed to this track
        std::vector<int> iyids = jointytrkIDs[iy];
        std::vector<int> jntids = iyids;
        for(unsigned int jy = iymin; jy<iymaxold; ++jy){
          // get the list of tracks that contributed to this track
          std::vector<int> jyids = jointytrkIDs[jy];
          // check if any of these ids overlap, if so don't try to join
          bool idoverlap = false;
          for(unsigned int i = 0; i<iyids.size(); ++i){
            for(unsigned int j = 0; j<jyids.size(); ++j){
              if(iyids[i] == jyids[j]){
                idoverlap = true;
                break;
              }
            }
            if(idoverlap){ break; }
          }
          if(idoverlap){ continue; }
          // check if this combination has been used yet
          for(unsigned int j = 0; j<jyids.size(); ++j){
            jntids.push_back(jyids[j]);
          }
          std::sort(jntids.begin(),jntids.end());
          bool used = false;
          for(unsigned int i = 0; i<jointytrkIDs.size(); ++i){
            if(jntids.size() != jointytrkIDs[i].size()){ continue; }
            bool same = true;
            std::vector<int> jntidscheck = jointytrkIDs[i];
            std::sort(jntidscheck.begin(),jntidscheck.end());
            for(unsigned int j = 0; j<jointytrkIDs[i].size(); ++j){
              if(jntids[j] != jntidscheck[j]){
                same = false;
                break;
              }
            }
            if(same){
              used = true;
              break;
            }
          }
          if(used){ continue; }
          bool join = CanJoinTracks(kgeo,allYTracks[iy],allYTracks[jy]);
          if(join){
            rb::Track JointTrack = JoinTracks(kgeo,allYTracks[iy],allYTracks[jy]);
            allYTracks.push_back(JointTrack);
            std::vector<int> yjointid;
            for(unsigned int i = 0; i<iyids.size(); ++i){
              yjointid.push_back(iyids[i]);
            }
            for(unsigned int j = 0; j<jyids.size(); ++j){
              yjointid.push_back(jyids[j]);
            }
            jointytrkIDs.push_back(yjointid);
          }
        }
        iymax = allYTracks.size();
      }
      iymax = allYTracks.size();
      iymin = iymaxold - 1;
    }


    // create a vector that tells the score of the possible matched tracks, lowest score >0 is best.
    double score[allXTracks.size()][allYTracks.size()];        
    for(unsigned int  ix = 0; ix<allXTracks.size();++ix){
      //find the begining and ending plane for each x track
      for(unsigned int iy = 0; iy<allYTracks.size();++iy){
        score[ix][iy] = CalcMatchScore(kgeo, allXTracks[ix],allYTracks[iy]);
      }
    }
      
    // find the best score of all the possible matches.
    // Now get the best matched tracks out of the group based on lowest score (not zero)
    bool doneMatching = false;
    std::vector<int> xMatched(allXTracks.size());
    for(unsigned int i = 0; i<xMatched.size();++i){
      xMatched[i] = 0;
    }
    std::vector<int> yMatched(allYTracks.size());
    for(unsigned int i = 0; i<yMatched.size();++i){
      yMatched[i] = 0;
    }
    while(!doneMatching){
      int xbest = -1;
      int ybest = -1;
      double bestscore = -1;
      // loop over all possible matched tracks to find the best one
      for(unsigned int ix = 0; ix<allXTracks.size();++ix){
        for(unsigned int iy = 0; iy<allYTracks.size();++iy){
          if((score[ix][iy] <= bestscore || bestscore == -1) && score[ix][iy] >= 0.0 && score[ix][iy] <= 5.0){
            xbest = ix;
            ybest = iy;
            bestscore = score[ix][iy];
          }
        }
      }

      // store the best matched track and reset things for the next best track
      if(bestscore >= 0){
        // make a 3d track out of the x and y tracks

        rb::Track newtrack = ViewMergeTracks(kgeo,allXTracks[xbest],allYTracks[ybest]);

        matchedtracks.push_back(newtrack);

        // loop over the ids of the x best track
        for(unsigned int i = 0; i<jointxtrkIDs[xbest].size(); ++i){
          int matchedtrkid = jointxtrkIDs[xbest][i];
          // loop over all x tracks
          for(unsigned int itrk = 0; itrk<allXTracks.size(); ++itrk){
            // if this track has been matched, no need in checking it again
            if(xMatched[itrk] == 0){
              // loop over all ids that make up this track
              bool usedtrk = false;
              for(unsigned int itrkid = 0; itrkid<jointxtrkIDs[itrk].size(); ++itrkid){
                if(jointxtrkIDs[itrk][itrkid] == matchedtrkid){
                  usedtrk = true;
                  break;
                }
              }
              if(usedtrk){
                // this track needs to be marked as used and all scores associated with it set to -1
                xMatched[itrk] = 1;
                for(unsigned int iytrk = 0; iytrk<allYTracks.size();++iytrk){
                  score[itrk][iytrk] = -1.0;
                }
              }
            }
          }
        }

        // loop over the ids of the y best track
        for(unsigned int i = 0; i<jointytrkIDs[ybest].size(); ++i){
          int matchedtrkid = jointytrkIDs[ybest][i];
          // loop over all x tracks
          for(unsigned int itrk = 0; itrk<allYTracks.size(); ++itrk){
            // if this track has been matched, no need in checking it again
            if(yMatched[itrk] == 0){
              // loop over all ids that make up this track
              bool usedtrk = false;
              for(unsigned int itrkid = 0; itrkid<jointytrkIDs[itrk].size(); ++itrkid){
                if(jointytrkIDs[itrk][itrkid] == matchedtrkid){
                  usedtrk = true;
                  break;
                }
              }
              if(usedtrk){
                // this track needs to be marked as used and all scores associated with it set to -1
                yMatched[itrk] = 1;
                for(unsigned int ixtrk = 0; ixtrk<allXTracks.size();++ixtrk){
                  score[ixtrk][itrk] = -1.0;
                }
              }
            }
          }
        }

      }
      else{ doneMatching = true; }
    } 
    // Now clean up any unmatched tracks
    for(unsigned int i = 0; i<xTracks.size();++i){
      if(xMatched[i] == 0){
        // make a new track object that is the same as the current track object
        rb::Cluster clust(geo::kX);
        rb::Track track(clust,xTracks[i].Start().X(),xTracks[i].Start().Z(),
                        xTracks[i].Dir().X(),xTracks[i].Dir().Z());
        for(unsigned int j = 1 ; j<xTracks[i].NTrajectoryPoints();++j){
          track.AppendTrajectoryPoint(xTracks[i].TrajectoryPoint(j).X(),xTracks[i].TrajectoryPoint(j).Z());
        }
        for(unsigned int j = 0; j<xTracks[i].NCell();++j){
          track.Add(xTracks[i].Cell(j));
        }
        track.SetID(xTracks[i].ID());
        unmatchedtracks.push_back(track);
      }
    }
    for(unsigned int i = 0; i<yTracks.size();++i){
      if(yMatched[i] == 0){
        // make a new track object that is the same as the current track object
        rb::Cluster clust(geo::kY);
        rb::Track track(clust,yTracks[i].Start().Y(),yTracks[i].Start().Z(),
                        yTracks[i].Dir().Y(),yTracks[i].Dir().Z());
        for(unsigned int j = 1 ; j<yTracks[i].NTrajectoryPoints();++j){
          track.AppendTrajectoryPoint(yTracks[i].TrajectoryPoint(j).Y(),yTracks[i].TrajectoryPoint(j).Z());
        }
        for(unsigned int j = 0; j<yTracks[i].NCell();++j){
          track.Add(yTracks[i].Cell(j));
        }
        track.SetID(yTracks[i].ID());
        unmatchedtracks.push_back(track);
      }
    }

  }

  ///////////////////////////////////////////////////////////////////////////
  bool     KalmanTrackMerge::CanJoinTracks(KalmanGeoHelper& kgeo,
                                           rb::Track trka, rb::Track trkb)
  {
 
    // check if these tracks can be added together
    // first check how many cells apart from closest ends
    TVector3 start1 = trka.Start();
    TVector3 stop1 = trka.Stop();
    TVector3 start2 = trkb.Start();
    TVector3 stop2 = trkb.Stop();
    int view = 0;
    if(trka.View() == geo::kY){ view = 1; }
    // calculate the intersection point of these two tracks
    
    double stst = (start1-start2).Mag();
    double stsp = (start1-stop2).Mag();
    double spst = (stop1-start2).Mag();
    double spsp = (stop1-stop2).Mag();
    TVector3 joint1;
    TVector3 joint2;
    TVector3 end1;
    TVector3 end2;
    
    if(trka.NTrajectoryPoints() < 2 || trkb.NTrajectoryPoints() < 2) return false;
    
    if(stst < stsp && stst < spst && stst< spsp){
      joint1 = start1;
      joint2 = start2;
      end1 = trka.TrajectoryPoint(1);
      end2 = trkb.TrajectoryPoint(1);
    }
    else if(stsp < stst &&  stsp < spst && stsp < spsp){
      joint1 = start1;
      joint2 = stop2;
      end1 = trka.TrajectoryPoint(1);
      end2 = trkb.TrajectoryPoint(trkb.NTrajectoryPoints()-2);
    }
    else if(spst< stst && spst< stsp && spst < spsp){
      joint1 = stop1;
      joint2 = start2;
      end1 = trka.TrajectoryPoint(trka.NTrajectoryPoints()-2);
      end2 = trkb.TrajectoryPoint(1);
    }
    else{
      joint1 = stop1;
      joint2 = stop2;
      end1 = trka.TrajectoryPoint(trka.NTrajectoryPoints()-2);
      end2 = trkb.TrajectoryPoint(trkb.NTrajectoryPoints()-2);
    }
    // Can't join if too large a gap between end points
    double jointv1 = joint1.X();
    double jointv2 = joint2.X();
    double endv1 = end1.X();
    double endv2 = end2.X();
    if(view == 1){
      jointv1 = joint1.Y();
      jointv2 = joint2.Y();
      endv1 = end1.Y();
      endv2 = end2.Y();
    }
    
    // first find the planes of the joints
    int plane1;
    int plane2;
    
    if(joint1.Z() == start1.Z()){plane1 = trka.MinPlane(); }
    else{ plane1 = trka.MaxPlane(); }
    if(joint2.Z() == start2.Z()){plane2 = trkb.MinPlane(); }
    else{ plane2 = trkb.MaxPlane(); }
    
    // make simple cuts first
    // 1) No back scatter in z
    // no backscatter in z, need to be careful about the joining if hits are on same plane, might look backscattered when really not
    // or might not look backscattered when they really are, treat this case seperate
    if(plane1 == plane2){
      // in this case use same joint z to determine backscatter
      if( (joint1.Z()-end1.Z())*(end2.Z()-joint1.Z()) < 0 ){
        return false;
      }
    }
    else{
      if( (joint1.Z()-end1.Z())*(joint2.Z()-joint1.Z()) < 0 ||
          (joint2.Z()-joint1.Z())*(end2.Z()-joint2.Z()) < 0 ){
        return false;
      }
    }

    if(trka.MinPlane() == trka.MaxPlane() && trka.MeanZ() < trkb.MeanZ()){ return false; }
    else if(trkb.MinPlane() == trkb.MaxPlane() && trkb.MeanZ() < trka.MeanZ()){ return false; }

    // 2) No vertical scatter at low z (almost no backscatter)
    // Also if one track is completely vertical only allow it to be linked if the vertical track is at high z
    if(joint1.Z() == end1.Z() && joint1.Z() < end2.Z()){ return false; }
    else if(joint2.Z() == end2.Z() && joint2.Z() < end1.Z()){ return false;  }
    
    // Got rid of easy checks, now check gaps and gap probabilities
    // First thing find all the cells on the track at the track ends to be joined
    int celllow1 = 9999;
    int cellhigh1 = 0;
    for(unsigned int iCell = 0; iCell<trka.NCell(); ++iCell){
      if(trka.Cell(iCell)->Plane() == plane1){
        if(trka.Cell(iCell)->Cell()>cellhigh1){ cellhigh1 = trka.Cell(iCell)->Cell(); }
        if(trka.Cell(iCell)->Cell()<celllow1){ celllow1 = trka.Cell(iCell)->Cell();}
      }
    }
    
    int celllow2 = 9999;
    int cellhigh2 = 0;
    for(unsigned int iCell = 0; iCell<trkb.NCell(); ++iCell){
      if(trkb.Cell(iCell)->Plane() == plane2){
        if(trkb.Cell(iCell)->Cell()>cellhigh2){ cellhigh2 = trkb.Cell(iCell)->Cell(); }
        if(trkb.Cell(iCell)->Cell()<celllow2){ celllow2 = trkb.Cell(iCell)->Cell();}
      }
    }
    
    int cellDiff = 0;
    // find the actual cell difference, if something strange happens where higher v has lower cell set cellDiff as 0
    // if it overlaps the window in which the v occurs
    if(jointv1 > jointv2){
      // cell diff should be celllow1-cellhigh2
      if(celllow1 <= cellhigh2){ cellDiff = celllow1- cellhigh2; }
      else if(cellhigh2 >= celllow1 && cellhigh2 <= cellhigh1){ cellDiff = 0; }
      else{ cellDiff = -(cellhigh2- celllow1); }
    }
    else{
      // cell diff should be celllow1-cellhigh2
      if(cellhigh1 <= celllow2){ cellDiff = celllow2-cellhigh1; }
      else if(cellhigh1 >= celllow2 && cellhigh1 <= cellhigh2){ cellDiff = 0;}
      else{ cellDiff = -(cellhigh1-celllow2); }
    }
    // #3 if the two tracks are to be joined at the same plane, make sure no gap exists between hits
    if(plane1 == plane2 && abs(cellDiff) > 1){ return false;  }
    
    // #4 Make sure there is not a gap in wrong direction to cause double scatters
    TVector3 diff1 = joint1-end1;
    TVector3 jointdiff = joint2-joint1;
    TVector3 diff2 = end2-joint2;
    double cos1 = diff1.Dot(jointdiff)/(diff1.Mag()*jointdiff.Mag());
    double cos2 = diff2.Dot(jointdiff)/(diff2.Mag()*jointdiff.Mag());
    double slope1 = diff1.X()/diff1.Z();
    double slope2 = diff2.X()/diff2.Z();
    double slopediff = jointdiff.X()/jointdiff.Z();
    if(view == 1){
      slope1 = diff1.Y()/diff1.Z();
      slope2 = diff2.Y()/diff2.Z();
      slopediff = jointdiff.Y()/jointdiff.Z();
    }
    // This is scattering cosines are correct unless it is backscattered , then it is off by 90 degrees
    double rotAngle1 = TMath::ATan(slope1);
    double rotAngle2 = TMath::ATan(slopediff);
    double joint2zrot = TMath::Cos(rotAngle1)*(joint2.Z() - joint1.Z()) - TMath::Sin(rotAngle1)*(jointv2 - jointv1);
    if(joint2zrot < 0){ cos1 = -cos1; }
    double end2zrot = TMath::Cos(rotAngle2)*(end2.Z() - joint2.Z()) - TMath::Sin(rotAngle2)*(endv2 - jointv2);
    if(end2zrot < 0){ cos2 = -cos2; }


    // check that the intersection points happen within a box that would make a hard scatter a single scatter
    double vint, zint;
    geo::LineIntersection(endv1,end1.Z(),jointv1,joint1.Z(),endv2,end2.Z(),jointv2,joint2.Z(),vint,zint);
    // if parallel lines don't join unless one cell apart (plastic), these would have been made one track by 2D tracking if possible
    double dx = jointv1-endv1;
    double dy = joint1.Z()-end1.Z();
    double dX = jointv2-endv2;
    double dY = joint2.Z()-end2.Z();
    double denom = dX*dy-dY*dx;
    if(denom == 0 && cellDiff>1){ return false; }
    double xyzpl1[3];
    fgeom->Plane(plane1)->Cell(celllow1)->GetCenter(xyzpl1);
    double dz1 = fgeom->Plane(plane1)->Cell(celllow1)->HalfD();
    double dv1 = fgeom->Plane(plane1)->Cell(celllow1)->HalfW();
    double zlow = xyzpl1[2]-dz1;
    double vlow = xyzpl1[view]-dv1;
    double xyzpl2[3];
    fgeom->Plane(plane2)->Cell(cellhigh2)->GetCenter(xyzpl2);
    double dz2 = fgeom->Plane(plane2)->Cell(cellhigh2)->HalfD();
    double dv2 = fgeom->Plane(plane2)->Cell(cellhigh2)->HalfW();
    double zhigh = xyzpl2[2]+dz2;
    double vhigh = xyzpl2[view]+dv2;

    if(joint2.Z() < joint1.Z()){
      zlow = xyzpl2[2]-dz2;
      zhigh = xyzpl1[2]+dz1;
    }
    if((slope1*slope2) > 0 && plane1 != plane2 && abs(cellDiff)>1){
      if(jointv2 < jointv1){
        fgeom->Plane(plane2)->Cell(celllow2)->GetCenter(xyzpl2);
        fgeom->Plane(plane1)->Cell(cellhigh1)->GetCenter(xyzpl1);
        vlow = xyzpl2[view]-dv2;
        vhigh = xyzpl1[view]+dv1;
      }
      if(vint < vlow || vint > vhigh || zint < zlow || zint > zhigh){ return false; }
    }
    else if((slope1*slope2) <= 0 && plane1 != plane2 && abs(cellDiff)>1){
      // if tracks have opposite slopes can only check that z falls between z1 and z2
      // check that v location is in the detector
      if(view == 0){
        vlow = -fgeom->DetHalfWidth();
        vhigh = fgeom->DetHalfWidth();
      }
      else{
        vlow = -fgeom->DetHalfHeight();
        vhigh = fgeom->DetHalfHeight();
      }
      if(zint < zlow || zint > zhigh || vint < vlow || vint > vhigh){ return false; }
    }
    else if(plane1 == plane2){
      // give more cushion here to match the uncertainty in the case wher plane1 != plane2
      if(zint < zlow-dz1 || zint > zhigh+dz1){ return false; }
    }

    // now get the missed cells between the potential link
    int numMissed = 0; 
    std::vector<geo::OfflineChan> hitsOnline;
    std::vector<double> distance;
    if(plane1 != plane2){
      // need to use intersection point to get true number of cells missed
      int numMissed2 = 0; 
      std::vector<geo::OfflineChan> hitsOnline2;
      std::vector<double> distance2;
      if(view == 0){
        numMissed = kgeo.CountMissedCellsOnLine(geo::kX, joint1.X(), joint1.Z(), vint, zint, hitsOnline, distance);
        numMissed2 = kgeo.CountMissedCellsOnLine(geo::kX, vint, zint, joint2.X(), joint2.Z(), hitsOnline2, distance2);
      }
      else{
        numMissed = kgeo.CountMissedCellsOnLine(geo::kY, joint1.Y(), joint1.Z(), vint, zint, hitsOnline, distance);
        numMissed2 = kgeo.CountMissedCellsOnLine(geo::kY, vint, zint, joint2.Y(), joint2.Z(), hitsOnline2, distance2);
      }
      // now add misses on the second bit of the scatter to the first if it doesn't exist already
      for(int imiss2 = 0; imiss2 < numMissed2; ++imiss2){
          ++numMissed;
         hitsOnline.push_back(hitsOnline2[imiss2]);
         distance.push_back(distance2[imiss2]);
      }
      // now loop over the hitsOnline to see if any fall into the cells at the end of either track
      std::vector<int> missesToRemove;
      for(unsigned int imiss = 0; imiss<hitsOnline.size();++imiss){
        if(hitsOnline[imiss].Plane() == plane1 && hitsOnline[imiss].Cell() >= celllow1 && hitsOnline[imiss].Cell() <= cellhigh1){
          missesToRemove.push_back(imiss);
        }
        else if(hitsOnline[imiss].Plane() == plane2 && hitsOnline[imiss].Cell() >= celllow2 && hitsOnline[imiss].Cell() <= cellhigh2){
          missesToRemove.push_back(imiss);
        } 
      }
      // careful to not remove wrong ones by erasing from highest index in vector to lowest
      reverse(missesToRemove.begin(),missesToRemove.end());
      for(unsigned int imiss = 0; imiss < missesToRemove.size(); ++imiss){
        --numMissed;
        hitsOnline.erase(hitsOnline.begin()+missesToRemove[imiss]);
        distance.erase(distance.begin()+missesToRemove[imiss]);
      }
    }
    else{
      // CountMissedCellsOnLine not quite right when both z values on the same plane
      unsigned short lowestCell = cellhigh1;
      unsigned short highestCell = celllow2;
      if(jointv1 > jointv2){
        lowestCell = cellhigh2;
        highestCell = celllow1;
      }   
      for(unsigned short imiss = lowestCell+1; imiss<highestCell;++imiss){
        geo::OfflineChan miss(plane1,imiss);
        hitsOnline.push_back(miss);
        // get the width of cell to travel through
        distance.push_back(2*fgeom->Plane(plane1)->Cell(imiss)->HalfW());
      }
    }
    // got the missed cells, now get the probability of missing these cells
    double probMiss = 1;
    for(unsigned int i = 0; i < distance.size(); ++i){

      // find the approximate w of this cell
      double w = 0;
      double zave = 0.5*joint1.Z()+0.5*joint2.Z();
      if(trka.View() == 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(trka.View() == 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]; }
      }      
      probMiss*=(kgeo.CalcMissFrac(distance[i],w,trka.View()));
    }

    double minProb = 0.00001;  
    double minScatProb = 0.1; 
    // #5 if prob miss is greater than the gap prob, then this has to be a hard scatter
    //    if it is a true hard scatter, be careful about joining, scale gap probability up by scattering angles
    // #6 or if the prob miss is less than the gap prob, then the only way to get this should be by going through plastic
    //    if we are in plastic, then the cell difference between the two ends should be zero or one only
    if(probMiss > minProb){
      // remove any remaining backscatter
      if((cos1+cos2)<=0){ return false; }
      if(probMiss < 2.0*minProb/(cos1+cos2)){ return false; }
      if(probMiss < minScatProb){ return false;  }      
    }
    else{
      // check that slopes are relatively flat ie less than 10 degrees to be consistent with traveling through large sections of plastic
      if(abs(cellDiff) > 1){ return false;  }      
      double trkcos1 = TMath::Cos(TMath::ATan(slope1));
      double trkcos2 = TMath::Cos(TMath::ATan(slope2));
      double cosmax = trkcos1;
      double cosmin = trkcos2;
      if(trkcos2 > trkcos1){
        cosmax = trkcos2;
        cosmin = trkcos1;
      }
      if(cosmin < 0.98 || cosmax < 0.995){ return false; }
    }

    // made it through all checks retrun that they can be joined
    return true;
  }

  ///////////////////////////////////////////////////////////////////////

  rb::Track KalmanTrackMerge::JoinTracks(KalmanGeoHelper& kgeo,
                                         rb::Track trka, rb::Track trkb)
  {
    // check that these two tracks belong to the same view
    assert(trka.View() == trkb.View());
    assert(trka.View() != geo::kXorY);
    rb::Cluster JointCluster(trka.View(),trka.ID());
    // add hits from trka and trkb
    JointCluster.Add(trka.AllCells());
    JointCluster.Add(trkb.AllCells());

    // find which ends to join together
    TVector3 start1 = trka.Start();
    TVector3 stop1 = trka.Stop();
    TVector3 start2 = trkb.Start();
    TVector3 stop2 = trkb.Stop();
    double stst = (start1-start2).Mag();
    double stsp = (start1-stop2).Mag();
    double spst = (stop1-start2).Mag();
    double spsp = (stop1-stop2).Mag();
    int ntraja = trka.NTrajectoryPoints();
    int ntrajb = trkb.NTrajectoryPoints();
    // find the start/stop planes
    std::vector<double> zends;
    zends.push_back(start1.Z());
    zends.push_back(stop1.Z());
    zends.push_back(start2.Z());
    zends.push_back(stop2.Z());
    std::vector<TVector3> trkends;
    trkends.push_back(start1);
    trkends.push_back(stop1);
    trkends.push_back(start2);
    trkends.push_back(stop2);
    // Find the planes that correspond to the start and stop z
    int trkaminpl = trka.MinPlane();
    int trkamaxpl = trka.MaxPlane();
    int trkbminpl = trkb.MinPlane();
    int trkbmaxpl = trkb.MaxPlane();
    int zplanes[4] = {trkaminpl, trkamaxpl, trkbminpl, trkbmaxpl};

    for(int iz = 0; iz < 4; ++iz){
      if(iz < 2){
        int plane = kgeo.MatchTrajectoryToPlaneInView(trkends[iz],trka.View(),trkaminpl,trkamaxpl);
        zplanes[iz] = plane;
      }
      else{
        int plane = kgeo.MatchTrajectoryToPlaneInView(trkends[iz],trkb.View(),trkbminpl,trkbmaxpl);
        zplanes[iz] = plane;
      }
    }

    // always joint the ends that are closest together
    std::vector<TVector3> traja(ntraja);
    for(int i = 0; i<ntraja; ++i){
      traja[i] = trka.TrajectoryPoint(i);
    }
    std::vector<TVector3> trajb(ntrajb);
    for(int i = 0; i<ntrajb; ++i){
      trajb[i] = trkb.TrajectoryPoint(i);
    }
    std::vector<TVector3> trajpts;
    bool sameplane = false;
    // get all of the hits on the end planes
    double plhitspetot[4] = {0, 0, 0, 0};
    for(unsigned int i = 0; i<trka.NCell(); ++i){
      if(trka.Cell(i)->Plane() == zplanes[0]){ plhitspetot[0]+=trka.Cell(i)->PE(); }
      if(trka.Cell(i)->Plane() == zplanes[1]){ plhitspetot[1]+=trka.Cell(i)->PE(); }
    }
    for(unsigned int i = 0; i<trkb.NCell(); ++i){
      if(trkb.Cell(i)->Plane() == zplanes[2]){ plhitspetot[2]+=trkb.Cell(i)->PE(); }
      if(trkb.Cell(i)->Plane() == zplanes[3]){ plhitspetot[3]+=trkb.Cell(i)->PE(); }
    }

    if(stst < stsp && stst < spst && stst< spsp){
      // link at start points
      if(zplanes[0] == zplanes[2]){ sameplane = true; }
      // check which end point is lower in z
      if(trajb[ntrajb-1].Z() < traja[ntraja-1].Z()){
        // trkb first then trka, but start with stop of b
        std::reverse(trajb.begin(),trajb.end());
        for(int i = 0; i<ntrajb-1;++i){
          trajpts.push_back(trajb[i]);
        }
        if(!sameplane){
          trajpts.push_back(trajb[ntrajb-1]);
          trajpts.push_back(traja[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[0]*traja[0]+plhitspetot[2]*trajb[ntrajb-1])*
            (1.0/(plhitspetot[0]+plhitspetot[2]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[0]); }
        }
        for(int i = 1; i<ntraja;++i){
          trajpts.push_back(traja[i]);
        }
      }
      else{
        // trka first then trkb, but start with stop of a
        std::reverse(traja.begin(),traja.end());
        for(int i = 0; i<ntraja-1;++i){
          trajpts.push_back(traja[i]);
        }
        if(!sameplane){
          trajpts.push_back(traja[ntraja-1]);
          trajpts.push_back(trajb[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[0]*traja[ntraja-1]+plhitspetot[2]*trajb[0])*
            (1.0/(plhitspetot[0]+plhitspetot[2]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[ntraja-1]); }
        }
        for(int i = 1; i<ntrajb;++i){
          trajpts.push_back(trajb[i]);
        }
      }
    }
    else if(stsp < stst &&  stsp < spst && stsp < spsp){
      // link at start in a stop in b
      if(zplanes[0] == zplanes[3]){ sameplane = true; }
      // check which end point is lower
      if(trajb[0].Z() < traja[ntraja-1].Z()){
        // trkb first then trka, start with start of b
        for(int i = 0; i<ntrajb-1;++i){
          trajpts.push_back(trajb[i]);
        }
        if(!sameplane){
          trajpts.push_back(trajb[ntrajb-1]);
          trajpts.push_back(traja[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[0]*traja[0]+plhitspetot[3]*trajb[ntrajb-1])*
            (1.0/(plhitspetot[0]+plhitspetot[3]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[0]); }
        }
        for(int i = 1; i<ntraja;++i){
          trajpts.push_back(traja[i]);
        }
      }
      else{
        // trka first then trkb, start with stop of a
        std::reverse(traja.begin(),traja.end());
        for(int i = 0; i<ntraja-1;++i){
          trajpts.push_back(traja[i]);
        }
        // add from stop in b
        std::reverse(trajb.begin(),trajb.end());
        if(!sameplane){
          trajpts.push_back(traja[ntraja-1]);
          trajpts.push_back(trajb[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[0]*traja[ntraja-1]+plhitspetot[3]*trajb[0])*
            (1.0/(plhitspetot[0]+plhitspetot[3]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[ntraja-1]); }
        }
        for(int i = 1; i<ntrajb;++i){
          trajpts.push_back(trajb[i]);
        }
      }
    }
    else if(spst< stst && spst< stsp && spst < spsp){
      // link at stop in a start in b
      if(zplanes[1] == zplanes[2]){ sameplane = true; }
      // check which end point is lower
      if(trajb[ntrajb-1].Z() < traja[0].Z()){
        // trkb first then trka, start with stop of b
        std::reverse(trajb.begin(),trajb.end());
        for(int i = 0; i<ntrajb-1;++i){
          trajpts.push_back(trajb[i]);
        }
        // add from stop in a
        std::reverse(traja.begin(),traja.end());
        if(!sameplane){
          trajpts.push_back(trajb[ntrajb-1]);
          trajpts.push_back(traja[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[1]*traja[0]+plhitspetot[2]*trajb[ntrajb-1])*
            (1.0/(plhitspetot[1]+plhitspetot[2]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[0]); }
        }
        for(int i = 1; i<ntraja;++i){
          trajpts.push_back(traja[i]);
        }
      }
      else{
        // trka first then trkb, start with start of a
        for(int i = 0; i<ntraja-1;++i){
          trajpts.push_back(traja[i]);
        }
        if(!sameplane){
          trajpts.push_back(traja[ntraja-1]);
          trajpts.push_back(trajb[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[1]*traja[ntraja-1]+plhitspetot[2]*trajb[0])*
            (1.0/(plhitspetot[1]+plhitspetot[2]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[ntraja-1]); }
        }
        // add from start in b
        for(int i = 1; i<ntrajb;++i){
          trajpts.push_back(trajb[i]);
        }
      }
    }
    else{
      // link at stop points
      if(zplanes[1] == zplanes[3]){ sameplane = true; }
     // check which end point is lower
      if(trajb[0].Z() < traja[0].Z()){
        // trkb first then trka,  start with start of b
        for(int i = 0; i<ntrajb-1;++i){
          trajpts.push_back(trajb[i]);
        }
        // add from stop in a
        std::reverse(traja.begin(),traja.end());
        if(!sameplane){
          trajpts.push_back(trajb[ntrajb-1]);
          trajpts.push_back(traja[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[1]*traja[0]+plhitspetot[3]*trajb[ntrajb-1])*
            (1.0/(plhitspetot[1]+plhitspetot[3]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[0]); }
        }
        for(int i = 1; i<ntraja;++i){
          trajpts.push_back(traja[i]);
        }
      }
      else{
        // trka first then trkb, start with start of a
        for(int i = 0; i<ntraja-1;++i){
          trajpts.push_back(traja[i]);
        }
        // add from stop in b
        std::reverse(trajb.begin(),trajb.end());
        if(!sameplane){
          trajpts.push_back(traja[ntraja-1]);
          trajpts.push_back(trajb[0]);
        }
        else{
          TVector3 mergetrj = (plhitspetot[1]*traja[ntraja-1]+plhitspetot[3]*trajb[0])*
            (1.0/(plhitspetot[1]+plhitspetot[3]));
          if(!std::isnan(mergetrj.X()) && !std::isnan(mergetrj.Y()) && !std::isnan(mergetrj.Z())){ trajpts.push_back(mergetrj); }
          else{ trajpts.push_back(traja[ntraja-1]); }
        }
        for(int i = 1; i<ntrajb;++i){
          trajpts.push_back(trajb[i]);
        }
      }
    }
    TVector3 dir = trajpts[1]-trajpts[0];
    rb::Track JointTrack(JointCluster,trajpts[0].X(),trajpts[0].Z(),dir.X(),dir.Z(),trka.ID());
    if(trka.View() == geo::kY){
      JointTrack.SetStart(trajpts[0].Y(),trajpts[0].Z());
      JointTrack.SetDir(dir.Y(),dir.Z());
    }
    for(unsigned int i = 1; i<trajpts.size();++i){
      if(trka.View() == geo::kX){ JointTrack.AppendTrajectoryPoint(trajpts[i].X(),trajpts[i].Z()); }
      else if(trka.View() == geo::kY){ JointTrack.AppendTrajectoryPoint(trajpts[i].Y(),trajpts[i].Z()); }
    }
    return JointTrack;
  }

  ///////////////////////////////////////////////////////////////////////
  double   KalmanTrackMerge::CalcMatchScore(KalmanGeoHelper& kgeo,rb::Track trka, rb::Track trkb)
  {
    double score = -1.0;
    rb::Track xTrack;
    rb::Track yTrack;
    if(trka.View() == geo::kX && trkb.View() == geo::kY){
      xTrack = trka;
      yTrack = trkb;
    }
    else if(trka.View() == geo::kY && trkb.View() == geo::kX){
      xTrack = trkb;
      yTrack = trka;
    }
    else{
      // don't have one x track and one y track, can't merge these return meaningless value for score
      return score;
    }
    
    // check that trajectory points are good
    bool xgood = true;
    for(unsigned int i = 0; i<xTrack.NTrajectoryPoints();++i){
      if(std::isnan(xTrack.TrajectoryPoint(i).X()) || std::isnan(xTrack.TrajectoryPoint(i).Y()) || std::isnan(xTrack.TrajectoryPoint(i).Z())){
        xgood = false;
      }
    }
    
    bool ygood = true;
    for(unsigned int i = 0; i<yTrack.NTrajectoryPoints();++i){
      if(std::isnan(yTrack.TrajectoryPoint(i).X()) || std::isnan(yTrack.TrajectoryPoint(i).Y()) || std::isnan(yTrack.TrajectoryPoint(i).Z())){
        ygood = false;
      }
    }
    if(!xgood || !ygood){return score;}
    
    TVector3 ixtraj = xTrack.Start();
    TVector3 fxtraj = xTrack.Stop();
    TVector3 iytraj = yTrack.Start();
    TVector3 fytraj = yTrack.Stop();

    // make sure the iZ is before the fZ
    if(fxtraj.Z() < ixtraj.Z()){ std::swap(ixtraj,fxtraj); }
    if(fytraj.Z() < iytraj.Z()){ std::swap(iytraj,fytraj); }

    // get the corresponding planes to these z locations
    int ixPlane = kgeo.MatchTrajectoryToPlaneInView(ixtraj,xTrack.View(),xTrack.MinPlane(),xTrack.MaxPlane());
    int fxPlane = kgeo.MatchTrajectoryToPlaneInView(fxtraj,xTrack.View(),xTrack.MinPlane(),xTrack.MaxPlane());
    int iyPlane = kgeo.MatchTrajectoryToPlaneInView(iytraj,yTrack.View(),yTrack.MinPlane(),yTrack.MaxPlane());
    int fyPlane = kgeo.MatchTrajectoryToPlaneInView(fytraj,yTrack.View(),yTrack.MinPlane(),yTrack.MaxPlane());

    // apply basic z geometry cuts to ensure that the two tracks at least have some minimal overlap
    if(iyPlane <= fxPlane && fyPlane >= ixPlane){
      if(yTrack.ExtentPlane()>1 && xTrack.ExtentPlane()>1){
        // find the overlap between the tracks
        int minoverlapplane;
        int maxoverlapplane;
        if(ixPlane<iyPlane){
          minoverlapplane = iyPlane;
          if(fxPlane<fyPlane){ maxoverlapplane = fxPlane; }
          else{ maxoverlapplane = fyPlane; }
        }
        else{
          minoverlapplane = ixPlane;
          if(fxPlane<fyPlane){ maxoverlapplane = fxPlane; }
          else{ maxoverlapplane = fyPlane; }
        }
        // get all of the xtrack and ytrack hits
        std::vector<art::Ptr<rb::CellHit> > allhits;
        for(unsigned int ihit = 0; ihit<trka.NCell(); ++ihit){
          allhits.push_back(trka.Cell(ihit));
        }
        for(unsigned int ihit = 0; ihit<trkb.NCell(); ++ihit){
          allhits.push_back(trkb.Cell(ihit));
        }
        // sort the cellhits by plane
        rb::SortByPlane(allhits);

        double overlap = 0;
        for(int iPlane = minoverlapplane; iPlane<=maxoverlapplane; ++iPlane){
          // for this plane to be overlapped, must have a hit on previous plane and next plane, also need a hit this plane
          bool hitcurr = false;
          bool hitprev = false;
          bool hitnext = false;
          for(unsigned int ihit = 0; ihit < allhits.size();++ihit){
            if(!hitcurr && allhits[ihit]->Plane() == iPlane){ hitcurr = true; }
            if(!hitprev && allhits[ihit]->Plane() == iPlane-1){ hitprev = true; }
            if(!hitnext && allhits[ihit]->Plane() == iPlane+1){hitnext = true; }
          }
          if(hitcurr && hitprev && hitnext){ overlap+=1.0; }

        }
        int idiff = TMath::Abs(iyPlane-ixPlane);
        int fdiff = TMath::Abs(fyPlane-fxPlane);

        score = ((double)idiff+fdiff)/overlap;
      }
      else if(yTrack.ExtentPlane() == 1){
        // for a vertical y track, matching x track must only appear in the two or less surronding planes
        // check that the x track only has hits in the plane before and after the y track
        if(ixPlane == (iyPlane-1) && fxPlane == (iyPlane+1)){score = 1.0;}
      }
      else{
        // we are in the case of a vertical x track only, matching y track must only appear in the two surronding planes only
        // check that the y track only has hits in the plane before and after the y track
        if(iyPlane == (ixPlane-1) && fyPlane == (ixPlane+1)){score = 1.0;}
      }
    }
    else if(ixPlane == fxPlane && iyPlane == fyPlane){
      // if x and y tracks appear in only one plane, allow match only if it is adjacent plane
      if(ixPlane == (iyPlane-1) || ixPlane == (iyPlane+1)){score = 1.0;}
    }
    return score;
  }

  ///////////////////////////////////////////////////////////////////////////
  rb::Track   KalmanTrackMerge::ViewMergeTracks(KalmanGeoHelper& kgeo, rb::Track xTrack, rb::Track yTrack)
  {
    // make a 3d track out of the x and y tracks
    rb::Track newtrack;
    // add all of the hits to the track.
    for(unsigned int i = 0; i<xTrack.NCell();++i){
      newtrack.Add(xTrack.Cell(i));
    }
    for(unsigned int i = 0; i<yTrack.NCell();++i){
      newtrack.Add(yTrack.Cell(i));
    }
    newtrack.SetID(xTrack.ID());

    std::vector<int> xPlanes;
    int minxPlane = xTrack.MinPlane();
    int maxxPlane = xTrack.MaxPlane();

    std::vector<int> yPlanes;
    int minyPlane = yTrack.MinPlane();
    int maxyPlane = yTrack.MaxPlane();

    if(xTrack.ExtentPlane() > 1 && yTrack.ExtentPlane() > 1){
      // Make a list of all planes that the trajectory points belong to
      std::vector<TVector3> xtrjpoints;
      double minxz = 99999;
      double maxxz = 0;
      for(unsigned int i = 0; i<xTrack.NTrajectoryPoints();++i){
        int xplane = kgeo.MatchTrajectoryToPlane(xTrack.TrajectoryPoint(i),minxPlane,maxxPlane);
        xPlanes.push_back(xplane);
      }

      for(unsigned int i =0; i<xPlanes.size(); ++i){
        if(i == 0 || i == xPlanes.size()-1){
          xtrjpoints.push_back(xTrack.TrajectoryPoint(i));
          if(xtrjpoints.back().Z() > maxxz){ maxxz = xtrjpoints.back().Z(); }
          if(xtrjpoints.back().Z() < minxz){ minxz = xtrjpoints.back().Z(); }
        }
        else if(!(xPlanes[i] == xPlanes[i-1] && xPlanes[i] == xPlanes[i+1])){
          xtrjpoints.push_back(xTrack.TrajectoryPoint(i));
          if(xtrjpoints.back().Z() > maxxz){ maxxz = xtrjpoints.back().Z(); }
          if(xtrjpoints.back().Z() < minxz){ minxz = xtrjpoints.back().Z(); }
        }
      }

      std::vector<TVector3> ytrjpoints;
      double minyz = 99999;
      double maxyz = 0;
      for(unsigned int i = 0; i<yTrack.NTrajectoryPoints();++i){
        int yplane = kgeo.MatchTrajectoryToPlane(yTrack.TrajectoryPoint(i),minyPlane,maxyPlane);
        yPlanes.push_back(yplane);
      }

      for(unsigned int i = 0; i<yPlanes.size(); ++i){
        if(i == 0 || i == yPlanes.size()-1){
          ytrjpoints.push_back(yTrack.TrajectoryPoint(i));
          if(ytrjpoints.back().Z() > maxyz){ maxyz = ytrjpoints.back().Z(); }
          if(ytrjpoints.back().Z() < minyz){ minyz = ytrjpoints.back().Z(); }
        }
        else if(!(yPlanes[i] == yPlanes[i-1] && yPlanes[i] == yPlanes[i+1])){
            ytrjpoints.push_back(yTrack.TrajectoryPoint(i));
            if(ytrjpoints.back().Z() > maxyz){ maxyz = ytrjpoints.back().Z(); }
            if(ytrjpoints.back().Z() < minyz){ minyz = ytrjpoints.back().Z(); }
        }
      }

      // check if these trajectory points will cause infinite lengths when interpolated
      // check low z side first
      if(minyz < minxz){
        if(xtrjpoints.size() >= 2){
          unsigned int xplane = xTrack.MinPlane();
          double deltaz = fgeom->Plane(xplane)->Cell(0)->HalfD();
          if(xtrjpoints[0].Z() <= xtrjpoints[xtrjpoints.size()-1].Z()){
            TVector3 endtraj = xtrjpoints[0];
            TVector3 nexttraj = xtrjpoints[1];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,xplane,xTrack,lowztraj,highztraj);
              xtrjpoints[0] = lowztraj;
              xtrjpoints[1] = highztraj;
            }
          }
          else{
            TVector3 endtraj = xtrjpoints[xtrjpoints.size()-1];
            TVector3 nexttraj = xtrjpoints[xtrjpoints.size()-2];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,xplane,xTrack,lowztraj,highztraj);
              xtrjpoints[xtrjpoints.size()-1] = lowztraj;
              xtrjpoints[xtrjpoints.size()-2] = highztraj;
            }
          }
        }
      }
      else{
        if(ytrjpoints.size() >= 2){
          unsigned int yplane = yTrack.MinPlane();
          double deltaz = fgeom->Plane(yplane)->Cell(0)->HalfD();
          if(ytrjpoints[0].Z() <= ytrjpoints[ytrjpoints.size()-1].Z()){
            TVector3 endtraj = ytrjpoints[0];
            TVector3 nexttraj = ytrjpoints[1];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,yplane,yTrack,lowztraj,highztraj);
              ytrjpoints[0] = lowztraj;
              ytrjpoints[1] = highztraj;
            }
          }
          else{
            TVector3 endtraj = ytrjpoints[ytrjpoints.size()-1];
            TVector3 nexttraj = ytrjpoints[ytrjpoints.size()-2];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,yplane,yTrack,lowztraj,highztraj);
              ytrjpoints[ytrjpoints.size()-1] = lowztraj;
              ytrjpoints[ytrjpoints.size()-2] = highztraj;
            }
          }
        }
      }

      // now check the high z side for interpolation problems 
      if(maxyz > maxxz){
        if(xtrjpoints.size() >= 2){
          unsigned int xplane = xTrack.MaxPlane();
          double deltaz = fgeom->Plane(xplane)->Cell(0)->HalfD();
          if(xtrjpoints[0].Z() >= xtrjpoints[xtrjpoints.size()-1].Z()){
            TVector3 endtraj = xtrjpoints[0];
            TVector3 nexttraj = xtrjpoints[1];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,xplane,xTrack,lowztraj,highztraj);
              xtrjpoints[0] = highztraj;
              xtrjpoints[1] = lowztraj;
            }
          }
          else{
            TVector3 endtraj = xtrjpoints[xtrjpoints.size()-1];
            TVector3 nexttraj = xtrjpoints[xtrjpoints.size()-2];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,xplane,xTrack,lowztraj,highztraj);
              xtrjpoints[xtrjpoints.size()-1] = highztraj;
              xtrjpoints[xtrjpoints.size()-2] = lowztraj;
            }
          }
        }
      }
      else{
        if(ytrjpoints.size() >= 2){
          unsigned int yplane = yTrack.MaxPlane();
          double deltaz = fgeom->Plane(yplane)->Cell(0)->HalfD();
          if(ytrjpoints[0].Z() >= ytrjpoints[ytrjpoints.size()-1].Z()){
            TVector3 endtraj = ytrjpoints[0];
            TVector3 nexttraj = ytrjpoints[1];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,yplane,yTrack,lowztraj,highztraj);
              ytrjpoints[0] = highztraj;
              ytrjpoints[1] = lowztraj;
            }
          }
          else{
            TVector3 endtraj = ytrjpoints[ytrjpoints.size()-1];
            TVector3 nexttraj = ytrjpoints[ytrjpoints.size()-2];
            if(fabs(endtraj.Z()-nexttraj.Z()) <= 2.0*deltaz){
              TVector3 lowztraj, highztraj;
              ShiftInterpolationPoints(endtraj,nexttraj,yplane,yTrack,lowztraj,highztraj);
              ytrjpoints[ytrjpoints.size()-1] = highztraj;
              ytrjpoints[ytrjpoints.size()-2] = lowztraj;
            }
          }
        }
      }

      // Replace track trajectory points with the new trajectory points avoid bad interpolation
      xTrack.ClearTrajectoryPoints();
      for(unsigned int i = 0; i<xtrjpoints.size(); ++i){
        if(i == 0){ xTrack.SetStart(xtrjpoints[0].X(),xtrjpoints[0].Z()); }
        else{ xTrack.AppendTrajectoryPoint(xtrjpoints[i].X(),xtrjpoints[i].Z()); }
      }

      yTrack.ClearTrajectoryPoints();
      for(unsigned int i = 0; i<ytrjpoints.size(); ++i){
        if(i == 0){ yTrack.SetStart(ytrjpoints[0].Y(),ytrjpoints[0].Z()); }
        else{ yTrack.AppendTrajectoryPoint(ytrjpoints[i].Y(),ytrjpoints[i].Z()); }
      }

      // also check if either of these two tracks backscatter in z
      bool backscatterx = false;
      std::vector<int> xbackscatind;

      for(unsigned int i = 0; i<xtrjpoints.size();++i){
        if(i > 0 && i < xtrjpoints.size()-1){
          double deltaz1 = xtrjpoints[i].Z()-xtrjpoints[i-1].Z();
          double deltaz2 = xtrjpoints[i+1].Z()-xtrjpoints[i].Z();
          if((deltaz1*deltaz2)<0){ xbackscatind.push_back(i); }
        }
      }

      if(xbackscatind.size()>0){ backscatterx = true; }

      bool backscattery = false;
      std::vector<int> ybackscatind;
      for(unsigned int i = 0; i<ytrjpoints.size();++i){
        if(i > 0 && i < ytrjpoints.size()-1){
          double deltaz1 = ytrjpoints[i].Z()-ytrjpoints[i-1].Z();
          double deltaz2 = ytrjpoints[i+1].Z()-ytrjpoints[i].Z();
          if((deltaz1*deltaz2)<0){ ybackscatind.push_back(i); }
        }
      } 

      if(ybackscatind.size()>0){ backscattery = true; }

      std::vector<TVector3> trjpoints;
      if((!backscatterx && !backscattery) || (backscatterx && backscattery)){
        std::vector<TVector3> newtrjpoints;
        for(unsigned int i = 0; i<xtrjpoints.size();++i){
          double x, y;
          yTrack.InterpolateXY(xtrjpoints[i].Z(),x,y);
          xtrjpoints[i].SetY(y);
          newtrjpoints.push_back(xtrjpoints[i]);
        }
        for(unsigned int i = 0; i<ytrjpoints.size();++i){
          double x, y;
          xTrack.InterpolateXY(ytrjpoints[i].Z(),x,y);
          ytrjpoints[i].SetX(x);
          newtrjpoints.push_back(ytrjpoints[i]);
        }
        std::sort(newtrjpoints.begin(),newtrjpoints.end(),sort_traj);
        trjpoints = newtrjpoints;
      }
      if((backscatterx && !backscattery)){
        // no backscatter in y can safely use the interpolate function for the y position of x hits trajectories
        std::vector<TVector3> newtrjpoints;
        for(unsigned int i = 0; i<xtrjpoints.size();++i){
          double x, y;
          yTrack.InterpolateXY(xtrjpoints[i].Z(),x,y);
          xtrjpoints[i].SetY(y);
          newtrjpoints.push_back(xtrjpoints[i]);
        }

        // insert the start and end points of the x track into the list of backscatters, these serve as end points
        xbackscatind.insert(xbackscatind.begin(),0);
        xbackscatind.push_back(xtrjpoints.size()-1);
        // check how many backscatters are there
        int nback = xbackscatind.size();

        // Define a vector with one entry per y trajectory point. The entry is a vector holding
        // which xsection(s) the y trajectory point belongs to
        std::vector<std::vector<int> > xsectytrj(ytrjpoints.size());
        for(unsigned int iytraj = 0; iytraj<ytrjpoints.size(); ++iytraj){
          double zy = ytrjpoints[iytraj].Z();
          bool foundsect = false;
          // Is this point in the overlap of any of the x track sections?
          for(unsigned int ixbrk = 0; ixbrk<xbackscatind.size()-1; ++ixbrk){
            double z1 = xtrjpoints[xbackscatind[ixbrk]].Z();
            double z2 = xtrjpoints[xbackscatind[ixbrk+1]].Z();
            if( (z1 < zy && zy < z2) || (z1 > zy && zy > z2) ){
              xsectytrj[iytraj].push_back(ixbrk);
              foundsect = true;
            }
          }
          if(!foundsect){
            // can't find an overlapping x track section this is either higher or lower in z than any x hits, pick the closest section
            double closestdist = (xtrjpoints[xbackscatind[0]].Z() - zy);
            int closestxsect = 0;
            for(unsigned int ixbrk = 0; ixbrk<xbackscatind.size(); ++ixbrk){
              double dist = fabs(xtrjpoints[xbackscatind[ixbrk]].Z() - zy);
              if(dist<closestdist){
                closestxsect = ixbrk;
                closestdist = dist;
              }
            }
            xsectytrj[iytraj].push_back(closestxsect);
          }
        }

        std::vector<TVector3> orderedtrjpoints;
        // loop over the x sections in the order that we get them
        for(int isect = 0; isect<nback; ++isect){
          std::vector<TVector3> xsecttrjs;
          std::vector<TVector3> ysecttrjs;
          // get all the x trajectory points that belong to this section
          // this should just be all the x trajectory points between the backscatters
          if(isect != nback-1){
            for(int ix = xbackscatind[isect]; ix<=xbackscatind[isect+1]; ++ix){
              xsecttrjs.push_back(xtrjpoints[ix]);
            }
          }
          else{
            xsecttrjs.push_back(xtrjpoints[xbackscatind[isect]-1]);
            xsecttrjs.push_back(xtrjpoints[xbackscatind[isect]]);
          }
          // find all of the ytrajectory points that belong to this section
          for(unsigned int iy = 0; iy<xsectytrj.size(); ++iy){
            for(unsigned int iyx = 0; iyx<xsectytrj[iy].size(); ++iyx){
              if(xsectytrj[iy][iyx] == isect){ ysecttrjs.push_back(ytrjpoints[iy]); }
            }
          }
          // now we need to interpolate the x point of all the ytrajectory points
          for(unsigned int iy = 0; iy<ysecttrjs.size(); ++iy){
            // get the zvalue of this point
            double zy = ysecttrjs[iy].Z();
            bool foundngbr = false;
            // loop over the x trajectory points and find the closest points to this zy value
            for(unsigned int ix = 0; ix<xsecttrjs.size()-1; ++ix){
              double z1 = xsecttrjs[ix].Z();
              double z2 = xsecttrjs[ix+1].Z();
              if( (z1 < zy && zy < z2) || (z1 > zy && zy > z2) ){
                foundngbr = true;
                double xint = xsecttrjs[ix].X() + (zy- z1)*(xsecttrjs[ix+1].X()-xsecttrjs[ix].X())/(z2-z1);
                ysecttrjs[iy].SetX(xint);
                break;
              }         
            }
            if(!foundngbr){
              // the z position is off the end of the track use two closest pts to interpolate
              if(fabs(xsecttrjs[0].Z() - zy) < fabs(xsecttrjs[xsecttrjs.size()-1].Z() - zy)){
                double x1 = xsecttrjs[0].X();
                double x2 = xsecttrjs[1].X();
                double z1 = xsecttrjs[0].Z();
                double z2 = xsecttrjs[1].Z();
                double xint = x1 + (zy-z1)*(x2-x1)/(z2-z1);
                ysecttrjs[iy].SetX(xint);
              }
              else{
                double x1 = xsecttrjs[xsecttrjs.size()-1].X();
                double x2 = xsecttrjs[xsecttrjs.size()-2].X();
                double z1 = xsecttrjs[xsecttrjs.size()-1].Z();
                double z2 = xsecttrjs[xsecttrjs.size()-2].Z();
                double xint = x1 + (zy-z1)*(x2-x1)/(z2-z1);
                ysecttrjs[iy].SetX(xint);
              }
            }
          }

          std::vector<TVector3> alltrjs;
          for(unsigned int i = 0; i<xsecttrjs.size();++i){
            if(isect < nback-1){
              if(i != xsecttrjs.size()-1){ alltrjs.push_back(xsecttrjs[i]); }
            }
            else{
              if(i != 0){ alltrjs.push_back(xsecttrjs[i]); }
            }

          }
          for(unsigned int i = 0; i<ysecttrjs.size();++i){
            alltrjs.push_back(ysecttrjs[i]);
          }
          // now order all of the trajectory points in this section
          double zDir = xsecttrjs[xsecttrjs.size()-1].Z() - xsecttrjs[0].Z();
          std::sort(alltrjs.begin(),alltrjs.end(),sort_traj);
          if(zDir < 0){ reverse(alltrjs.begin(),alltrjs.end()); }
          for(unsigned int i = 0; i<alltrjs.size(); ++i){
            orderedtrjpoints.push_back(alltrjs[i]);
          }
        }
        trjpoints = orderedtrjpoints;
      }
      if((!backscatterx && backscattery)){
        // no backscatter in x can safely use the interpolate function for the x position of y hits trajectories
        std::vector<TVector3> newtrjpoints;
        for(unsigned int i = 0; i<ytrjpoints.size();++i){
          double x, y;
          xTrack.InterpolateXY(ytrjpoints[i].Z(),x,y);
          ytrjpoints[i].SetX(x);
          newtrjpoints.push_back(ytrjpoints[i]);
        }

        // insert the start and end points of the y track into the list of backscatters, these serve as end points
        ybackscatind.insert(ybackscatind.begin(),0);
        ybackscatind.push_back(ytrjpoints.size()-1);
        // check how many backscatters are there
        int nback = ybackscatind.size();

        // Define a vector with one entry per x trajectory point. The entry is a vector holding
        // which ysection(s) the x trajectory point belongs to
        std::vector<std::vector<int> > ysectxtrj(xtrjpoints.size());
        for(unsigned int ixtraj = 0; ixtraj<xtrjpoints.size(); ++ixtraj){
          double zx = xtrjpoints[ixtraj].Z();
          bool foundsect = false;
          // Is this point in the overlap of any of the x track sections?
          for(unsigned int iybrk = 0; iybrk<ybackscatind.size()-1; ++iybrk){
            double z1 = ytrjpoints[ybackscatind[iybrk]].Z();
            double z2 = ytrjpoints[ybackscatind[iybrk+1]].Z();
            if( (z1 < zx && zx < z2) || (z1 > zx && zx > z2) ){
              ysectxtrj[ixtraj].push_back(iybrk);
              foundsect = true;
            }
          }
          if(!foundsect){
            // can't find an overlapping y track section this is either higher or lower in z than any y hits, pick the closest section
            double closestdist = (ytrjpoints[ybackscatind[0]].Z() - zx);
            int closestysect = 0;
            for(unsigned int iybrk = 0; iybrk<ybackscatind.size(); ++iybrk){
              double dist = fabs(ytrjpoints[ybackscatind[iybrk]].Z() - zx);
              if(dist<closestdist){
                closestysect = iybrk;
                closestdist = dist;
              }
            }
            ysectxtrj[ixtraj].push_back(closestysect);
          }
        }

        std::vector<TVector3> orderedtrjpoints;
        // loop over the y sections in order that we get them
        for(int isect = 0; isect<nback; ++isect){
          std::vector<TVector3> xsecttrjs;
          std::vector<TVector3> ysecttrjs;
          // get all the y trajectory points that belong to this section
          // this should just be all the y trajectory points between the backscatters
          if(isect != nback-1){
            for(int iy = ybackscatind[isect]; iy<=ybackscatind[isect+1]; ++iy){
              ysecttrjs.push_back(ytrjpoints[iy]);
            }
          }
          else{
            ysecttrjs.push_back(ytrjpoints[ybackscatind[isect]-1]);
            ysecttrjs.push_back(ytrjpoints[ybackscatind[isect]]);
          }
          // find all of the xtrajectory points that belong to this section
          for(unsigned int ix = 0; ix<ysectxtrj.size(); ++ix){
            for(unsigned int ixy = 0; ixy<ysectxtrj[ix].size(); ++ixy){
              if(ysectxtrj[ix][ixy] == isect){ xsecttrjs.push_back(xtrjpoints[ix]); }
            }
          }
          // now we need to interpolate the y point of all the xtrajectory points
          for(unsigned int ix = 0; ix<xsecttrjs.size(); ++ix){
            // get the zvalue of this point
            double zx = xsecttrjs[ix].Z();
            bool foundngbr = false;
            // loop over the y trajectory points and find the closest points to this zx value
            for(unsigned int iy = 0; iy<ysecttrjs.size()-1; ++iy){
              double z1 = ysecttrjs[iy].Z();
              double z2 = ysecttrjs[iy+1].Z();
              if( (z1 < zx && zx < z2) || (z1 > zx && zx > z2) ){
                foundngbr = true;
                double yint = ysecttrjs[iy].Y() + (zx- z1)*(ysecttrjs[iy+1].Y()-ysecttrjs[iy].Y())/(z2-z1);
                xsecttrjs[ix].SetY(yint);
                break;
              }         
            }
            if(!foundngbr){
              // the z position is off the end of the track use two closest pts to interpolate
              if(fabs(ysecttrjs[0].Z() - zx) < fabs(ysecttrjs[ysecttrjs.size()-1].Z() - zx)){
                double y1 = ysecttrjs[0].Y();
                double y2 = ysecttrjs[1].Y();
                double z1 = ysecttrjs[0].Z();
                double z2 = ysecttrjs[1].Z();
                double yint = y1 + (zx-z1)*(y2-y1)/(z2-z1);
                xsecttrjs[ix].SetY(yint);
              }
              else{
                double y1 = ysecttrjs[ysecttrjs.size()-1].Y();
                double y2 = ysecttrjs[ysecttrjs.size()-2].Y();
                double z1 = ysecttrjs[ysecttrjs.size()-1].Z();
                double z2 = ysecttrjs[ysecttrjs.size()-2].Z();
                double yint = y1 + (zx-z1)*(y2-y1)/(z2-z1);
                xsecttrjs[ix].SetY(yint);
              }
            }
          }

          std::vector<TVector3> alltrjs;
          for(unsigned int i = 0; i<ysecttrjs.size();++i){
            if(isect < nback-1){
              if(i != ysecttrjs.size()-1){
                alltrjs.push_back(ysecttrjs[i]);
              }
            }
            else{
              if(i != 0){
                alltrjs.push_back(ysecttrjs[i]);
              }
            }
          }
          for(unsigned int i = 0; i<xsecttrjs.size();++i){
            alltrjs.push_back(xsecttrjs[i]);
          }
          // now order all of the trajectory points in this section
          double zDir = ysecttrjs[ysecttrjs.size()-1].Z() - ysecttrjs[0].Z();
          std::sort(alltrjs.begin(),alltrjs.end(),sort_traj);
          if(zDir < 0){ reverse(alltrjs.begin(),alltrjs.end()); }
          for(unsigned int i = 0; i<alltrjs.size(); ++i){
            orderedtrjpoints.push_back(alltrjs[i]);
          }
        }
        trjpoints = orderedtrjpoints;
      }

      newtrack.SetStart(trjpoints[0]);
      for(unsigned int i = 1; i<trjpoints.size();++i){
        newtrack.AppendTrajectoryPoint(trjpoints[i]);
      }
      
      double tanThetax = TMath::Tan(TMath::ACos(xTrack.Dir().X()));
      double tanThetay = TMath::Tan(TMath::ACos(yTrack.Dir().Y()));
      double cosThetax = 1/(tanThetax*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
      double cosThetay = 1/(tanThetay*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
      TVector3 newDir(cosThetax,cosThetay,sqrt(1-cosThetax*cosThetax-cosThetay*cosThetay));
      if(std::isnan(newDir.X()) || std::isnan(newDir.Y()) || std::isnan(newDir.Z())){
        if(trjpoints.size() >= 2){
          newDir.SetXYZ(trjpoints[1].X()-trjpoints[0].X(),trjpoints[1].Y()-trjpoints[0].Y(),trjpoints[1].Z()-trjpoints[0].Z());
          newDir.Unit();
        }
      }
      newtrack.SetDir(newDir);
    }
    else if(xTrack.ExtentPlane() == 1){
      // check if both tracks only exist in 1 plane
      if(yTrack.ExtentPlane() == 1){
        // both tracks are in one plane only, assume goes high to low cell
        double yHigh[3];
        double yLow[3];
        fgeom->Plane(yTrack.MinPlane())->Cell(yTrack.MinCell(geo::kY))->GetCenter(yLow);
        fgeom->Plane(yTrack.MaxPlane())->Cell(yTrack.MaxCell(geo::kY))->GetCenter(yHigh);
        double ydeltaz = fgeom->Plane(yTrack.MinPlane())->Cell(yTrack.MinCell(geo::kY))->HalfD();
        double yzlow = yLow[2]-ydeltaz;
        double ylow = yLow[1];
        double yzhigh = yHigh[2]+ydeltaz;
        double yhigh = yHigh[1];
        double xHigh[3];
        double xLow[3];
        fgeom->Plane(xTrack.MinPlane())->Cell(xTrack.MinCell(geo::kX))->GetCenter(xLow);
        fgeom->Plane(xTrack.MaxPlane())->Cell(xTrack.MaxCell(geo::kX))->GetCenter(xHigh);
        double xdeltaz  = fgeom->Plane(xTrack.MinPlane())->Cell(xTrack.MinCell(geo::kX))->HalfD();
        double xzlow = xLow[2]-xdeltaz;
        double xlow = xLow[0];
        double xzhigh = xHigh[2]+xdeltaz;
        double xhigh = xHigh[0];
        if(yzlow<xzlow){
          TVector3 start(xlow+(xhigh-xlow)*(yzlow-xzlow)/(xzhigh-xzlow),ylow,yzlow);
          TVector3 stop(xhigh,yhigh+(yhigh-ylow)*(xzhigh-yzhigh)/(yzhigh-yzlow),xzhigh);
          newtrack.SetStart(start);
          newtrack.AppendTrajectoryPoint(stop);
          TVector3 diffs(stop.X()-start.X(),stop.Y()-start.Y(),stop.Z()-start.Z());
          newtrack.SetDir(diffs);
        }
        else{
          TVector3 start(xlow,(yhigh-ylow)*(xzlow-yzlow)/(yzhigh-yzlow)+ylow,xzlow);
          TVector3 stop(xhigh+(xhigh-xlow)*(yzhigh-xzhigh)/(xzhigh-xzlow),yhigh,yzhigh);
          newtrack.SetStart(start);
          newtrack.AppendTrajectoryPoint(stop);
          TVector3 diffs(stop.X()-start.X(),stop.Y()-start.Y(),stop.Z()-start.Z());
          newtrack.SetDir(diffs);
        }
      }
      else{
        // only the xtrack is in one plane
        // use the center of the top most and bottom most cells as the trajectory points (assume high to low going)
        double xHigh[3];
        double xLow[3];
        fgeom->Plane(xTrack.MinPlane())->Cell(xTrack.MinCell(geo::kX))->GetCenter(xLow);
        fgeom->Plane(xTrack.MaxPlane())->Cell(xTrack.MaxCell(geo::kX))->GetCenter(xHigh);
        double xdeltaz  = fgeom->Plane(xTrack.MinPlane())->Cell(xTrack.MinCell(geo::kX))->HalfD();
        double xzlow = xLow[2]-xdeltaz;
        double xlow = xLow[0];
        double xzhigh = xHigh[2]+xdeltaz;
        double xhigh = xHigh[0];
        double xslope = (xhigh-xlow)/(xzhigh-xzlow);
        for(unsigned int ytraj = 0;ytraj<yTrack.NTrajectoryPoints();++ytraj){
          double y = yTrack.TrajectoryPoint(ytraj).Y();
          double z = yTrack.TrajectoryPoint(ytraj).Z();
          TVector3 start(xlow+xslope*(z-xzlow),y,z);
          if(ytraj == 0){newtrack.SetStart(start);}
          else{newtrack.AppendTrajectoryPoint(start);}
        }
        double tanThetax = TMath::Tan(TMath::ACos(xslope/sqrt(1+xslope*xslope)));
        double tanThetay = TMath::Tan(TMath::ACos(yTrack.Dir().Y()));
        double cosThetax = 1/(tanThetax*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
        double cosThetay = 1/(tanThetay*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
        TVector3 newDir(cosThetax,cosThetay,sqrt(1-cosThetax*cosThetax-cosThetay*cosThetay));   
        newtrack.SetDir(newDir);
      }
    }
    else{
      // only the ytrack is in one plane
      // use the center of the top most and bottom most cells as the trajectory points (assume high to low going)
      double yHigh[3];
      double yLow[3];
      fgeom->Plane(yTrack.MinPlane())->Cell(yTrack.MinCell(geo::kY))->GetCenter(yLow);
      fgeom->Plane(yTrack.MaxPlane())->Cell(yTrack.MaxCell(geo::kY))->GetCenter(yHigh);
      double deltaz  = fgeom->Plane(yTrack.MinPlane())->Cell(yTrack.MinCell(geo::kY))->HalfD();
      double yzlow = yLow[2]-deltaz;
      double ylow = yLow[1];
      double yzhigh = yHigh[2]+deltaz;
      double yhigh = yHigh[1];
      double yslope = (yhigh-ylow)/(yzhigh-yzlow);
      for(unsigned int xtraj = 0;xtraj<xTrack.NTrajectoryPoints();++xtraj){
        double x = xTrack.TrajectoryPoint(xtraj).X();
        double z = xTrack.TrajectoryPoint(xtraj).Z();
        TVector3 start(x,ylow+yslope*(z-yzlow),z);
        if(xtraj == 0){newtrack.SetStart(start);}
        else{ newtrack.AppendTrajectoryPoint(start);}
      }
      double tanThetay = TMath::Tan(TMath::ACos(yslope/sqrt(1+yslope*yslope)));
      double tanThetax = TMath::Tan(TMath::ACos(xTrack.Dir().X()));
      double cosThetax = 1/(tanThetax*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
      double cosThetay = 1/(tanThetay*sqrt(1+1.0/(tanThetax*tanThetax)+1.0/(tanThetay*tanThetay)));
      TVector3 newDir(cosThetax,cosThetay,sqrt(1-cosThetax*cosThetax-cosThetay*cosThetay));     
      newtrack.SetDir(newDir);
    }
    return newtrack;
  } // end of ViewMergeTracks function

  //////////////////////////////////////////////////////////
  bool KalmanTrackMerge::sort_traj(const TVector3& a, const TVector3& b)
  {
    return a.Z()<b.Z();
  }
  //////////////////////////////////////////////////////////
  
  void KalmanTrackMerge::ShiftInterpolationPoints(TVector3 endtraj, TVector3 nexttraj,
                                                  unsigned int plane, rb::Track track,
                                                  TVector3 &lowztraj, TVector3 &highztraj)
  {
        
    // need to adjust these trajectory points so interpolation does something sensible
    // get the min/max cells on this plane
    unsigned int mincell = 10000;
    unsigned int maxcell = 0;
    for(unsigned int icell = 0; icell < track.NCell(); ++icell){
      if(track.Cell(icell)->Plane() == plane){
        if(track.Cell(icell)->Cell() > maxcell){ maxcell = track.Cell(icell)->Cell(); }
        if(track.Cell(icell)->Cell() < mincell){ mincell = track.Cell(icell)->Cell(); }
      }
    }
    double xyzLow[3];
    double xyzHigh[3];
    fgeom->Plane(plane)->Cell(mincell)->GetCenter(xyzLow);
    fgeom->Plane(plane)->Cell(maxcell)->GetCenter(xyzHigh);
    double deltaz = fgeom->Plane(plane)->Cell(mincell)->HalfD();
    double zmid = 0.5*xyzLow[2]+0.5*xyzHigh[2];
    lowztraj.SetZ(zmid-deltaz);
    highztraj.SetZ(zmid+deltaz);
    if(track.View() == geo::kX){
      if((endtraj-track.TrajectoryPoint(0)).Mag() <= (endtraj-track.TrajectoryPoint(track.NTrajectoryPoints()-1)).Mag()){
        if(endtraj.X() <= nexttraj.X()){
          lowztraj.SetX(xyzLow[0]);
          highztraj.SetX(xyzHigh[0]);
        }
        else{
          lowztraj.SetX(xyzHigh[0]);
          highztraj.SetX(xyzLow[0]);
        }
      }
      else{
        if(endtraj.X() <= nexttraj.X()){
          highztraj.SetX(xyzLow[0]);
          lowztraj.SetX(xyzHigh[0]);
        }
        else{
          highztraj.SetX(xyzHigh[0]);
          lowztraj.SetX(xyzLow[0]);
        }
      }
    }
    else if(track.View() == geo::kY){
      if((endtraj-track.TrajectoryPoint(0)).Mag() <= (endtraj-track.TrajectoryPoint(track.NTrajectoryPoints()-1)).Mag()){
        if(endtraj.Y() <= nexttraj.Y()){
          lowztraj.SetY(xyzLow[1]);
          highztraj.SetY(xyzHigh[1]);
        }
        else{
          lowztraj.SetY(xyzHigh[1]);
          highztraj.SetY(xyzLow[1]);
        }
      }
      else{
        if(endtraj.Y() <= nexttraj.Y()){
          highztraj.SetY(xyzLow[1]);
          lowztraj.SetY(xyzHigh[1]);
        }
        else{
          highztraj.SetY(xyzHigh[1]);
          lowztraj.SetY(xyzLow[1]);
        }
      }
    }

  } // end of ShiftInterpolationPoints function


}

namespace trk{

  DEFINE_ART_MODULE(KalmanTrackMerge)

}