// $Id$
//
//    File: DTrackCandidate_factory.cc
// Created: Mon Jul 18 15:23:04 EDT 2005
// Creator: davidl (on Darwin wire129.jlab.org 7.8.0 powerpc)
//

/// Factory to match track candidates generated by the FDC- and CDC-specific 
/// track finding codes.  The code uses several algorithms in the following 
/// order of precedence:
///      Method 1:  Swims cdc candidates pointing in the forward direction 
///                 and tries to match them geometrically to fdc candidates
///                 (whose positions are reported at the cdc endplate in z).
///      Method 2:  Projects the fdc candidates into the cdc region and tries
///                 to match to the cdc hits in each cdc candidate that points 
///                 in the forward direction.
///      Method 3:  Similar to method 2, except that it projects a cdc 
///                 candidate into the FDC region.
///      Method 4:  Matches left-over fdc candidates to track candidates that 
///                 have already been added to the output list of candidates.
///      Method 5:  Matches cdc candidates that do not point toward the cdc
///                 endplate that were not matched by previous methods to fdc
///                 candidates.
///      Method 6:  Tries to match stray unused cdc hits with fdc candidates
///      Method 7:  Alternate method to match leftover fdc candidates that 
///                 have already been matched to other track candidates
/// If a match is found, the code attempts to improve the track parameters by
/// redoing the Riemann Circle fit with the additional hits.  If an fdc 
/// candidate is matched to a cdc candidate, or several previously unused hits
/// are added to the track, the code attempts to place the "start" position 
/// parameters of the track at a radius just outside the start counter.

#include <cmath>
using namespace std;

#include "DTrackCandidate_factory.h"
#include "DTrackCandidate_factory_CDC.h"
#include "DANA/DApplication.h"

#include <TROOT.h>
#include <TH2F.h>
#include <TMath.h>

#define CUT 10.
#define RADIUS_CUT 50.0
#define BEAM_VAR 0.01 // cm^2
#define EPS 0.001

//------------------
// cdc_fdc_match
//------------------
inline bool cdc_fdc_match(double p_fdc,double p_cdc,double dist){
  //double frac=fabs(1.-p_cdc/p_fdc);
  //double frac2=fabs(1.-p_fdc/p_cdc);
  double p=p_fdc;
  if (p_cdc <p ) p=p_cdc;
  if (dist<10. && dist < 1.75+1.75/p
      //&& (frac<0.5 || frac2<0.5)
      ) return true;
  return false;
}

//------------------
// SegmentSortByLayerincreasing
//------------------
inline bool SegmentSortByLayerincreasing(const DFDCSegment* const &segment1, const DFDCSegment* const &segment2) {
	// Compare DFDCSegment->DFDCPseudo[0]->DFDCWire->layer
	int layer1 = 100; // defaults just in case there is a segment with no hits
	int layer2 = 100;
	
	if(segment1->hits.size()>0)layer1=segment1->hits[0]->wire->layer;
	if(segment2->hits.size()>0)layer2=segment2->hits[0]->wire->layer;

	return layer1 < layer2;
}

//------------------
// CDCHitSortByLayerincreasing
//------------------
inline bool CDCHitSortByLayerincreasing(const DCDCTrackHit* const &hit1, const DCDCTrackHit* const &hit2) {
	// Used to sort CDC hits by layer (ring) with innermost layer hits first

	// if same ring, sort by wire number
	if(hit1->wire->ring == hit2->wire->ring)
	{
		if(hit1->wire->straw == hit2->wire->straw)
			return (hit1->dE > hit2->dE);
		return hit1->wire->straw < hit2->wire->straw;
	}

	return hit1->wire->ring < hit2->wire->ring;
}

//------------------
// FDCHitSortByLayerincreasing
//------------------
inline bool FDCHitSortByLayerincreasing(const DFDCPseudo* const &hit1, const DFDCPseudo* const &hit2) {
	// Used to sort CDC hits by layer (ring) with innermost layer hits first

	// if same ring, sort by wire number
	if(hit1->wire->layer == hit2->wire->layer)
	{
		if(hit1->wire->wire == hit2->wire->wire)
			return (hit1->dE > hit2->dE);
		return (hit1->wire->wire < hit2->wire->wire);
	}

	return hit1->wire->layer < hit2->wire->layer;
}

//------------------
// init
//------------------
jerror_t DTrackCandidate_factory::init(void)
{
	MAX_NUM_TRACK_CANDIDATES = 20;

	return NOERROR;
}

//------------------
// brun
//------------------
jerror_t DTrackCandidate_factory::brun(JEventLoop* eventLoop,int runnumber){
  DApplication* dapp=dynamic_cast<DApplication*>(eventLoop->GetJApplication());
  const DGeometry *dgeom  = dapp->GetDGeometry(runnumber);
  bfield = dapp->GetBfield();
  FactorForSenseOfRotation=(bfield->GetBz(0.,0.,65.)>0.)?-1.:1.;

  // Get the position of the exit of the CDC endplate from DGeometry
  double endplate_z,endplate_dz,endplate_rmin;
  dgeom->GetCDCEndplate(endplate_z,endplate_dz,endplate_rmin,endplate_rmax);
  cdc_endplate.SetZ(endplate_z+endplate_dz);

  dgeom->GetTargetZ(TARGET_Z);
 
   // Initialize the stepper
  stepper=new DMagneticFieldStepper(bfield);
  stepper->SetStepSize(1.0);

  DEBUG_HISTS=false;
  gPARMS->SetDefaultParameter("TRKFIND:DEBUG_HISTS",DEBUG_HISTS);

  if(DEBUG_HISTS){
    dapp->Lock();
    match_dist=(TH2F*)gROOT->FindObject("match_dist");
    if (!match_dist){
      match_dist=new TH2F("match_dist","Matching distance",
			  60,0.,60.,100,0,25.);
      match_dist->SetXTitle("r (cm)");
      match_dist->SetYTitle("#Deltar (cm)");
    }
    match_dist_vs_p=(TH2F*)gROOT->FindObject("match_dist_vs_p");
    if (!match_dist_vs_p) {
      match_dist_vs_p=new TH2F("match_dist_vs_p","Matching distance vs p",
			       50,0.,7.,100,0,25.);
      match_dist_vs_p->SetYTitle("#Deltar (cm)");
      match_dist_vs_p->SetXTitle("p (GeV/c)");
    }
    dapp->Unlock();
  }

  gPARMS->SetDefaultParameter("TRKFIND:MAX_NUM_TRACK_CANDIDATES", MAX_NUM_TRACK_CANDIDATES);
  
  DEBUG_LEVEL=0;
  gPARMS->SetDefaultParameter("TRKFIND:DEBUG_LEVEL", DEBUG_LEVEL);

  return NOERROR;
}

//------------------
// erun
//------------------
jerror_t DTrackCandidate_factory::erun(void)
{
  if (stepper) delete stepper;
        return NOERROR;
}

//------------------
// erun
//------------------
jerror_t DTrackCandidate_factory::fini(void)
{
 
  if (stepper) delete stepper;
        return NOERROR;
}



//------------------
// evnt
//------------------
jerror_t DTrackCandidate_factory::evnt(JEventLoop *loop, int eventnumber)
{
  // Clear private vectors
  cdctrackcandidates.clear();
  fdctrackcandidates.clear();
  mycdchits.clear();

  // Get the track candidates from the CDC and FDC candidate finders
  loop->Get(cdctrackcandidates, "CDC");
  loop->Get(fdctrackcandidates, "FDCCathodes");

  // List of cdc hits
  loop->Get(mycdchits);

  // Vectors for keeping track of cdc matches and projections to the endplate
  vector<unsigned int> cdc_forward_ids;
  vector<unsigned int> cdc_backward_ids;
  vector<DVector3> cdc_endplate_projections;
 
  // Vector to keep track of cdc hits used in candidates
  vector<unsigned int>used_cdc_hits(mycdchits.size());

  // vector to keep track of the matching status of each fdc candidate
  vector<int>forward_matches(fdctrackcandidates.size());
	
  // Normal vector for CDC endplate
  DVector3 norm(0,0,1);

  //Loop through the list of CDC candidates, flagging those that point toward 
  // the FDC.  The others are put in the final candidate list.
  for(unsigned int i=0; i<cdctrackcandidates.size(); i++){	
    const DTrackCandidate *srccan = cdctrackcandidates[i];
    DVector3 mom=srccan->momentum();
    DVector3 pos=srccan->position();
    double theta=mom.Theta();
       
    // Propagate track to CDC endplate
    if (theta<M_PI_4 && fdctrackcandidates.size()>0){
      cdc_forward_ids.push_back(i);

      //ProjectHelixToZ(cdc_endplate.z(),srccan->charge(),mom,pos); 
      stepper->SetCharge(srccan->charge());
      stepper->SwimToPlane(pos,mom,cdc_endplate,norm,NULL);
      cdc_endplate_projections.push_back(pos);
    }
    else{
      cdc_backward_ids.push_back(i);
    }
  }

  // Variables for candidate number accounting
  int num_forward_cdc_cands_remaining=cdc_forward_ids.size();
  int num_fdc_cands_remaining=fdctrackcandidates.size();
 
  // Loop through the list of FDC candidates looking for matches between the
  // CDC and the FDC in the transition region.
  if (num_forward_cdc_cands_remaining>0){
    for(unsigned int i=0; i<fdctrackcandidates.size(); i++){
      // Break out if there are no forward-pointing cdc candidates that have 
      // not already been matched to fdc candidates.
      if (num_forward_cdc_cands_remaining==0) break;
      
      // The current FDC candidate
      const DTrackCandidate *fdccan = fdctrackcandidates[i];
      vector<const DFDCSegment *>segments;
      fdccan->GetT(segments);
      sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);
      if (segments[0]->package!=0) continue;

      // Flag for matching status
      bool got_match=false;
    
      if (DEBUG_LEVEL>0) _DBG_ << "Attempting FDC/CDC matching method #1..." <<endl; 

      // First try the matching method that projects the cdc candidate to the
      // cdc endplate where the fdc candidates are reported.
      if (MatchMethod1(fdccan,cdc_forward_ids,cdc_endplate_projections,
		       used_cdc_hits)){
	if (DEBUG_LEVEL>0) _DBG_ << "... matched to FDC candidate #" << i <<endl;

	got_match=true;
      }
      else{  
	if (DEBUG_LEVEL>0) _DBG_ << "Attempting FDC/CDC matching method #2..." <<endl; 

	// Try to gather up stray CDC hits from candidates that were not 
	// matched with the previous algorithm by projecting the helical track
	// from the fdc into the cdc region
	if (MatchMethod2(fdccan,cdc_forward_ids,used_cdc_hits)){
	  if (DEBUG_LEVEL>0) _DBG_ << "... matched to FDC candidate #" << i <<endl;
	       
	  got_match=true;
	}
      }

      if (got_match){
	// Mark the FDC candidate as matched
	forward_matches[i]=1;
	num_fdc_cands_remaining--;
	
	// update number of unmatched forward-pointing cdc candidates
	num_forward_cdc_cands_remaining--;	
      }
    } // loop over fdc candidates
  } // check for forward-pointing cdc candidates
  
  // If starting with the fdc candidates did not lead to a complete set of
  // CDC-FDC matches, try looping over the remaining CDC candidates that point
  // toward the FDC.
  vector<int>cdc_forward_matches(cdc_forward_ids.size());
  for (unsigned int i=0;i<cdc_forward_ids.size();i++){ 
    const DTrackCandidate *cdccan=cdctrackcandidates[cdc_forward_ids[i]];
    cdc_forward_matches[i]=0;

    if (DEBUG_LEVEL>0) _DBG_ << "Attempting FDC/CDC matching method #3..." <<endl; 
    
    // This method projects the cdc track into the FDC region
    if (MatchMethod3(cdccan,forward_matches,used_cdc_hits)){
      num_fdc_cands_remaining--;

      //Mark the cdc track candidate as matched
      cdc_forward_matches[i]=1;

      if (DEBUG_LEVEL>0) _DBG_ << "... matched to CDC candidate #" << i <<endl;
    }
  } // loop over forward-pointing cdc candidates
  
  // Unmatched CDC track candidates
  for (unsigned int j=0;j<cdc_forward_ids.size();j++){
    if (cdc_forward_matches[j]==0){
      DTrackCandidate *can = new DTrackCandidate;
      const DTrackCandidate *cdccan = cdctrackcandidates[cdc_forward_ids[j]];
      vector<const DCDCTrackHit *>cdchits;
      cdccan->GetT(cdchits);
      sort(cdchits.begin(), cdchits.end(), CDCHitSortByLayerincreasing);
      
      can->setMomentum(cdccan->momentum());
      can->setPosition(cdccan->position());
      can->setCharge(cdccan->charge());
      
      for (unsigned int n=0;n<cdchits.size();n++){
	used_cdc_hits[cdccan->used_cdc_indexes[n]]=1;
	can->AddAssociatedObject(cdchits[n]);
      }
      can->chisq=cdccan->chisq;
      can->Ndof=cdccan->Ndof;
      _data.push_back(can);
    }	
  }
  
  for (unsigned int j=0;j<cdc_backward_ids.size();j++){	  
    DTrackCandidate *can = new DTrackCandidate;
    const DTrackCandidate *cdccan = cdctrackcandidates[cdc_backward_ids[j]]; 

    can->setMomentum(cdccan->momentum());
    can->setPosition(cdccan->position());
    can->setCharge(cdccan->charge());

    // Get the cdc hits and add them to the candidate
    vector<const DCDCTrackHit *>cdchits;
    cdccan->GetT(cdchits);
    sort(cdchits.begin(), cdchits.end(), CDCHitSortByLayerincreasing);
    for (unsigned int n=0;n<cdchits.size();n++){
      used_cdc_hits[cdccan->used_cdc_indexes[n]]=1;
      can->AddAssociatedObject(cdchits[n]);
    }
    can->chisq=cdccan->chisq;
    can->Ndof=cdccan->Ndof;
      
    // Sometimes the cdc track candidate parameters for tracks that are actually
    // going forward are so poor that they don't seem to point towards the cdc
    // end plate, so the previous matching methods fail.  Try one more time 
    // to match these to FDC segments...
    if (num_fdc_cands_remaining>0){
      if (DEBUG_LEVEL>0) _DBG_ << "Attempting FDC/CDC matching method #5..." 
			       <<endl;
      if (MatchMethod5(can,cdchits,forward_matches)){
	num_fdc_cands_remaining--;

	if (DEBUG_LEVEL>0) _DBG_ << "... matched to CDC candidate #" 
				 <<cdc_backward_ids[j]<<endl;

      }
    }

    _data.push_back(can);
  }	

  unsigned int num_unmatched_cdcs=0;
  for (unsigned int i=0;i<used_cdc_hits.size();i++){
    if (!used_cdc_hits[i]) num_unmatched_cdcs++;
  }

  // If there are FDC candidates remaining, use the best track candidate 
  // knowledge we have so far to do one final check for matches
  if (num_fdc_cands_remaining>0){
    for (unsigned int j=0;j<_data.size();j++){
      if (DEBUG_LEVEL>0) _DBG_ << "Attempting FDC/CDC matching method #4..." <<endl; 
      
      // Match to a track that is already in the combined list.  This track 
      // could have either FDC or CDC hits (or both) associated with it already.
      MatchMethod4(_data[j],forward_matches,num_fdc_cands_remaining);   
    } 
  }

  // Copy remaining FDC candidates to final candidate list
  if (num_fdc_cands_remaining>0){
    for (unsigned int i=0;i<forward_matches.size();i++){
      if (forward_matches[i]==0){
	num_fdc_cands_remaining--;
	forward_matches[i]=1;

	DTrackCandidate *can = new DTrackCandidate;
	const DTrackCandidate *fdccan = fdctrackcandidates[i];
	
	can->Ndof=fdccan->Ndof;
	can->chisq=fdccan->chisq;
	can->setMomentum(fdccan->momentum());
	can->setPosition(fdccan->position());
	can->setCharge(fdccan->charge());
	
	// Get the segment data
	vector<const DFDCSegment *>segments;
	fdccan->GetT(segments);
	sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);
      
	// Add the FDC hits to the final track candidate
	for (unsigned int m=0;m<segments.size();m++){
	  for (unsigned int n=0;n<segments[m]->hits.size();n++){
	    can->AddAssociatedObject(segments[m]->hits[n]);
	  }
	}

	// Try to match cdc hits that were not linked into track candidates
	// with the fdc candidate
	if (num_unmatched_cdcs>2 && segments[0]->package==0){
	  if (DEBUG_LEVEL>0) _DBG_ "Matching FDC candidates to stray CDC hits" 
			     <<endl;
	  MatchMethod6(i,can,segments,used_cdc_hits,num_unmatched_cdcs,
		       forward_matches,num_fdc_cands_remaining);
	}
	
	_data.push_back(can);
      } // check if fdc candidate already matched...
    }
  }

  if((int(_data.size()) > MAX_NUM_TRACK_CANDIDATES) && (MAX_NUM_TRACK_CANDIDATES >= 0))
	{
	  if (DEBUG_LEVEL>0) _DBG_ << "Number of candidates = " << _data.size()
				   << " > " <<  MAX_NUM_TRACK_CANDIDATES
				   << " --- skipping track fitting! " << endl;
	  for(size_t loc_i = 0; loc_i < _data.size(); ++loc_i)
	    delete _data[loc_i];
	  _data.clear();
  	}
  
  return NOERROR;
  }


// Obtain position and momentum at the exit of a given package using the 
// helical track model.
jerror_t DTrackCandidate_factory::GetPositionAndMomentum(
					      const DFDCSegment *segment,
					      DVector3 &pos, DVector3 &mom){
  // Position of track segment at last hit plane of package
  double x=segment->xc+segment->rc*cos(segment->Phi1);
  double y=segment->yc+segment->rc*sin(segment->Phi1);
  double z=segment->hits[0]->wire->origin.z();

  // Track parameters
  //double kappa=segment->q/(2.*segment->rc);
  double phi0=segment->phi0;
  double tanl=segment->tanl;
  double z0=segment->z_vertex;

  // Useful intermediate variables
  double cosp=cos(phi0);
  double sinp=sin(phi0);
  double sperp=(z-z0)/tanl;
  //double twoks=2.*kappa*sperp;
  double twoks=FactorForSenseOfRotation*segment->q*sperp/segment->rc;
  double sin2ks=sin(twoks);
  double cos2ks=cos(twoks); 

  // Get Bfield
  double B=fabs(bfield->GetBz(x,y,z));

  // Momentum
  double pt=0.003*B*segment->rc;
  double px=pt*(cosp*cos2ks-sinp*sin2ks);
  double py=pt*(sinp*cos2ks+cosp*sin2ks);
  double pz=pt*tanl;

  pos.SetXYZ(x,y,z);
  mom.SetXYZ(px,py,pz);

  return NOERROR;
}


// Get the position and momentum at a fixed radius from the beam line
jerror_t DTrackCandidate_factory::GetPositionAndMomentum(DHelicalFit &fit,
							 double Bz,
							 DVector3 &pos,
							 DVector3 &mom){
  double r2=90.0;
  double xc=fit.x0;
  double yc=fit.y0;
  double rc=fit.r0;
  double rc2=rc*rc;
  double xc2=xc*xc;
  double yc2=yc*yc;
  double xc2_plus_yc2=xc2+yc2;
  double a=(r2-xc2_plus_yc2-rc2)/(2.*rc);
  double temp1=yc*sqrt(xc2_plus_yc2-a*a);
  double temp2=xc*a;
  double cosphi_plus=(temp2+temp1)/xc2_plus_yc2;
  double cosphi_minus=(temp2-temp1)/xc2_plus_yc2;

  // Direction tangent and transverse momentum
  double tanl=fit.tanl;
  double pt=0.003*Bz*rc;

  if(!isfinite(temp1) || !isfinite(temp2)){
    // We did not find an intersection between the two circles, so return 
    // an error.  The values of mom and pos are not changed. 
    //    _DBG_ << endl;
    return VALUE_OUT_OF_RANGE;
  }

  double phi_plus=acos(cosphi_plus);
  double phi_minus=acos(cosphi_minus);
  double x_plus=xc+rc*cosphi_plus;
  double x_minus=xc+rc*cosphi_minus;
  double y_plus=yc+rc*sin(phi_plus);
  double y_minus=yc+rc*sin(phi_minus);

  // if the resulting radial position on the circle from the fit does not agree
  // with the radius to which we are matching, we have the wrong sign for phi+ 
  // or phi-
  double r2_plus=x_plus*x_plus+y_plus*y_plus;
  double r2_minus=x_minus*x_minus+y_minus*y_minus;  
  if (fabs(r2-r2_plus)>EPS){
    phi_plus*=-1.;
    y_plus=yc+rc*sin(phi_plus);
  }
  if (fabs(r2-r2_minus)>EPS){
    phi_minus*=-1.;
    y_minus=yc+rc*sin(phi_minus);
  }

  if (fit.h<0.){
    phi_minus=M_PI-phi_minus;
    pos.SetXYZ(x_minus,y_minus,0.); // z will be filled later
    mom.SetXYZ(pt*sin(phi_minus),pt*cos(phi_minus),pt*tanl);
  }
  else{ 
    phi_plus*=-1.;   
    //phi_plus=M_PI-phi_plus;
    pos.SetXYZ(x_plus,y_plus,0.); // z will be filled later
    mom.SetXYZ(pt*sin(phi_plus),pt*cos(phi_plus),pt*tanl);
  }

  return NOERROR;
}


// Get the position and momentum at a fixed radius from the beam line
jerror_t DTrackCandidate_factory::GetPositionAndMomentum(DHelicalFit &fit,
							 double Bz,
							 const DVector3 &origin,
							 DVector3 &pos,
							 DVector3 &mom){
  double r2=90.0;
  double xc=fit.x0;
  double yc=fit.y0;
  double rc=fit.r0;
  double rc2=rc*rc;
  double xc2=xc*xc;
  double yc2=yc*yc;
  double xc2_plus_yc2=xc2+yc2;
  double a=(r2-xc2_plus_yc2-rc2)/(2.*rc);
  double temp1=yc*sqrt(xc2_plus_yc2-a*a);
  double temp2=xc*a;
  double cosphi_plus=(temp2+temp1)/xc2_plus_yc2;
  double cosphi_minus=(temp2-temp1)/xc2_plus_yc2;

  // Direction tangent and transverse momentum
  double tanl=fit.tanl;
  double pt=0.003*Bz*rc;

  if(!isfinite(temp1) || !isfinite(temp2)){
    // We did not find an intersection between the two circles, so return 
    // an error.  The values of mom and pos are not changed. 
    //    _DBG_ << endl;
    return VALUE_OUT_OF_RANGE;
  }

  double phi_plus=acos(cosphi_plus);
  double phi_minus=acos(cosphi_minus);
  double x_plus=xc+rc*cosphi_plus;
  double x_minus=xc+rc*cosphi_minus;
  double y_plus=yc+rc*sin(phi_plus);
  double y_minus=yc+rc*sin(phi_minus);

  // if the resulting radial position on the circle from the fit does not agree
  // with the radius to which we are matching, we have the wrong sign for phi+ 
  // or phi-
  double r2_plus=x_plus*x_plus+y_plus*y_plus;
  double r2_minus=x_minus*x_minus+y_minus*y_minus;  
  if (fabs(r2-r2_plus)>EPS){
    phi_plus*=-1.;
    y_plus=yc+rc*sin(phi_plus);
  }
  if (fabs(r2-r2_minus)>EPS){
    phi_minus*=-1.;
    y_minus=yc+rc*sin(phi_minus);
  }

  // Choose phi- or phi+ depending on proximity to one of the cdc hits
  double xwire=origin.x();
  double ywire=origin.y();
  double dx=x_minus-xwire;
  double dy=y_minus-ywire;
  double d2_minus=dx*dx+dy*dy;
  dx=x_plus-xwire;
  dy=y_plus-ywire;
  double d2_plus=dx*dx+dy*dy;
 
  if (d2_plus>d2_minus){
    fit.h=-1.;
    phi_minus=M_PI-phi_minus;
    pos.SetXYZ(x_minus,y_minus,0.); // z will be filled later
    mom.SetXYZ(pt*sin(phi_minus),pt*cos(phi_minus),pt*tanl);

  }
  else{
    fit.h=1.;
    phi_plus*=-1.;   
    pos.SetXYZ(x_plus,y_plus,0.); // z will be fillted later
    mom.SetXYZ(pt*sin(phi_plus),pt*cos(phi_plus),pt*tanl);

  }
  
  return NOERROR;
}



// Find the position along a helical path at the z-position z
void DTrackCandidate_factory::ProjectHelixToZ(const double z,const double q,
					      const DVector3 &mom,
					      DVector3 &pos){
  double pt=mom.Perp();
  double phi=mom.Phi();
  double sinphi=sin(phi);
  double cosphi=cos(phi);
  double tanl=tan(M_PI_2-mom.Theta());
  double x0=pos.X();
  double y0=pos.Y();
  double z0=pos.Z();
  double B=fabs(bfield->GetBz(x0,y0,z0));
  double twokappa=FactorForSenseOfRotation*0.003*B*q/pt;
  double one_over_twokappa=1./twokappa;
  double sperp=(z-z0)/tanl;
  double twoks=twokappa*sperp;
  double sin2ks=sin(twoks);
  double one_minus_cos2ks=1.-cos(twoks);
  double x=x0+(cosphi*sin2ks-sinphi*one_minus_cos2ks)*one_over_twokappa;
  double y=y0+(sinphi*sin2ks+cosphi*one_minus_cos2ks)*one_over_twokappa;
  
  pos.SetXYZ(x,y,z);
}


// Routine to do a crude match between cdc wires and a helical approximation to
// the trajectory
double DTrackCandidate_factory::DocaToHelix(const DCDCTrackHit *hit,
					    double q,
					    const DVector3 &pos,
					    const DVector3 &mom){
  double pt=mom.Perp();
  double phi=mom.Phi();
  double sinphi=sin(phi);
  double cosphi=cos(phi);
  double tanl=tan(M_PI_2-mom.Theta());
  double x0=pos.X();
  double y0=pos.Y();
  double z0=pos.Z();
  double B=fabs(bfield->GetBz(x0,y0,z0));
  double twokappa=FactorForSenseOfRotation*0.003*B*q/pt;
  double one_over_twokappa=1./twokappa;
  
  double sperp=0;
  DVector3 origin=hit->wire->origin;
  double z0w=origin.z();
  DVector3 dir=(1./hit->wire->udir.z())*hit->wire->udir;
  
  DVector3 wirepos;
  double old_doca2=1e8;
  double doca2=old_doca2;
  double z=z0;
  double x=x0,y=y0;
  while (z>50. && z<170. && x<60. && y<60.){
    old_doca2=doca2;
    sperp-=1.;
    double twoks=twokappa*sperp;
    double sin2ks=sin(twoks);
    double one_minus_cos2ks=1.-cos(twoks);
    x=x0+(cosphi*sin2ks-sinphi*one_minus_cos2ks)*one_over_twokappa;
    y=y0+(sinphi*sin2ks+cosphi*one_minus_cos2ks)*one_over_twokappa;
    z=z0+sperp*tanl;
    
    wirepos=origin+(z-z0w)*dir;
    double dxw=x-wirepos.x();
    double dyw=y-wirepos.y();
    
    doca2=dxw*dxw+dyw*dyw;
    if (doca2>old_doca2){
      break;
    } 
  }

  return old_doca2;
}


// Find the particle charge given the helical fit result and an fdc hit 
// on the track
double DTrackCandidate_factory::GetSenseOfRotation(DHelicalFit &fit,
					  const DFDCPseudo *fdchit,
					  const DVector3 &pos
					  ){
  // Get circle parameters
  double rc=fit.r0;
  double xc=fit.x0;
  double yc=fit.y0;
  // Compute phi rotation from "vertex" to fdc hit
  double dphi=(fdchit->wire->origin.Z()-pos.z())/(rc*fit.tanl);
  double Phi1=atan2(pos.Y()-yc,pos.X()-xc);
  
  // Positive and negative changes in phi
  double phiplus=Phi1+dphi;
  double phiminus=Phi1-dphi;
  DVector2 plus(xc+rc*cos(phiplus),yc+rc*sin(phiplus));
  DVector2 minus(xc+rc*cos(phiminus),yc+rc*sin(phiminus));

  // Compute differences 
  double d2plus=(plus-fdchit->xy).Mod2();
  double d2minus=(minus-fdchit->xy).Mod2();
    
  if (d2minus<d2plus)
    return (-1.0);
  else
    return (+1.0);
}



// Redo the circle fit using all of the cdc axial wires and fdc hits associated
// with the track candidate.  Also compute the average Bz.
jerror_t DTrackCandidate_factory::DoRefit(DHelicalFit &fit,
					  vector<const DFDCSegment *>segments,
					  vector<const DCDCTrackHit *>cdchits,
					  double &Bz){
  unsigned int num_hits=0;
  // Initialize Bz
  Bz=0.;

  // Add the cdc axial wires to the list of hits to use in the fit 
  for (unsigned int k=0;k<cdchits.size();k++){	
    if (cdchits[k]->is_stereo==false){
      double cov=0.213;  //guess
      const DVector3 origin=cdchits[k]->wire->origin;
      double x=origin.x(),y=origin.y(),z=origin.z();
      fit.AddHitXYZ(x,y,z,cov,cov,0.,true);
      Bz+=bfield->GetBz(x,y,z);
      num_hits++;
    }   
  }
  // Add the FDC hits and estimate Bz
  for (unsigned int k=0;k<segments.size();k++){
    for (unsigned int n=0;n<segments[k]->hits.size();n++){
      const DFDCPseudo *fdchit=segments[k]->hits[n];
      fit.AddHit(fdchit);
      //bfield->GetField(x,y,z,Bx,By,Bz);
      //Bz_avg-=Bz;
      Bz+=bfield->GetBz(fdchit->xy.X(),fdchit->xy.Y(),fdchit->wire->origin.z());
    }
    num_hits+=segments[k]->hits.size();
  }	
  Bz=fabs(Bz)/double(num_hits);
	  
  // Fake point at origin
  //fit.AddHitXYZ(0.,0.,TARGET_Z,BEAM_VAR,BEAM_VAR,0.);
  
  // Fit the points to a circle
  return (fit.FitCircleRiemann(segments[0]->rc));
}

// Method to match FDC and CDC candidates that projects the CDC candidate to the
// cdc endplate, where the FDC candidates are reported.
bool DTrackCandidate_factory::MatchMethod1(const DTrackCandidate *fdccan,
					   vector<unsigned int> &cdc_forward_ids,
					   vector<DVector3>&cdc_endplate_projections,
					   vector<unsigned int>&used_cdc_hits){
  // Magnitude of the momentum of the potential cdc match
  double p_cdc=0.;
  
  // Momentum and position vectors for the FDC candidate
  DVector3 mom=fdccan->momentum();
  DVector3 pos=fdccan->position();
  double p_fdc=mom.Mag();
  
  // loop over the cdc candidates looking for the smallest distance
  // between the cdc and fdc projections to the end plate
  double diff_min=1000.; // candidate matching difference
  unsigned int jmin=0;
  double radius=0.;
  for (unsigned int j=0;j<cdc_forward_ids.size();j++){
    double diff=(cdc_endplate_projections[j]-pos).Mag();
    if (diff<diff_min){
      diff_min=diff;
      jmin=j;
      radius=pos.Perp();
      p_cdc=cdctrackcandidates[cdc_forward_ids[jmin]]->momentum().Mag();
    }
  }
  
  if (DEBUG_HISTS){
    match_dist->Fill(radius,diff_min);
    match_dist_vs_p->Fill(p_fdc,diff_min);
  }
  
  if (cdc_fdc_match(p_fdc,p_cdc,diff_min)){
    // Get the segment data
    vector<const DFDCSegment *>segments;
    fdccan->GetT(segments);

    // The following code snippet is intended to prevent matching a cdc
    // track candidate to a low momentum particle curling from the cdc endplate
    if (segments.size()>1){
      double theta=180./M_PI*mom.Theta();
      if (p_fdc<0.3 && theta<5.) return false;
    }
    
    // JANA does not maintain the order that the segments were added
    // as associated objects. Therefore, we need to reorder them here
    // so segment[0] is the most upstream segment.
    sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);
        
    unsigned int cdc_index=cdc_forward_ids[jmin];
    const DTrackCandidate *cdccan=cdctrackcandidates[cdc_index];
    
    // Get the associated cdc hits
    vector<const DCDCTrackHit *>cdchits;
    cdccan->GetT(cdchits);
     
    // Sort CDC hits by layer
    sort(cdchits.begin(), cdchits.end(), CDCHitSortByLayerincreasing);
        
    // Abort if the track would have to pass from one quadrant into another
    // quadrant (this is more likely two roughly back-to-back tracks rather than 
    // a single track).
    const DVector2 fdc_hit_pos=segments[0]->hits[0]->xy;
    const DVector3 cdc_wire_origin=cdchits[0]->wire->origin;  
    if (cdc_wire_origin.x()*fdc_hit_pos.X()<0.
	|| cdc_wire_origin.y()*fdc_hit_pos.Y()<0.){
      if (DEBUG_LEVEL>1) _DBG_ << "Skipping match of potential back-to-back tracks." <<endl;
      return false; 
    }
    
    // average Bz
    double Bz_avg=0.;
     
    // Redo circle fit with additional hits
    DHelicalFit fit;
    if (DoRefit(fit,segments,cdchits,Bz_avg)==NOERROR){
      // Determine the polar angle
      double theta=fdccan->momentum().Theta();
      fit.tanl=tan(M_PI_2-theta);
      
      if (GetPositionAndMomentum(fit,Bz_avg,cdchits[0]->wire->origin,
				 pos,mom)==NOERROR){
	// FDC hit
	const DFDCPseudo *fdchit=segments[0]->hits[0];
	// Find the z-position at the new position in x and y
	DVector2 xy0(pos.X(),pos.Y());
	double tworc=2.*fit.r0;
	double ratio=(fdchit->xy-xy0).Mod()/tworc;
	double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
	
	// Create new track candidate object 
	DTrackCandidate *can = new DTrackCandidate;
	can->used_cdc_indexes=cdccan->used_cdc_indexes;
	
	// Add cdc and fdc hits to the track as associated objects
	for (unsigned int m=0;m<segments.size();m++){
	  for (unsigned int n=0;n<segments[m]->hits.size();n++){
	    can->AddAssociatedObject(segments[m]->hits[n]);
	  }
	}
	for (unsigned int n=0;n<cdchits.size();n++){
	  used_cdc_hits[cdccan->used_cdc_indexes[n]]=1;
	  can->AddAssociatedObject(cdchits[n]); 
	}
	
	pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);

	can->chisq=fit.chisq;
	can->Ndof=fit.ndof;
	can->setCharge(FactorForSenseOfRotation*fit.h);
	can->setMomentum(mom);
	can->setPosition(pos);
    
	_data.push_back(can);	    
    
	// Remove the CDC candidate from the id list because we 
	// found a match
	cdc_forward_ids.erase(cdc_forward_ids.begin()+jmin);
	
	if (DEBUG_LEVEL>0) _DBG_ << ".. matched to CDC candidate #" << cdc_index <<endl;
    
	return true;
      }
    }
  } // got a cdc-fdc match
  
  // no match
  return false;
}
 
// Method to match CDC and FDC candidates that projects an FDC candidate into 
// the CDC region using a helical approximation to the trajectory.  The code
// computes a doca to each wire in the cdc candidate and counts matches based
// on the probability that the cdc wire is on the track.
 bool DTrackCandidate_factory::MatchMethod2(const DTrackCandidate *fdccan,
					    vector<unsigned int> &cdc_forward_ids,
					    vector<unsigned int>&used_cdc_hits
					    ){
   // loop over the forward-pointing cdc candidates
   for (unsigned int j=0;j<cdc_forward_ids.size();j++){
     const DTrackCandidate *cdccan=cdctrackcandidates[cdc_forward_ids[j]];

     // Get the cdc hits associated with this cdc candidate
     vector<const DCDCTrackHit *>cdchits;
     cdccan->GetT(cdchits);	 
     sort(cdchits.begin(),cdchits.end(),CDCHitSortByLayerincreasing);
	  
     // Variables to count the number of matching hits and the match fraction
     unsigned int num_match=0;
     unsigned int num_cdc=0;
	  
     // Get the track parameters from the fdc candidate
     DVector3 pos=fdccan->position();
     DVector3 mom=fdccan->momentum();
     double q=fdccan->charge();

     // loop over the cdc hits and count hits that agree with a projection of 
     // the helix into the cdc 
     for (unsigned int m=0;m<cdchits.size();m++){
       double variance=1.0;
       // Use a helical approximation to the track to match both axial and
       // stereo wires
       double dr2=DocaToHelix(cdchits[m],q,pos,mom);
       double prob=isfinite(dr2) ? TMath::Prob(dr2/variance,1):0.0;

       if (prob>0.01) num_match++;
       if (DEBUG_LEVEL>1) _DBG_ << "CDC s: " << cdchits[m]->wire->straw
				<< " r: " << cdchits[m]->wire->ring
				<< " prob: " << prob << endl;
       
       num_cdc++;
     }

     if (num_match>=3 && double(num_match)/double(num_cdc)>0.33){
       // Put the fdc candidate in the combined list
       DTrackCandidate *can = new DTrackCandidate;
       can->setCharge(q);
       
       // copy the list of cdc indices over from the cdc candidate
       can->used_cdc_indexes=cdccan->used_cdc_indexes;
              
       // Get the segment data
       vector<const DFDCSegment *>segments;
       fdccan->GetT(segments);
       
       // JANA does not maintain the order that the segments were added
       // as associated objects. Therefore, we need to reorder them here
       // so segment[0] is the most upstream segment.
       sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);

       // Abort if the track would have to pass from one quadrant into the opposite 
       // quadrant (this is more likely two roughly back-to-back tracks rather than 
       // a single track).
       const DVector2 fdc_hit_pos=segments[0]->hits[0]->xy;
       const DVector3 cdc_wire_origin=cdchits[0]->wire->origin;  
       if (cdc_wire_origin.x()*fdc_hit_pos.X()<0.
	   && cdc_wire_origin.y()*fdc_hit_pos.Y()<0.){
	 if (DEBUG_LEVEL>1) _DBG_ << "Skipping match of potential back-to-back tracks." <<endl;
	 continue; 
       }

       // Add the fdc hits to track candidate as associated objects 
       unsigned int num_fdc_hits=0;
       for (unsigned int m=0;m<segments.size();m++){
	 for (unsigned int n=0;n<segments[m]->hits.size();n++){
	   can->AddAssociatedObject(segments[m]->hits[n]);
	   num_fdc_hits++;
	 }
       }
       // Add the CDC hits to the track candidate
       for (unsigned int m=0;m<cdchits.size();m++){
	 used_cdc_hits[cdccan->used_cdc_indexes[m]]=1;
	 can->AddAssociatedObject(cdchits[m]);
       }
       
       // Average Bz
       double Bz_avg=0.;
       
       // Redo circle fit with additional hits
       DHelicalFit fit;
       if (DoRefit(fit,segments,cdchits,Bz_avg)==NOERROR){
	 // Determine the polar angle
	 double theta=fdccan->momentum().Theta();	      
	 fit.tanl=tan(M_PI_2-theta);
	 
	 if (GetPositionAndMomentum(fit,Bz_avg,cdchits[0]->wire->origin,
				    pos,mom)==NOERROR){
	   // FDC hit
	   const DFDCPseudo *fdchit=segments[0]->hits[0];
	   // Find the z-position at the new position in x and y
	   DVector2 xy0(pos.X(),pos.Y());
	   double tworc=2.*fit.r0;
	   double ratio=(fdchit->xy-xy0).Mod()/tworc;
	   double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
	   
	   pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);
	   can->setCharge(FactorForSenseOfRotation*fit.h);
	 }
	 else{
	   //Set the charge for the stepper 
	   stepper->SetCharge(q);
	   // Swim to a radius just outside the start counter
	   stepper->SwimToRadius(pos,mom,9.5,NULL);   
	 }

	 can->chisq=fit.chisq;
	 can->Ndof=fit.ndof;
	 can->setMomentum(mom);
	 can->setPosition(pos);
       }
       else{
	 //_DBG_ << endl;
	 can->Ndof=fdccan->Ndof;
	 can->chisq=fdccan->chisq;
	 can->setMomentum(fdccan->momentum());
	 can->setPosition(fdccan->position());	    
       }
       
       _data.push_back(can);
       
       // Remove the CDC candidate from the list
       cdc_forward_ids.erase(cdc_forward_ids.begin()+j); 

       if (DEBUG_LEVEL>0) _DBG_ << ".. matched to CDC candidate #" <<cdc_forward_ids[j] <<endl;
       
       return true;
     }     
   } // loop over forward cdc candidates

   return false;
 }
 
 // Method to match CDC and FDC candidates that projects the CDC track into the
 // FDC region.
 bool DTrackCandidate_factory::MatchMethod3(const DTrackCandidate *cdccan,
					    vector<int> &forward_matches,
					    vector<unsigned int>&used_cdc_hits
){   
   // Get hits already linked to this candidate from associated objects
   vector<const DCDCTrackHit *>cdchits;
   cdccan->GetT(cdchits);
   sort(cdchits.begin(), cdchits.end(), CDCHitSortByLayerincreasing);
   
   // loop over fdc candidates
   for (unsigned int k=0;k<fdctrackcandidates.size();k++){
     if (forward_matches[k]==0){
       const DTrackCandidate *fdccan = fdctrackcandidates[k];
       // fdc and cdc candidate momenta
       double p_fdc=fdccan->momentum().Mag();
       double p_cdc=cdccan->momentum().Mag();
       if (p_fdc/p_cdc>0.5){       
	 // Get the segment data
	 vector<const DFDCSegment *>segments;
	 fdccan->GetT(segments);
	 sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);
	 
	 // Abort if the track would have to pass from one quadrant into the opposite 
	 // quadrant (this is more likely two roughly back-to-back tracks rather than 
	 // a single track).
	 const DVector2 fdc_hit_pos=segments[0]->hits[0]->xy;
	 const DVector3 cdc_wire_origin=cdchits[0]->wire->origin;  
	 if (cdc_wire_origin.x()*fdc_hit_pos.X()<0.
	     && cdc_wire_origin.y()*fdc_hit_pos.Y()<0.){
	   if (DEBUG_LEVEL>1) _DBG_ << "Skipping match of potential back-to-back tracks." <<endl;
	   continue; 
	 }
       
	 // Momentum and position vectors for the CDC candidate
	 DVector3 mom=cdccan->momentum();
	 DVector3 pos=cdccan->position();
	 double q=cdccan->charge();
	 
	 // Try to match unmatched fdc candidates
	 int num_match=0;
	 int num_hits=0;
	 for (unsigned int m=0;m<segments.size();m++){
	   for (unsigned int n=0;n<segments[m]->hits.size();n++){
	     unsigned int ind=segments[m]->hits.size()-1-n;
	     const DFDCPseudo *hit=segments[m]->hits[ind];
	     
	     ProjectHelixToZ(hit->wire->origin.z(),q,mom,pos);

	     // difference
	     DVector2 XY=hit->xy;
	     double dx=XY.X()-pos.x();
	     double dy=XY.Y()-pos.y();
	     double dr2=dx*dx+dy*dy;
	     
	     double variance=1.0;
	     double prob = isfinite(dr2) ? TMath::Prob(dr2/variance,1):0.0;
	     if (prob>0.01) num_match++;
	     
	     num_hits++;
	   }
	   if (num_match>=3 
	       || (num_match>0 && double(num_match)/double(num_hits)>0.33)){
	     DTrackCandidate *can = new DTrackCandidate;
	     
	     can->setCharge(cdccan->charge());
	     can->used_cdc_indexes=cdccan->used_cdc_indexes;
	     
	     // mark the fdc track candidate as matched
	     forward_matches[k]=1; 
	     
	     // Add cdc hits to the candidate
	     for (unsigned int m=0;m<cdchits.size();m++){
	       used_cdc_hits[cdccan->used_cdc_indexes[m]]=1;
	       can->AddAssociatedObject(cdchits[m]);
	     } 
	     
	     // Add fdc hits to the candidate
	     for (unsigned int m=0;m<segments.size();m++){
	       for (unsigned int n=0;n<segments[m]->hits.size();n++){
		 can->AddAssociatedObject(segments[m]->hits[n]);
	       }
	     }
	     
	     // average Bz
	     double Bz_avg=0.;
	     
	     // Redo helical fit with all available hits
	     DHelicalFit fit; 
	     if (DoRefit(fit,segments,cdchits,Bz_avg)==NOERROR){
	       // Determine the polar angle
	       double theta=fdccan->momentum().Theta();
	       fit.tanl=tan(M_PI_2-theta);
	       
	       // Redo the line fit
	       fit.FitLineRiemann();
	       
	       // Set the charge
	       fit.FindSenseOfRotation();   
	       can->setCharge(FactorForSenseOfRotation*fit.h);
	       
	       if (GetPositionAndMomentum(fit,Bz_avg,cdchits[0]->wire->origin,
					  pos,mom)==NOERROR){
	       // FDC hit
		 const DFDCPseudo *fdchit=segments[0]->hits[0];
		 // Find the z-position at the new position in x and y
		 DVector2 xy0(pos.X(),pos.Y());
		 double tworc=2.*fit.r0;
		 double ratio=(fdchit->xy-xy0).Mod()/tworc;
		 double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
		 
		 pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);	    
	       }
	       else{
		 mom=fdccan->momentum();
		 pos=fdccan->position();
	       }
	       can->chisq=fit.chisq;
	       can->Ndof=fit.ndof;
	       can->setMomentum(mom);
	       can->setPosition(pos);
	     }
	     else{
	       //_DBG_ << endl;
	       can->Ndof=cdccan->Ndof;
	       can->chisq=cdccan->chisq;
	       can->setMomentum(cdccan->momentum());
	       can->setPosition(cdccan->position());
	     }
	   
	     _data.push_back(can);
	     
	     if (DEBUG_LEVEL>0) _DBG_ << ".. matched to CDC candidate #" << k <<endl;
	   
	     return true;
	   } // got a match
	 } // loop over segments
       } // check that we have not already matched to this fdc candidate
     } // momentum check
   } // loop over fdc candidates
   
   return false;
 }

// This method tries to match a candidate that is already in the final list 
// of matched candidates to any additional outstanding fdc candidates
void DTrackCandidate_factory::MatchMethod4(DTrackCandidate *srccan, 
					   vector<int> &forward_matches,
					   int &num_fdc_cands_remaining){
  // Momentum of candidate to which we are attempting to match
  double p=srccan->momentum().Mag();

  // Get hits already linked to this candidate from associated objects
  vector<const DFDCPseudo *>fdchits;
  srccan->GetT(fdchits);
  if (fdchits.size()==0) return;
  vector<const DCDCTrackHit *>cdchits;
  srccan->GetT(cdchits);
	  
  sort(cdchits.begin(), cdchits.end(), CDCHitSortByLayerincreasing);
  sort(fdchits.begin(), fdchits.end(), FDCHitSortByLayerincreasing);
  
  unsigned int pack1=(fdchits[fdchits.size()-1]->wire->layer-1)/6;

  for (unsigned int k=0;k<fdctrackcandidates.size();k++){
    if (num_fdc_cands_remaining==0) break;
    if (forward_matches[k]==0){
      const DTrackCandidate *fdccan = fdctrackcandidates[k];
      // Momentum of this candidate 
      double p_fdc=fdccan->momentum().Mag();
      if (p_fdc/p>0.5){
	// Get the segment data
	vector<const DFDCSegment *>segments;
	fdccan->GetT(segments);
	sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);
	
	const DFDCPseudo *firsthit=segments[0]->hits[0];
	unsigned int pack2=segments[0]->package;
	if (pack2>pack1){
	  // Momentum and position vectors for the input candidate
	  DVector3 mom=srccan->momentum();
	  DVector3 pos=srccan->position();
	  double q=srccan->charge();
	  DVector3 norm(0.,0.,1.);
	  
	  // Swim out to the first hit in the candidate to which we wish to match
	  // using the stepper
	  stepper->SetCharge(q);
	  stepper->SwimToPlane(pos,mom,firsthit->wire->origin,norm,NULL);
	  
	  double dx=pos.x()-firsthit->xy.X();
	  double dy=pos.y()-firsthit->xy.Y();
	  double d2=dx*dx+dy*dy;
	  double variance=1.;
	  double prob=TMath::Prob(d2/variance,1);
	  
	  unsigned int num_match=(prob>0.01)?1:0;
	  
	  // Try to match unmatched fdc candidates
	  for (unsigned int i=1;i<segments[0]->hits.size();i++){
	    const DFDCPseudo *hit=segments[0]->hits[i];
	    
	    ProjectHelixToZ(hit->wire->origin.z(),q,mom,pos);
	    
	    DVector2 XY=hit->xy;
	    double dx=XY.X()-pos.x();
	    double dy=XY.Y()-pos.y();
	    double dr2=dx*dx+dy*dy;
	    
	    double variance=1.0;
	    double prob = isfinite(dr2) ? TMath::Prob(dr2/variance,1):0.0;
	  if (prob>0.01) num_match++;
	  } 
	  if (num_match>=3){
	    forward_matches[k]=1;
	    
	    // Add extra fdc hits as associated objects
	    for (unsigned int m=0;m<segments.size();m++){
	      for (unsigned int n=0;n<segments[m]->hits.size();n++){
		srccan->AddAssociatedObject(segments[m]->hits[n]);
	      }
	    }
	
	    // average Bz
	    double Bz_avg=0.;
	    unsigned int num_hits=0;
	    
	    // Redo helical fit with all available hits
	    DHelicalFit fit; 
	    for (unsigned int m=0;m<cdchits.size();m++){
	      if (cdchits[m]->is_stereo==false){
		double cov=0.213; //guess=1.6*1.6/12 (cell size)
		const DVector3 origin=cdchits[m]->wire->origin;
		fit.AddHitXYZ(origin.x(),origin.y(),origin.z(),cov,cov,0.,true);
	      }
	    }
	    for (unsigned int m=0;m<fdchits.size();m++){
	      fit.AddHit(fdchits[m]);
	      
	      //bfield->GetField(x,y,z,Bx,By,Bz);
	      //Bz_avg-=Bz;
	      Bz_avg+=bfield->GetBz(fdchits[m]->xy.X(),fdchits[m]->xy.Y(),
				    fdchits[m]->wire->origin.z());
	      
	      num_hits++;
	    }
	    for (unsigned int m=0;m<segments.size();m++){
	      for (unsigned int n=0;n<segments[m]->hits.size();n++){
		const DFDCPseudo *fdchit=segments[m]->hits[n];
		fit.AddHit(fdchit);
		//bfield->GetField(x,y,z,Bx,By,Bz);
		//Bz_avg-=Bz;
		Bz_avg+=bfield->GetBz(fdchit->xy.X(),fdchit->xy.Y(),
				      fdchit->wire->origin.z());
	      }
	      num_hits+=segments[m]->hits.size();
	    }
	    
	    // Fake point at origin
	    //fit.AddHitXYZ(0.,0.,TARGET_Z,BEAM_VAR,BEAM_VAR,0.,true);
	    // Fit the points to a circle
	    if (fit.FitCircleRiemann(segments[0]->rc)==NOERROR){
	    // Compute new transverse momentum
	      Bz_avg=fabs(Bz_avg)/double(num_hits);
	      
	      // Guess for theta and z from input candidates
	      double theta=fdccan->momentum().Theta();  
	      fit.tanl=tan(M_PI_2-theta);
	      fit.z_vertex=srccan->position().Z();
	      
	      // Redo line fit
	      fit.FitLineRiemann();
	      
	      // Guess charge from fit
	      fit.h=GetSenseOfRotation(fit,firsthit,srccan->position());
	      srccan->setCharge(FactorForSenseOfRotation*fit.h);
	      
	      // Position and momentum just outside of start counter
	      int cdc_id=(cdchits.size()>0)?0:-1;
	      UpdatePositionAndMomentum(srccan,fdchits[0],fit,Bz_avg,cdc_id);
	      
	      num_fdc_cands_remaining--;
	      if (DEBUG_LEVEL>0) _DBG_ << "Found a match using method #4" <<endl;
	    } // circle fit
	  } // got a match?
	} // is the first package in the new fdc candidate downstream of the last?
      } // momentum check
    } // check that we have not already matched this fdc candidate
  } // loop over fdc track candidates
}

// Matching method to try to deal with CDC candidates that do not point 
// directly at the the cdc endplate but could possibly be matched to FDC 
// candidates.
bool DTrackCandidate_factory::MatchMethod5(DTrackCandidate *can,
					   vector<const DCDCTrackHit *>&cdchits,
					   vector<int> &forward_matches
					   ){
  // Loop over the fdc track candidates
  for (unsigned int k=0;k<fdctrackcandidates.size();k++){
    if (forward_matches[k]==0){
      const DTrackCandidate *fdccan = fdctrackcandidates[k];
      
      unsigned int num_match=0;
      unsigned int num_cdc=0;
      
      DVector3 pos=fdccan->position();
      DVector3 mom=fdccan->momentum();
      double q=fdccan->charge();

      for (unsigned int m=0;m<cdchits.size();m++){
	double variance=1.0;
	// Use a helical approximation to the track to match both axial and
	// stereo wires
	double dr2=DocaToHelix(cdchits[m],q,pos,mom);
	double prob=isfinite(dr2) ? TMath::Prob(dr2/variance,1):0.0;
	
	if (prob>0.01) num_match++;
	if (DEBUG_LEVEL>1) _DBG_ << "CDC s: " << cdchits[m]->wire->straw
				 << " r: " << cdchits[m]->wire->ring
				 << " prob: " << prob << endl;

	num_cdc++;
      }

      if (num_match>=3 && double(num_match)/double(num_cdc)>0.33){	
	// Get the segment data
	vector<const DFDCSegment *>segments;
	fdccan->GetT(segments);
	
	// JANA does not maintain the order that the segments were added
	// as associated objects. Therefore, we need to reorder them here
	// so segment[0] is the most upstream segment.
	sort(segments.begin(), segments.end(), SegmentSortByLayerincreasing);

	// Abort if the track would have to pass from one quadrant into the opposite 
	// quadrant (this is more likely two roughly back-to-back tracks rather than 
	// a single track).
	const DVector2 fdc_hit_pos=segments[0]->hits[0]->xy;
	const DVector3 cdc_wire_origin=cdchits[0]->wire->origin;  
	if (cdc_wire_origin.x()*fdc_hit_pos.X()<0.
	    && cdc_wire_origin.y()*fdc_hit_pos.Y()<0.){
	  if (DEBUG_LEVEL>1) _DBG_ << "Skipping match of potential back-to-back tracks." <<endl;
	  continue; 
	}

	// Mark this fdc candidate as having a match to a cdc candidate
	forward_matches[k]=1;
	
	if (DEBUG_LEVEL>0) _DBG_ << "... matched to FDC candidate #" << k <<endl;
	
	// Add fdc hits to the candidate
	for (unsigned int m=0;m<segments.size();m++){
	  for (unsigned int n=0;n<segments[m]->hits.size();n++){
	    can->AddAssociatedObject(segments[m]->hits[n]);
	  }
	}
	
	// Average Bz
	double Bz_avg=0.;
	
	// Instantiate the helical fitter to do the refit
	DHelicalFit fit;
	if (DoRefit(fit,segments,cdchits,Bz_avg)==NOERROR){
	  // Determine the polar angle
	  double theta=fdccan->momentum().Theta();
	  fit.tanl=tan(M_PI_2-theta);
	  
	  if (GetPositionAndMomentum(fit,Bz_avg,cdchits[0]->wire->origin,
				     pos,mom)==NOERROR){
	    // FDC hit
	    const DFDCPseudo *fdchit=segments[0]->hits[0];
	    // Find the z-position at the new position in x and y
	    DVector2 xy0(pos.X(),pos.Y());
	    double tworc=2.*fit.r0;
	    double ratio=(fdchit->xy-xy0).Mod()/tworc;
	    double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
	    
	    pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);
	    can->setCharge(FactorForSenseOfRotation*fit.h);
	  }    
	  can->chisq=fit.chisq;
	  can->Ndof=fit.ndof;
	  can->setMomentum(mom);
	  can->setPosition(pos);
	}
	return true;
      } // check for match
    } // check that the candidate has not already been matched
  } // loop over fdc candidates
  
  return false;
}

// Method to match FDC candidates with stray CDC hits unassociated with track
// candidates
void DTrackCandidate_factory::MatchMethod6(unsigned int fdc_id,
					   DTrackCandidate *can, 
					   vector<const DFDCSegment *>&segments,
					   vector<unsigned int>&used_cdc_hits,  
					   unsigned int &num_unmatched_cdcs,
					   vector<int>&forward_matches,
					   int &num_fdc_cands_remaining
					   ){
  // Input candidate parameters
  DVector3 pos=can->position();
  DVector3 mom=can->momentum();
  double q=can->charge();

  // Variables to keep track of cdc hits 
  unsigned int num_match_cdc=0;
  unsigned int num_axial=0;
  int id_for_smallest_dr=-1;
  int old_ring=-1;
  vector<int>matched_ids;
  double dr2_min=1e6;
  
  for (unsigned int k=0;k<used_cdc_hits.size();k++){
    // We only want to use one hit from a given ring to avoid confusing the 
    // circle refit with curlers...
    if (mycdchits[k]->wire->ring!=old_ring){
      if (id_for_smallest_dr>=0){
	if (!used_cdc_hits[id_for_smallest_dr]){
	  num_match_cdc++;
	  num_unmatched_cdcs--;
	  used_cdc_hits[id_for_smallest_dr]=1;
	  
	  can->used_cdc_indexes.push_back(id_for_smallest_dr);
	  can->AddAssociatedObject(mycdchits[id_for_smallest_dr]);
	  
	  if (mycdchits[id_for_smallest_dr]->is_stereo==false){
	    num_axial++;
	    matched_ids.push_back(id_for_smallest_dr);
	  }
	}
      }
      // Reset matching variables
      dr2_min=1e6;
      id_for_smallest_dr=-1;
    }
    
    if (!used_cdc_hits[k]){
      double variance=1.0;
      // Use a helical approximation to the track to match both axial and
      // stereo wires
      double dr2=DocaToHelix(mycdchits[k],q,pos,mom);
      double prob=isfinite(dr2) ? TMath::Prob(dr2/variance,1):0.0;

      if (prob>0.5){
	// Look for closest match
	if (dr2<dr2_min){
	  dr2_min=dr2;
	  id_for_smallest_dr=k;
	}
      }
    }	 
    old_ring=mycdchits[k]->wire->ring;
  } 
  // Grab the last potential match
  if (id_for_smallest_dr>=0){
    if (!used_cdc_hits[id_for_smallest_dr]){
      num_match_cdc++;
      num_unmatched_cdcs--;
      used_cdc_hits[id_for_smallest_dr]=1;
      
      can->used_cdc_indexes.push_back(id_for_smallest_dr);
      can->AddAssociatedObject(mycdchits[id_for_smallest_dr]);
      
      if (mycdchits[id_for_smallest_dr]->is_stereo==false){
	num_axial++;
	matched_ids.push_back(id_for_smallest_dr);
      }
    }
  }

  // We found some matched hits.  Update the start position of the candidate.
  if (num_match_cdc>0){
    const DFDCPseudo *firsthit=segments[0]->hits[0];
    
    int axial_id=-1;
    if (num_axial>0) axial_id=matched_ids[0];

    // Compute the average Bz
    double Bz_avg=0.,denom=0.;
    for (unsigned int m=0;m<segments.size();m++){
      for (unsigned int n=0;n<segments[m]->hits.size();n++){	
	const DFDCPseudo *fdchit=segments[m]->hits[n];
	double x=fdchit->xy.X();
	double y=fdchit->xy.Y();
	double z=fdchit->wire->origin.z();
	Bz_avg+=bfield->GetBz(x,y,z);
	denom+=1.;
      }
    }
    Bz_avg=fabs(Bz_avg)/denom;

    // Store the current track parameters in the DHelicalFit class
    DHelicalFit fit;
    fit.r0=mom.Perp()/(0.003*Bz_avg);
    fit.phi=mom.Phi();
    fit.h=q*FactorForSenseOfRotation;
    fit.x0=pos.x()-fit.h*fit.r0*sin(fit.phi);
    fit.y0=pos.y()+fit.h*fit.r0*cos(fit.phi);
    fit.tanl=tan(M_PI_2-mom.Theta());
    fit.z_vertex=0; // this will be changed later

    // Update the position and momentum of the candidate based on the most 
    // recent fit results
    UpdatePositionAndMomentum(can,firsthit,fit,Bz_avg,axial_id);
  
    if (DEBUG_LEVEL>0) _DBG_ << "... matched stray CDC hits ..." << endl;
  } // match at least one cdc hit
}

// Method to match fdc track stubs to other track candidates using fdc stubs
// that have already been matched to cdc hits
bool DTrackCandidate_factory::MatchMethod7(unsigned int current_id,
					   unsigned int pack1,
					   DHelicalFit &fit,
					   DTrackCandidate *can, 
					   vector<int>&forward_matches,
					   int &num_fdc_cands_remaining
					   ){
  bool got_match=false;

  // loop over all fdc candidates, skipping the current one, and skipping those
  // that have already been matched to other track stubs
  for (unsigned int k=0;k<fdctrackcandidates.size();k++){
    if (num_fdc_cands_remaining==0) break;

    if (k!=current_id && forward_matches[k]==0){
      const DTrackCandidate *fdccan = fdctrackcandidates[k];
	      
      // Get the segment data
      vector<const DFDCSegment *>segments2;
      fdccan->GetT(segments2);
      sort(segments2.begin(),segments2.end(),SegmentSortByLayerincreasing);
	     
      unsigned int pack2=segments2[0]->package;
      if (pack2>pack1){
	DVector3 mom=can->momentum();
	DVector3 pos=can->position();
	double q=can->charge();
	DVector3 norm(0.,0.,1.);

	// Swim out to the first hit in the candidate to which we wish to match
	// using the stepper
	stepper->SetCharge(q);
	stepper->SwimToPlane(pos,mom,segments2[0]->hits[0]->wire->origin,norm,NULL);

	const DFDCPseudo *hit=segments2[0]->hits[0];
	double dx=pos.x()-hit->xy.X();
	double dy=pos.y()-hit->xy.Y();
	double d2=dx*dx+dy*dy;
	double variance=1.;
	double prob=TMath::Prob(d2/variance,1);

	unsigned int num_match=(prob>0.01)?1:0;
	
	// Use a helical approximation for the remaining hits in the segment
	for (unsigned int m=1;m<segments2[0]->hits.size();m++){
	  hit=segments2[0]->hits[m];
	  ProjectHelixToZ(hit->wire->origin.z(),q,mom,pos);  
	  dx=pos.x()-hit->xy.X();
	  dy=pos.y()-hit->xy.Y();
	  d2=dx*dx+dy*dy;
	  prob=TMath::Prob(d2/variance,1);

	  if (prob>0.01) num_match++;
	  if (num_match>3){
	    got_match=true;
	    break;
	  }
	}
	if (got_match){
	  forward_matches[k]=1;
	  num_fdc_cands_remaining--;

	  // Now the last package in the candidate we just matched to should
	  // be compared to the first package in any subsequent track..
	  pack1=segments2[segments2.size()-1]->package;
	  
	  // Add the FDC hits to the final track candidate and to the
	  // fit object
	  for (unsigned int m=0;m<segments2.size();m++){
	    for (unsigned int n=0;n<segments2[m]->hits.size();n++){
	      const DFDCPseudo *hit=segments2[m]->hits[n];
	      can->AddAssociatedObject(hit);
	      fit.AddHit(hit);
	    }
	  }
	  // Redo the fit
	  fit.FitCircleRiemann(fit.r0);
	
	  if (DEBUG_LEVEL>0) _DBG_ << "... matched to FDC candidate #" 
				   << k <<endl;      
	}
      }
    }
  } // loop over candidates

  return got_match;
}

// Update the momentum and position entries for the candidate based on the
// results of the most recent helical fit
void DTrackCandidate_factory::UpdatePositionAndMomentum(DTrackCandidate *can,
							const DFDCPseudo *fdchit,
							DHelicalFit &fit,
							double Bz_avg,
							int axial_id){
  DVector3 pos=can->position(),mom=can->momentum();
  double q=can->charge();

  if (GetPositionAndMomentum(fit,Bz_avg, pos,mom)==NOERROR){  
    // Find the z-position at the new position in x and y
    DVector2 xy0(pos.X(),pos.Y());
    double tworc=2.*fit.r0;
    double ratio=(fdchit->xy-xy0).Mod()/tworc;
    double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
    pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);
    
    // update the track parameters
    can->setMomentum(mom);
    can->setPosition(pos);
  }
  else if (axial_id>=0){
    // if the previous attempt to get the position at r^2=90 did not work,
    // then the circle after the refit does not intercept this radius.  
    // Use these previous estimates before the refit for the track parameters
    // and the inner-most axial wire to estimate the position.
    fit.r0=mom.Perp()/(0.003*Bz_avg);
    fit.phi=mom.Phi();
    fit.h=q*FactorForSenseOfRotation;
    fit.x0=pos.x()-fit.h*fit.r0*sin(fit.phi);
    fit.y0=pos.y()+fit.h*fit.r0*cos(fit.phi);
    fit.tanl=tan(M_PI_2-mom.Theta());
    fit.z_vertex=0; // this will be changed later
     
    if (GetPositionAndMomentum(fit,Bz_avg,mycdchits[axial_id]->wire->origin,pos,
			       mom)==NOERROR){  
      // Find the z-position at the new position in x and y
      DVector2 xy0(pos.X(),pos.Y());
      double tworc=2.*fit.r0;
      double ratio=(fdchit->xy-xy0).Mod()/tworc;
      double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
      pos.SetZ(fdchit->wire->origin.z()-sperp*fit.tanl);
      
      // update the track parameters
      can->setMomentum(mom);
      can->setPosition(pos);
    }
  }
}