////////////////////////////////////////////////////////////////////////
// Package: CandFitTrackCam - A Simplified implementation of the Kalman filter
//
// AlgFitTrackCam.cxx
//
// marshall@hep.phy.cam.ac.uk
////////////////////////////////////////////////////////////////////////
// 10/19/2006
// Latest version of the code. Includes ND Spectrometer Swim.
//
// Note: Sets all fitted track properties in CandFitTrackHandle,
// CandTrackHandle, CandRecoHandle and CandHandle
//

#include <cassert>
#include <cmath>

#include "MessageService/MsgService.h"
#include "MinosObjectMap/MomNavigator.h"

#include "Algorithm/AlgConfig.h"
#include "Algorithm/AlgFactory.h"
#include "Algorithm/AlgHandle.h"

#include "Candidate/CandContext.h"

#include "CandFitTrackCam/AlgFitTrackCam.h"
#include "CandFitTrackCam/AlgFitTrackCam.h"
#include "CandFitTrackCam/CandFitTrackCamHandle.h"

#include "Calibrator/Calibrator.h"

#include "Validity/VldContext.h"

#include "CandData/CandRecord.h"
#include "CandData/CandHeader.h"

#include "RecoBase/CandStrip.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "RecoBase/AlgTrack.h"
#include "RecoBase/CandSliceHandle.h"
#include "RecoBase/CandStripListHandle.h"
#include "CandDigit/CandDigitListHandle.h"

#include "BField/BField.h"

#include "Swimmer/SwimGeo.h"
#include "Swimmer/SwimSwimmer.h"
#include "Swimmer/SwimStepper.h"
#include "Swimmer/SwimPrintStepAction.h"
#include "Swimmer/SwimParticle.h"
#include "Swimmer/SwimZCondition.h"
#include "Swimmer/SwimPlaneInterface.h"

#include "GeoSwimmer/GeoSwimmer.h"
#include "GeoSwimmer/GeoSwimParticle.h"
#include "GeoSwimmer/GeoSwimZCondition.h"

#include "UgliGeometry/UgliGeomHandle.h"

#include "DataUtil/GetMomFromRange.h"

#include <sys/time.h>

ClassImp(AlgFitTrackCam)

CVSID("$Id: AlgFitTrackCam.cxx,v 1.66 2008/10/24 10:56:39 blake Exp $");

////////////////////////////////////////////////////////////////////////
AlgFitTrackCam::AlgFitTrackCam()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
AlgFitTrackCam::~AlgFitTrackCam()
{
}
////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::RunAlg(AlgConfig & ac, 
                            CandHandle &ch, CandContext &cx)
{
  // get alg parameters
  if(!ac.Get("UseGeoSwimmer",UseGeoSwimmer)) cout << "Couldn't Get UseGeoSwimmer\n";

 // Standard set-up

  assert(ch.InheritsFrom("CandFitTrackCamHandle"));
  CandFitTrackCamHandle &cth = dynamic_cast<CandFitTrackCamHandle &>(ch);
  
  nbfield=0;
  bave=0;
  TIter FitTrackStripItr = cth.GetDaughterIterator();
  while(CandStripHandle* FitTrackStrip = dynamic_cast<CandStripHandle*>(FitTrackStripItr())) cth.RemoveDaughter(FitTrackStrip);
  
  assert(cx.GetDataIn());
  
  // Get the track from the track finder
  assert(cx.GetDataIn()->InheritsFrom("CandTrackHandle"));
  track = dynamic_cast<const CandTrackHandle*>(cx.GetDataIn());
  if( !track || track->GetNDaughters()<1 )  { return; }
  cth.SetFinderTrack((CandTrackHandle*)(track));
  
  const CandSliceHandle* slice = dynamic_cast<const CandSliceHandle*>(track->GetCandSlice());
  assert(slice);
  cth.SetCandSlice(slice);
  
  
  //////////////////
  // Track fitter //
  ////////////////////////////////////////////////////////////////////////
  // Switch for screen output
  debug=false;
  
  // Initialisations
  /////////////////////////////////////////
  if( track->GetEndZ() > track->GetVtxZ() )  {ZIncreasesWithTime=true;}
  else {ZIncreasesWithTime=false;}
  
  SaveData=false;
  SwimThroughShower=false;
  PassTrack=true;
  
  MaxPlane=-20;
  MinPlane=500;

  DeltaZ=-99;
  DeltaPlane=-99;
  ShowerEntryPlane=-99;
  
  NIter=0; TotalNSwimFail=0; NumFinderStrips=0;
  
  for (unsigned int i=0; i<5; ++i) {
    x_k[i]=0; x_k_minus[i]=0; H_k[i]=0; K_k[i]=0;
    VtxCov[i]=-999; EndCov[i]=-999;
    for (unsigned int j=0; j<5; ++j) {
      C_k[i][j]=0; C_k_minus[i][j]=0; C_k_intermediate[i][j]=0;
      F_k[i][j]=0; F_k_minus[i][j]=0;
      Q_k[i][j]=0; Q_k_minus[i][j]=0;
      Identity[i][j]=0;
    }
  }
  
  Identity[0][0]=1; Identity[1][1]=1; Identity[2][2]=1; Identity[3][3]=1; Identity[4][4]=1; 
  /////////////////////////////////////////
  
  
  // Set initial parameters
  /////////////////////////////////////////
  x_k_minus[0]=track->GetVtxU();
  x_k_minus[1]=track->GetVtxV();
  if(track->GetVtxDirCosZ()!=0.) {
    x_k_minus[2]=track->GetVtxDirCosU()/track->GetVtxDirCosZ();
    x_k_minus[3]=track->GetVtxDirCosV()/track->GetVtxDirCosZ();
  }
  x_k_minus[4]=0.;
  x_k4_biased=0;
  /////////////////////////////////////////
  
  
  // Get header information
  /////////////////////////////////////////
  CandRecord* candrec = (CandRecord*)(cx.GetCandRecord());
  assert(candrec);
  
  vldc = (VldContext*)(candrec->GetVldContext());
  assert(vldc);
  
  CandHeader* candhead = (CandHeader*)(candrec->GetCandHeader());
  assert(candhead);
  
  MSG("AlgFitTrackCam",Msg::kDebug) << "RunAlg, New track, Run: " << candhead->GetRun() 
				    << " Snarl: " << candhead->GetSnarl() << endl;
  /////////////////////////////////////////
  
  
  // Run the high level methods
  /////////////////////////////////////////
  InitialFramework(slice,cx);
  GetFitData(MinPlane,MaxPlane);
  RunTheFitter(cth);
  /////////////////////////////////////////
  
  
  // Clear up
  /////////////////////////////////////////
  if(vldc->GetDetector()==Detector::kNear) {CleanNDLists(cth,cx);}
  
  for (unsigned int i=0; i<490; ++i) {
    InitTrkStripData[i].clear();
    SlcStripData[i].clear();
    TrkStripData[i].clear();
    FilteredData[i].clear();
  }
  /////////////////////////////////////////
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::InitialFramework(const CandSliceHandle* slice,CandContext &cx)
{
  // Store CandStripHandles and make the strips accessible by plane number
  Detector::Detector_t detector = vldc->GetDetector();

  TIter SlcStripItr = slice->GetDaughterIterator();
  TIter TrkStripItr = track->GetDaughterIterator();
  
  // Store all strips in slice
  int SlicePlane; 

  while(CandStripHandle* SlcStrip = dynamic_cast<CandStripHandle*>(SlcStripItr()))
    {
      SlicePlane=SlcStrip->GetPlane();
      if(detector==Detector::kNear && SlicePlane>=121) {continue;}

      StripStruct temp;
      temp.csh=SlcStrip;
     
      SlcStripData[SlicePlane].push_back(temp);
    }
  SlcStripItr.Reset();


  // Store all track strips found, except those in the Near Spectrometer
  int TrackPlane; 
  MeanTrackTime=0;

  while(CandStripHandle* TrkStrip = dynamic_cast<CandStripHandle*>(TrkStripItr()))
    {    
      TrackPlane=TrkStrip->GetPlane();
      if(detector==Detector::kNear && TrackPlane>=121) {continue;}

      StripStruct temp;
      temp.csh=TrkStrip;

      InitTrkStripData[TrackPlane].push_back(temp);
      NumFinderStrips++;
      
      // Identify ends of initial track
      if (TrackPlane>MaxPlane) {MaxPlane=TrackPlane;}
      if (TrackPlane<MinPlane) {MinPlane=TrackPlane;}

      if(detector==Detector::kNear) {MeanTrackTime+=NDStripBegTime(TrkStrip);}
    }
  TrkStripItr.Reset();

  // For DeMuxing ND Spectrometer
  if(detector==Detector::kNear) {
    if(NumFinderStrips!=0) {MeanTrackTime/=double(NumFinderStrips);}
    GenerateNDSpectStrips(slice,cx);
  }


} 
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::ShowerStrips()
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "ShowerStrips, Look for large vertex shower" << endl;
     
  // Initialisations
  int Increment; int NumberOfStrips;
  int Plane; int NewPlane;

  int VtxShwWindow=8; 
  int StripsForShw=4;
  double PEThreshold=2.;

  if(ZIncreasesWithTime==true) {Plane=MinPlane; Increment=1;}
  else {Plane=MaxPlane; Increment=-1;}
  NewPlane=Plane;


  // Identify any vertex showers
  /////////////////////////////////
  while(abs(Plane-NewPlane)<=VtxShwWindow && NewPlane>=MinPlane && NewPlane<=MaxPlane) {

    if(SlcStripData[NewPlane].size()>0) {
      NumberOfStrips=0;


      // Set the number of hits on a plane required for the plane to be identified as 'in the 
      // shower'. We account for the gradient of the track, with the factor of 0.25 representing
      // the approximate ratio of strip thickness to strip width.
      if(FilteredData[NewPlane].size()>0) {
	if(SlcStripData[NewPlane][0].csh->GetPlaneView()==2) {
	  StripsForShw=min(7,int( 4+(0.25*fabs(FilteredData[NewPlane][0].x_k2)) ));
	}
	else {StripsForShw=min(7,int( 4+(0.25*fabs(FilteredData[NewPlane][0].x_k3)) ));}
      }
      else {StripsForShw=4;}


      // Count number of strips on plane with greater than 2PEs
      for(unsigned int j=0; j<SlcStripData[NewPlane].size(); ++j) {
	if(SlcStripData[NewPlane][j].csh->GetCharge()>PEThreshold) {NumberOfStrips++;}
      }


      // If a vertex shower is found, note that we should use the Swimmer
      // to find the most likely track strips inside the shower
      if(NumberOfStrips>=StripsForShw) {ShowerEntryPlane=NewPlane; SwimThroughShower=true; break;}
      
      NewPlane+=Increment;
    }
    else {NewPlane+=Increment;}
  }
  /////////////////////////////////


 
  // Find the plane at which the 'clean' section of track enters the shower
  /////////////////////////////////
  if(SwimThroughShower==true) {
    NewPlane=ShowerEntryPlane+Increment;
    int PlanesSinceLastHit=0;
    int PlaneWindow=4;

    while(PlanesSinceLastHit<PlaneWindow && NewPlane>=MinPlane && NewPlane<=MaxPlane) {
      if(SlcStripData[NewPlane].size()>0) {
	NumberOfStrips=0;

	// Account for gradient of track, as before
	if(FilteredData[NewPlane].size()>0) { 
	  if(SlcStripData[NewPlane][0].csh->GetPlaneView()==2) {
	    StripsForShw=min(7,int(4+(0.25*fabs(FilteredData[NewPlane][0].x_k2)) ));
	  }
	  else {StripsForShw=min(7,int(4+(0.25*fabs(FilteredData[NewPlane][0].x_k3)) ));}
	}
	else {StripsForShw=4;}


	// Count number of strips on plane with greater than 2PEs
	for(unsigned int j=0; j<SlcStripData[NewPlane].size(); ++j) {
	  if(SlcStripData[NewPlane][j].csh->GetCharge()>PEThreshold) {NumberOfStrips++;}
	}
	if(NumberOfStrips>=StripsForShw) {
	  ShowerEntryPlane=NewPlane; NewPlane+=Increment; PlanesSinceLastHit=0;
	}
	else {PlanesSinceLastHit++; NewPlane+=Increment;}
	
      }
      else {PlanesSinceLastHit++; NewPlane+=Increment;}
    }
  }
  /////////////////////////////////
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::RunTheFitter(CandFitTrackCamHandle &cth)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "RunTheFitter, Call methods in the appropriate order" << endl;
  GetInitialCovarianceMatrix(true);
  
  
  // Control the iterations backwards and forwards
  /////////////////////////////////
  Detector::Detector_t detector = vldc->GetDetector();

  // Control iterations over a track for which ZIncreasesWithTime
  if(ZIncreasesWithTime==true)
    {
      // First iteration
      NIter++;
  
      // Vtx to End

      SaveData=true; StoreFilteredData(MinPlane); 
      LastIteration=false;
      GoForwards(); ResetCovarianceMatrix();
      

      // End back to vtx, swimming through any large vtx shower
      ShowerStrips();  // Try to identify vtx showers, now we have an idea of gradient
      if(SwimThroughShower==true) {RemoveTrkHitsInShw();}
      
      for (unsigned int i=0; i<490; ++i) {FilteredData[i].clear();}
      StoreFilteredData(MaxPlane); GoBackwards();
      
      if(SwimThroughShower==true) {ShowerSwim();}
      ResetCovarianceMatrix();

      bool StripsFound = FindTheStrips(cth,false);
      
      // Second iteration
      if(StripsFound==true) { // Guard against finding no strips
	for(int nint=0;nint<=1;nint++){  
	  NIter++;
	  if(nint==1)LastIteration = true;	  
	  // Vtx to End again, identifying any strips in ND spectrometer
	  for (unsigned int i=0; i<490; ++i) {TrkStripData[i].clear();} 
	  GetFitData(MinPlane,MaxPlane);	
	  SaveData=false; GoForwards();
	  
	  if(detector==Detector::kNear && nint==0) {SpectrometerSwim(cth);}
	  ResetCovarianceMatrix();
	  
	  // End back to vtx again
	  if(nint==0) {for (unsigned int i=0; i<490; ++i) {FilteredData[i].clear();}}	  
	  SaveData=true;  StoreFilteredData(MaxPlane);	  
	  GoBackwards();   
	  if(nint==0) {
	    x_k4_biased= x_k[4];
	    eqp_biased = pow(VtxCov[4],0.5);
	  }
	  ResetCovarianceMatrix();
	}
      }
      else {PassTrack=false;}
    }
  
  
  // Control iterations over a track for which ZDecreasesWithTime
  else
    {
      // First iteration
      NIter++;

      // Vtx to End
      SaveData=true; StoreFilteredData(MaxPlane); 
      LastIteration=false;

      GoBackwards(); ResetCovarianceMatrix();
 
      // End back to vtx, swimming through any large vtx shower and
      // identifying any strips in ND spectrometer
      ShowerStrips();  // Try to identify vtx showers, now we have an idea of gradient
      if(SwimThroughShower==true) {RemoveTrkHitsInShw();}

      for (unsigned int i=0; i<490; ++i) {FilteredData[i].clear();}
      StoreFilteredData(MinPlane); GoForwards();

      if(SwimThroughShower==true) {ShowerSwim();}

      if(detector==Detector::kNear) {SpectrometerSwim(cth);}
      ResetCovarianceMatrix();

      bool StripsFound = FindTheStrips(cth,false);

      // Second iteration
      if(StripsFound==true) { // Guard against finding no strips
	for(int nint=0;nint<=1;nint++){  
	  if(nint==1)LastIteration = true;	  	  
	  NIter++;
	  // Vtx to End again
	  for (unsigned int i=0; i<490; ++i) {TrkStripData[i].clear();} 
	  GetFitData(MinPlane,MaxPlane);	  
	  SaveData=false; GoBackwards(); ResetCovarianceMatrix();
	  
	  // End to Vtx again
	  if(nint==0)  {for (unsigned int i=0; i<490; ++i) {FilteredData[i].clear();}}
	  SaveData=true;  StoreFilteredData(MinPlane);
	  GoForwards(); ResetCovarianceMatrix();
	  if(nint==0) {x_k4_biased= x_k[4];}
	}
      }
      else {PassTrack=false;}

    }
  /////////////////////////////////
  
  

  // Organise the output
  /////////////////////////////////

  // If the fit was successful
  if(x_k[4]!=0. && PassTrack==true) {
        
    //JAM modify tweak following range bias removal
    // Tweak q/p to remove offset
    //    x_k[4]*=1.01+(0.1*fabs(x_k[4]));

    x_k4_biased *=1.01+(0.1*fabs(x_k[4]));
    x_k[4] *=1.013;

    // Find final strips and add them to the fitted track
    FillGapsInTrack();
    bool FinalStripsFound = FindTheStrips(cth,true);
    
    // If final strips found, set the fitted track properties
    if(FinalStripsFound==true) {

      // Check there is >1 strip in each view. If not, then fail track.
      int NumInUView=0; int NumInVView=0;
      
      TIter FitTrackStripItr = cth.GetDaughterIterator();
      while(CandStripHandle* FitTrackStrip = dynamic_cast<CandStripHandle*>(FitTrackStripItr()))
	{
	  if(FitTrackStrip->GetPlaneView()==2) {NumInUView++;}
	  else if(FitTrackStrip->GetPlaneView()==3) {NumInVView++;}
	  
	  if(NumInUView>1 && NumInVView>1) {break;}
	}
      
      if(NumInUView>1 && NumInVView>1) {
	cth.SetPass(1);
	SetTrackProperties(cth);
      }
      else {
	PassTrack=false;
      }
      
    }
    // Otherwise fail the track at this final stage
    else {
      PassTrack=false;
    }
  }
  
  
  // If the fit has failed (e.g. q/p is zero and/or u, v are nonsense)
  if(x_k[4]==0. || PassTrack==false) {
 
    // Remove any existing strips in the failed fitted track
    vector<CandStripHandle*> Daughters;

    TIter FitTrackStripItr = cth.GetDaughterIterator();
    while(CandStripHandle* FitTrackStrip = dynamic_cast<CandStripHandle*>(FitTrackStripItr()))
      {Daughters.push_back(FitTrackStrip);}

    for(unsigned int i=0; i<Daughters.size(); ++i) {cth.RemoveDaughter(Daughters[i]);}
    Daughters.clear();


    // Put strips from track finder in failed fitted track
    TIter TrkStripItr = track->GetDaughterIterator();
    while(CandStripHandle* TrkStrip = dynamic_cast<CandStripHandle*>(TrkStripItr()))
      {cth.AddDaughterLink(*TrkStrip);}
    
    // Set position/direction properties using the finder track
    cth.SetPass(0); 
    cth.SetMomentumCurve(0.); cth.SetEMCharge(0); cth.SetVtxQPError(-999.);
    SetPropertiesFromFinderTrack(cth);
  }
  /////////////////////////////////
}  
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::RemoveTrkHitsInShw()
{
  // If the 'clean' section of track is large enough, remove the track finding
  // data for planes before the ShowerEntryPlane
  MSG("AlgFitTrackCam",Msg::kDebug) << "RemoveTrkHitsInShw, Discard track finding data in shower" << endl;

  int NumTrackStripsLeft=0;
  
  if(ZIncreasesWithTime==true) {
    for(int i=ShowerEntryPlane; i<=MaxPlane; ++i) {
      if(TrkStripData[i].size()>0) {NumTrackStripsLeft++;}
    }
  }
  else if(ZIncreasesWithTime==false) {
    for(int i=MinPlane; i<=ShowerEntryPlane; ++i) {
      if(TrkStripData[i].size()>0) {NumTrackStripsLeft++;}
    }
  }
  
  // Carry out removal if there will be 6 or more strips left afterwards
  if(NumTrackStripsLeft>5) { 
    if(ZIncreasesWithTime==true) {
      for(int i=MinPlane; i<=ShowerEntryPlane; ++i) {TrkStripData[i].clear();}
    }   
    else if(ZIncreasesWithTime==false) {
      for(int i=ShowerEntryPlane; i<=MaxPlane; ++i) {TrkStripData[i].clear();    }
    }
  } 
  // Otherwise note that we should not run the ShowerSwim method
  else {
    MSG("AlgFitTrackCam",Msg::kDebug) << "RemoveTrkHitsInShw, not enough hits after removal. Must use all finder data." << endl;
    SwimThroughShower=false;
  }
  
  
  // Find the new max and min planes
  MaxPlane=-20; MinPlane=500;
  for (int i=0; i<490; ++i) {   
    if(TrkStripData[i].size()>0) {
      if(i>MaxPlane) {MaxPlane=i;}
      if(i<MinPlane) {MinPlane=i;}
    }
  }
  
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::ShowerSwim()
{
  // Method is called if we have a large shower near the track vertex
  //
  // The Swimmer is used to find the most likely track strip in the shower
  // and this strip is added to the fit
  MSG("AlgFitTrackCam",Msg::kDebug) << "ShowerSwim, improved track finding in shower" << endl;

  // Initialisations
  int Plane; int NewPlane;
  double StateVector[5]; double NState[5];
  bool GoForward; bool SwimBack;
  int PlanesSinceLastHit=0;
  int PlaneView;
  int Increment;
  
  double StripDistance; double MinDistanceToStrip;
  double StripWidth=4.108e-2;


  if(ZIncreasesWithTime==true) {GoForward=false; Plane=MinPlane; Increment=-1;}
  else {GoForward=true; Plane=MaxPlane; Increment=1;}

  NewPlane=Plane+Increment;


  // Continue until we reach a 4 plane window with no likely hit or we reach 
  // the end of the detector
  while(PlanesSinceLastHit<4 && NewPlane>0 && NewPlane<=485) {
    if(SlcStripData[NewPlane].size()>0) {

      if(SlcStripData[NewPlane][0].csh->GetPlaneView()==2) {PlaneView=0;}
      else {PlaneView=1;}
      
      for(int i=0; i<5; ++i) {StateVector[i]=x_k_minus[i];}
      SwimBack=Swim(StateVector, NState, Plane, NewPlane, GoForward);
      if(!SwimBack){break;}
      for(int i=0; i<5; ++i) {x_k[i]=NState[i];}
 
 
      // Find the closest strip (within a distance 'MinDistanceToStrip') and
      // temporarily store CandStripHandle
      // Results are very sensitive to value of MinDistanceToStrip
      CandStripHandle* CurrentStrip=0;
      MinDistanceToStrip=(1.5*StripWidth)+ fabs(0.0055*x_k[PlaneView+2]);
      
      for(unsigned int j=0; j<SlcStripData[NewPlane].size(); ++j) {
	StripDistance=fabs(SlcStripData[NewPlane][j].csh->GetTPos()-x_k[PlaneView]);
	
	if(StripDistance<MinDistanceToStrip) {
	  MinDistanceToStrip=StripDistance;
	  CurrentStrip=SlcStripData[NewPlane][j].csh;
	}
      }            
      
      // If we find a likely track strip, add it to the fit data and call the Kalman
      // update equations before repeating process to find next track strips in the shower
      if(CurrentStrip) {
	StripStruct temp;
	temp.csh = CurrentStrip;
	InitTrkStripData[NewPlane].push_back(temp);
	
	// Convert the strip to data required for Kalman fit
	GetFitData(NewPlane,NewPlane);

	// Carry out the Kalman fit
	if (PlaneView==1) {H_k[0]=0; H_k[1]=1; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	else {H_k[0]=1; H_k[1]=0; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	
	bool CombiPropagatorOk=GetCombiPropagator(Plane,NewPlane,GoForward);
	
	if(CombiPropagatorOk ) {
	  GetNoiseMatrix(Plane,NewPlane);
	  ExtrapCovMatrix();
	  CalcKalmanGain(NewPlane);
	  UpdateStateVector(Plane,NewPlane,GoForward);
	  UpdateCovMatrix();
	  MoveArrays();
	  StoreFilteredData(NewPlane);
	  
	  if(ZIncreasesWithTime) {MinPlane=NewPlane; Plane=MinPlane;}
	  else {MaxPlane=NewPlane; Plane=MaxPlane;}
	  NewPlane=Plane+Increment;
	  
	  PlanesSinceLastHit=0;
	}
      }
      else {NewPlane+=Increment; PlanesSinceLastHit++;}
      
    }
    else {NewPlane+=Increment; PlanesSinceLastHit++;}
  }
  
  // Note that shower swim is complete
  SwimThroughShower=false;

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GetFitData(int Plane1, int Plane2)
{
  // Loop over the initial track strip data and create the final data for fitting
  MSG("AlgFitTrackCam",Msg::kDebug) << "GetFitData" << endl;

  // Initialisations
  double MisalignmentError=4e-6;  // Squared error for misalignment of strips
  double SumChargeTPos=0; double SumCharge=0; int MaxStrip=-20; int MinStrip=200;
  bool NewStripFound=true;

  int ThisStrip;

  // Get the data for region between the planes specified
  for(int i=Plane1; i<=Plane2; ++i) {
    if(InitTrkStripData[i].size()>0) {     
      
      // Find max and min strip numbers for strips in plane
      for(unsigned int j=0; j<InitTrkStripData[i].size(); ++j) {
	if(InitTrkStripData[i][j].csh->GetStrip()<MinStrip) {MinStrip=InitTrkStripData[i][j].csh->GetStrip();}
	if(InitTrkStripData[i][j].csh->GetStrip()>MaxStrip) {MaxStrip=InitTrkStripData[i][j].csh->GetStrip();}	
      }


      // Find continuous groups of strips
      /////////////////////////////
      NewStripFound=true;
      
      while(NewStripFound==true) {
	
	NewStripFound=false;
	
	for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {

	  ThisStrip=SlcStripData[i][j].csh->GetStrip();

	  if( ThisStrip==(MaxStrip+1) ) {
	    MaxStrip+=1;
	    NewStripFound=true;
	    
	    InitTrkStripData[i].push_back(SlcStripData[i][j]);
	  }
	  
	  if( ThisStrip==(MinStrip-1) ) {
	    MinStrip-=1;
	    NewStripFound=true;
	    
	    InitTrkStripData[i].push_back(SlcStripData[i][j]);
	  }
	}
      }
      /////////////////////////////



      // Get the data for fitting
      /////////////////////////////
      for(unsigned int j=0; j<InitTrkStripData[i].size(); ++j) {
	SumCharge+=InitTrkStripData[i][j].csh->GetCharge();

	// JAM 11/08:  Change to support strip rotations
	PlaneView::PlaneView_t plnvw =InitTrkStripData[i][j].csh->GetPlaneView();
	Double_t orthopos = track->GetV(InitTrkStripData[i][j].csh->GetPlane());
	if(plnvw == PlaneView::kV){
	  orthopos = track->GetU(InitTrkStripData[i][j].csh->GetPlane());
	}
	// JAM 11/08:  to activate strip rotation support add orthopos as argument to GetTPos()
	SumChargeTPos+=InitTrkStripData[i][j].csh->GetCharge()*InitTrkStripData[i][j].csh->GetTPos();
      }

      // Charge weighted TPos and Flat distribution error
      if(SumCharge!=0.) {
	TrkDataStruct temp;
	
	temp.ZPos=InitTrkStripData[i][0].csh->GetZPos();
	temp.PlaneView=InitTrkStripData[i][0].csh->GetPlaneView();

	temp.TPos=SumChargeTPos/SumCharge;
	temp.TPosError=( (1.406305333e-4 * (1 + MaxStrip-MinStrip) )+ MisalignmentError);
	
	TrkStripData[i].push_back(temp);
      }
      /////////////////////////////


      // Reset      
      SumChargeTPos=0; SumCharge=0; MaxStrip=-20; MinStrip=200;
    }
  }
  
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::FillGapsInTrack()
{
  // If there is no filtered data for a plane (between MinPlane and MaxPlane),
  // but this plane has hits in the slice, we interpolate from the nearest 
  // state vectors
  //
  // As with all filtered data, the interpolated data will be compared to
  // strip positions in the FindTheStrips method
  MSG("AlgFitTrackCam",Msg::kDebug) << "FillGapsInTrack" << endl;


  int CurrentPlane; int ForwardsPlane; int BackwardsPlane;
  int Plane; int NewPlane;  bool GoForward;
  double StateVector[5]; double Prediction[5]; bool GetPrediction;
  for(int i=0; i<5; i++) { Prediction[i]=0.; }


  for (int i=MinPlane; i<=MaxPlane; ++i) {   
    if(SlcStripData[i].size()>0) {
 
      if(FilteredData[i].size()==0) {


	// Find nearest filtered state vectors (within two planes) and ZPos differences
	///////////////////////////////
	// Forwards
	CurrentPlane=i+1; ForwardsPlane=-99;

	while(CurrentPlane<=MaxPlane && CurrentPlane<=(i+2)) {
	  if(FilteredData[CurrentPlane].size()>0) {ForwardsPlane=CurrentPlane; break;}
	  else {CurrentPlane++;}
	}

	// Backwards
	CurrentPlane=i-1; BackwardsPlane=-99;

	while(CurrentPlane>=MinPlane && CurrentPlane>=(i-2) ) {
	  if(FilteredData[CurrentPlane].size()>0) {BackwardsPlane=CurrentPlane; break;}
	  else {CurrentPlane--;}
	}
	///////////////////////////////


	// Find and store possible new filtered data, range and dS
	///////////////////////////////
	if(ForwardsPlane!=-99 && BackwardsPlane!=-99) {

	  // Swimmer method
	  GetPrediction=false;
	  NewPlane=i;
	  if(ZIncreasesWithTime==true) {Plane=ForwardsPlane; GoForward=false;}
	  else{Plane=BackwardsPlane; GoForward=true;}
	  if(FilteredData[Plane].size()>0) {
	    StateVector[0] = FilteredData[Plane][0].x_k0;
	    StateVector[1] = FilteredData[Plane][0].x_k1;
	    StateVector[2] = FilteredData[Plane][0].x_k2;
	    StateVector[3] = FilteredData[Plane][0].x_k3;
	    StateVector[4] = FilteredData[Plane][0].x_k4;	    
	    GetPrediction=Swim(StateVector, Prediction, Plane, NewPlane, GoForward);
	    
	    if(GetPrediction==true) {
     	      // Store possible new state vector
	      FiltDataStruct temp;
	      temp.x_k0 = Prediction[0];
	      temp.x_k1 = Prediction[1];
	      temp.x_k2 = Prediction[2]; 
	      temp.x_k3 = Prediction[3];
	      temp.x_k4 = Prediction[4];	      
	      FilteredData[i].push_back(temp);
	    }
	  }

	}
	///////////////////////////////
      }
    }
  }

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::FindTheStrips(CandFitTrackCamHandle &cth, bool MakeTheTrack)
{ 
  // Given output data from Kalman filter, we find the strips that match most closely
  // and store the CandStripHandles in plane order in InitTrkStripData
  //
  // At end of track fitting, method is called with MakeTheTrack==true, so fit track
  // strips are finalised

  // JM ADDED 6/07:  Add pulse height requirement for first 3 hits at track vertex, and 
  //                 remove isolated single hit at vertex. This reduces tendency to add 
  //                 extra hits upstream of vertex.
  
  // Initialisations
  ////////////////////////////////////  
  for (unsigned int i=0; i<490; ++i) {InitTrkStripData[i].clear();}

  double Extrem1=0; double Extrem2=0;
  double StripCentre=0;
  double StripWidth=4.108e-2;
  double HalfStripWidth=0.5*StripWidth;
  double TwoStripWidth=2.*StripWidth;  
  double PEthresh = 3.0;
  double MinDistanceToStrip;
  ////////////////////////////////////  
  
  int NoOfHitPlanes=0; int Counter=0; int PlanesAdded=0;
  Bool_t foundMIP=false; 
  Bool_t RemovedHit=false;
  for (int i=MinPlane; i<=MaxPlane; ++i) {if(FilteredData[i].size()>0) {NoOfHitPlanes++;} }

  // Loop over the entire output from Kalman filter
  ////////////////////////////////////  
  
  if(ZIncreasesWithTime==true){
    for (int i=MinPlane; i<=MaxPlane; ++i) {   
      if(FilteredData[i].size()>0) {
	Counter++;
	int numStrips = 0;
	
	// Mark the possible extremities in transverse position within scintillator
	// by multiplying gradient by half scintillator thickness and adding or subtracting
	if(SlcStripData[i][0].csh->GetPlaneView()==2) {
	  Extrem1=FilteredData[i][0].x_k0 + (0.0055*FilteredData[i][0].x_k2);
	  Extrem2=FilteredData[i][0].x_k0 - (0.0055*FilteredData[i][0].x_k2);
	}
	else {
	  Extrem1=FilteredData[i][0].x_k1 + (0.0055*FilteredData[i][0].x_k3);
	  Extrem2=FilteredData[i][0].x_k1 - (0.0055*FilteredData[i][0].x_k3);
	}
	
	// Add strips in the case that only one strip can have its centre within 
	// half a strip width of an extremal position...
	//////////////////////////////////// 
	if(fabs(Extrem1-Extrem2)<StripWidth) {
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    if(fabs(StripCentre-Extrem1)<HalfStripWidth || fabs(StripCentre-Extrem2)<HalfStripWidth) {
	      if(Counter>1 || SlcStripData[i][j].csh->GetCharge()>PEthresh || foundMIP==true || SlcStripData[i].size()==1){
		foundMIP=true;
		if(MakeTheTrack==true) {cth.AddDaughterLink(*SlcStripData[i][j].csh);}	      
		InitTrkStripData[i].push_back(SlcStripData[i][j]);
	
		if(j==0)PlanesAdded++;		
	      } 
	      
	      numStrips++;
	    }
	  }
	}
	//////////////////////////////////// 
	
	
	// ...Otherwise, cover the cases where multiple strips can have their centre
	// within half a strip width of an extremal position
	//////////////////////////////////// 
	else {
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    if( fabs(StripCentre-Extrem1)<HalfStripWidth || fabs(StripCentre-Extrem2)<HalfStripWidth
		|| (Extrem1>StripCentre && Extrem2<StripCentre) 
		|| (Extrem1<StripCentre && Extrem2>StripCentre) ) {
	      if(Counter>1 || SlcStripData[i][j].csh->GetCharge()>PEthresh || foundMIP==true){
		foundMIP=true;
		if(MakeTheTrack==true) {cth.AddDaughterLink(*SlcStripData[i][j].csh);}
		InitTrkStripData[i].push_back(SlcStripData[i][j]);
		if(j==0)PlanesAdded++;
	      }
	      numStrips++;	      

	    }
	  }
	}
	//////////////////////////////////// 		
	// If we have found no strips, we consider looking further, finding the closest  
	// strip within a distance 'MinDistanceToStrip' of an extremal position
	//////////////////////////////////// 
	if(numStrips==0) {
	  CandStripHandle* CurrentStrip=0;
	  
	  // Be more demanding near track vertex
	  if(Counter>2) {
	    MinDistanceToStrip = TwoStripWidth;
	    if(MakeTheTrack==true) MinDistanceToStrip = 2*TwoStripWidth;
	  }
	  else {MinDistanceToStrip=StripWidth;}
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    // Find the closest strip and temporarily store its CandStripHandle
	    if(fabs(StripCentre-Extrem1)<MinDistanceToStrip) {
	      MinDistanceToStrip=StripCentre-Extrem1;
	      CurrentStrip=SlcStripData[i][j].csh;
	    }
	    if(fabs(StripCentre-Extrem2)<MinDistanceToStrip) {
	      MinDistanceToStrip=StripCentre-Extrem2;
	      CurrentStrip=SlcStripData[i][j].csh;
	    }
	  }
	  
	  // If we have found a strip then we add it
	  if(CurrentStrip) {
	    if(Counter>1 || CurrentStrip->GetCharge()>PEthresh || foundMIP==true){
	      foundMIP=true;
	      if(MakeTheTrack==true) {cth.AddDaughterLink(*CurrentStrip);}	    
	      StripStruct temp;
	      temp.csh = CurrentStrip;
	      InitTrkStripData[i].push_back(temp);
	      PlanesAdded++;
	    }
	  }
	}
	//////////////////////////////////// 
      }
      if(PlanesAdded==3 && !RemovedHit ){  // remove first hit if it is separated by >1 plane from second
	Int_t np = 0;
	Int_t planebuf[3];
	Int_t pln = MinPlane;
	while(np<3 && pln<490){
	  if(InitTrkStripData[pln].size()>0){
	    planebuf[np] = InitTrkStripData[pln][0].csh->GetPlane();
	    np++;
	  }
	  pln++;
	}
	if(np==3 && SlcStripData[planebuf[0]].size()>1){
	  if(abs(planebuf[1]-planebuf[0])/abs(planebuf[2]-planebuf[1])>1.5){
	    for(unsigned int j=0; j<InitTrkStripData[planebuf[0]].size(); ++j) {
	      CandStripHandle* Strip = InitTrkStripData[planebuf[0]][j].csh;	 
	      cth.RemoveDaughter(Strip);
	      RemovedHit=true;
	    }
	    InitTrkStripData[planebuf[0]].clear();
	  }
	}
      }
    }
  }
  else{
    for (int i=MaxPlane; i>=MinPlane; --i) {   
      if(FilteredData[i].size()>0) {
	Counter++;
	int numStrips=0;
	
	
	// Mark the possible extremities in transverse position within scintillator
	// by multiplying gradient by half scintillator thickness and adding or subtracting
	if(SlcStripData[i][0].csh->GetPlaneView()==2) {
	  Extrem1=FilteredData[i][0].x_k0 + (0.0055*FilteredData[i][0].x_k2);
	  Extrem2=FilteredData[i][0].x_k0 - (0.0055*FilteredData[i][0].x_k2);
	}
	else {
	  Extrem1=FilteredData[i][0].x_k1 + (0.0055*FilteredData[i][0].x_k3);
	  Extrem2=FilteredData[i][0].x_k1 - (0.0055*FilteredData[i][0].x_k3);
	}
	
	
	// Add strips in the case that only one strip can have its centre within 
	// half a strip width of an extremal position...
	//////////////////////////////////// 
	if(fabs(Extrem1-Extrem2)<StripWidth) {
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    if(fabs(StripCentre-Extrem1)<HalfStripWidth || fabs(StripCentre-Extrem2)<HalfStripWidth) {
	      if(Counter>1 || SlcStripData[i][j].csh->GetCharge()>PEthresh || foundMIP|| SlcStripData[i].size()==1){
		foundMIP=true;
		if(MakeTheTrack==true) {cth.AddDaughterLink(*SlcStripData[i][j].csh);}
		InitTrkStripData[i].push_back(SlcStripData[i][j]);
		numStrips++;
		if(j==0)PlanesAdded++;
	      }      
	    }
	  }
	}
	//////////////////////////////////// 
	
	
	// ...Otherwise, cover the cases where multiple strips can have their centre
	// within half a strip width of an extremal position
	//////////////////////////////////// 
	else {
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    if( fabs(StripCentre-Extrem1)<HalfStripWidth || fabs(StripCentre-Extrem2)<HalfStripWidth
		|| (Extrem1>StripCentre && Extrem2<StripCentre) 
		|| (Extrem1<StripCentre && Extrem2>StripCentre) ) {
	      if(Counter>1 || SlcStripData[i][j].csh->GetCharge()>PEthresh || foundMIP){
		foundMIP=true;
		if(MakeTheTrack==true) {cth.AddDaughterLink(*SlcStripData[i][j].csh);}		
		InitTrkStripData[i].push_back(SlcStripData[i][j]);
		numStrips++;
		if(j==0)PlanesAdded++;
	      }
	    }
	  }
	}
	//////////////////////////////////// 
	
	// If we have found no strips, we consider looking further, finding the closest  
	// strip within a distance 'MinDistanceToStrip' of an extremal position
	//////////////////////////////////// 
	if(numStrips==0) {
	  CandStripHandle* CurrentStrip=0;
	  
	  // Be more demanding near track vertex
	  if(Counter>2) {
	    MinDistanceToStrip = TwoStripWidth;
	    if(MakeTheTrack==true) MinDistanceToStrip = 2*TwoStripWidth;
	  }
	  else {MinDistanceToStrip=StripWidth;}
	  
	  for(unsigned int j=0; j<SlcStripData[i].size(); ++j) {
	    StripCentre=SlcStripData[i][j].csh->GetTPos();
	    
	    // Find the closest strip and temporarily store its CandStripHandle
	    if(fabs(StripCentre-Extrem1)<MinDistanceToStrip) {
	      MinDistanceToStrip=StripCentre-Extrem1;
	      CurrentStrip=SlcStripData[i][j].csh;
	    }
	    if(fabs(StripCentre-Extrem2)<MinDistanceToStrip) {
	      MinDistanceToStrip=StripCentre-Extrem2;
	      CurrentStrip=SlcStripData[i][j].csh;
	    }
	  }
	  
	  // If we have found a strip then we add it
	  if(CurrentStrip) {
	    if(Counter>1 || CurrentStrip->GetCharge()>PEthresh || foundMIP){
	      foundMIP=true;    
	      if(MakeTheTrack==true) {cth.AddDaughterLink(*CurrentStrip);}
	      StripStruct temp;
	      temp.csh = CurrentStrip;
	      InitTrkStripData[i].push_back(temp);
	      PlanesAdded++;
	    }
	  }
	}
	//////////////////////////////////// 
      }
     if(PlanesAdded==3 && !RemovedHit){  // remove first hit if it is separated by >1 plane from second
	Int_t np = 0;
	Int_t planebuf[3];
	Int_t pln = MaxPlane;
	while(np<3 && pln>=0){
	  if(InitTrkStripData[pln].size()>0){
	    planebuf[np]= InitTrkStripData[pln][0].csh->GetPlane();
	    np++;
	  }
	  pln--;
	}
	if(np==3 && SlcStripData[planebuf[0]].size()>1){
	  if(abs(planebuf[1]-planebuf[0])/abs(planebuf[2]-planebuf[1])>1.5){
	  for(unsigned int j=0; j<InitTrkStripData[planebuf[0]].size(); ++j) {
	    CandStripHandle* Strip = InitTrkStripData[planebuf[0]][j].csh;	 
	    cth.RemoveDaughter(Strip);
	    RemovedHit=true;
	  }
	  InitTrkStripData[planebuf[0]].clear();
	  }
	}
      }
    }  
  }
  ////////////////////////////////////  
  

  // Find new max and min planes
  MaxPlane=-20; MinPlane=500;
  for (int i=0; i<490; ++i) {   
    if(InitTrkStripData[i].size()>0) {
      if(i>MaxPlane) {MaxPlane=i;}
      if(i<MinPlane) {MinPlane=i;}
    }
  }

  if(MaxPlane==-20 || MinPlane==500) {return false;}
  else {return true;}

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GoForwards()
{ 
  // Carry out the Kalman fit along the track in the direction of increasing z
  MSG("AlgFitTrackCam",Msg::kDebug) << "GoForwards, carry out fit in positive z direction" << endl;

  // JAM in 2nd iteration, stop when end of range is reached.
 
  Int_t StartPlane = MinPlane; Int_t EndPlane=MaxPlane;
  if(!ZIncreasesWithTime){
    StartPlane = EndofRangePlane;
  }
  else EndofRangePlane = MaxPlane;

  for (int i=StartPlane; i<=EndPlane; ++i) {
    EndofRange = false;
    if (TrkStripData[i].size()>0) {
      if (PassTrack) {
	// Find Next Plane
	int NewPlane=-99;
	int k=(i+1);
	while (k<=MaxPlane) {
	  if (TrkStripData[k].size()>0) {NewPlane=k; break;}
	  ++k;
	}
	if (NewPlane!=-99) {
	  // Define measurement function
	  if (TrkStripData[NewPlane][0].PlaneView==3) {H_k[0]=0; H_k[1]=1; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	  else if (TrkStripData[NewPlane][0].PlaneView==2)  {H_k[0]=1; H_k[1]=0; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	  
 	  MSG("AlgFitTrackCam",Msg::kVerbose) << "GoForwards, Plane " << i << " ZPos " << TrkStripData[i][0].ZPos 
					      << " PlaneView " << TrkStripData[i][0].PlaneView << endl
					      << " NewPlane " << NewPlane << " NewZPos " << TrkStripData[NewPlane][0].ZPos 
					      << " NewPlaneView " << TrkStripData[NewPlane][0].PlaneView << endl;
	  
	  bool CombiPropagatorOk=GetCombiPropagator(i,NewPlane,true);
	  
	  if(CombiPropagatorOk ) {
	    GetNoiseMatrix(i,NewPlane);
	    ExtrapCovMatrix();
	    CalcKalmanGain(NewPlane);
	    UpdateStateVector(i,NewPlane,true);
	    UpdateCovMatrix();
	    MoveArrays();
 	    if(SaveData) {StoreFilteredData(NewPlane);}
	    MSG("AlgFitTrackCam",Msg::kVerbose) << "GoForwards, Filtered q/p " << x_k[4] << endl;
	  }
	  else 
	    {
	      PassTrack=false; 
	    }
	}
	
      }
      else {MSG("AlgFitTrackCam",Msg::kDebug) << "GoForwards, Outside of detector - track failed" << endl;}
    }
    //JAM end of range found
    if(EndofRange && LastIteration && ZIncreasesWithTime){
      EndofRangePlane=i;
      break;
    }
  }


  // Store entries from covariance matrix for use in setting track properties
  if(NIter>1) {
    if(ZIncreasesWithTime==true) {
      EndCov[0]=C_k[0][0]; EndCov[1]=C_k[1][1];
      EndCov[2]=C_k[2][2]; EndCov[3]=C_k[3][3];
      EndCov[4]=C_k[4][4];
    }
    else {
      VtxCov[0]=C_k[0][0]; VtxCov[1]=C_k[1][1];
      VtxCov[2]=C_k[2][2]; VtxCov[3]=C_k[3][3];
      VtxCov[4]=C_k[4][4];
    }
  }

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GoBackwards()
{ 
  // Carry out the Kalman fit along the track in the direction of decreasing z
  MSG("AlgFitTrackCam",Msg::kDebug) << "GoBackwards, carry out fit in negative z direction" << endl;
  Int_t StartPlane = MaxPlane; Int_t EndPlane=MinPlane;
  if(ZIncreasesWithTime){
    StartPlane = EndofRangePlane;
  }
  else EndofRangePlane = MinPlane;
 
  for (int i=StartPlane; i>=EndPlane; --i) {  
    if (TrkStripData[i].size()>0) {
      if (PassTrack) {

	//Find Prev Plane
	int NewPlane=-99;
	int k=(i-1);
	while (k>=MinPlane) {
	  if (TrkStripData[k].size()>0) {NewPlane=k; break;}
	  --k;
	}
	
	
	if (NewPlane!=-99) {
	  // Define measurement function
	  if (TrkStripData[NewPlane][0].PlaneView==3) {H_k[0]=0; H_k[1]=1; H_k[2]=0; H_k[3]=0; H_k[4]=0;}	  else if (TrkStripData[NewPlane][0].PlaneView==2)  {H_k[0]=1; H_k[1]=0; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	  
 	  MSG("AlgFitTrackCam",Msg::kVerbose) << "GoBackwards, Plane " << i << " ZPos " << TrkStripData[i][0].ZPos 
					      << " PlaneView " << TrkStripData[i][0].PlaneView << endl
					      << " NewPlane " << NewPlane << " NewZPos " << TrkStripData[NewPlane][0].ZPos 
					      << " NewPlaneView " << TrkStripData[NewPlane][0].PlaneView << endl;
	  
	  bool CombiPropagatorOk=GetCombiPropagator(i,NewPlane,false);
	  
	  if(CombiPropagatorOk ) {
	    GetNoiseMatrix(i,NewPlane);
	    ExtrapCovMatrix();
	    CalcKalmanGain(NewPlane);
	    UpdateStateVector(i,NewPlane,false);
	    UpdateCovMatrix();
	    MoveArrays();
	    if(SaveData) {StoreFilteredData(NewPlane);}
	    MSG("AlgFitTrackCam",Msg::kVerbose) << "GoBackwards, Filtered q/p " << x_k[4] << endl;
	  }
	  else {
	    PassTrack=false;
	  }
	}
	
      }
      else {MSG("AlgFitTrackCam",Msg::kDebug) << "GoBackwards, Outside detector - track failed" << endl;}
    }
    //JAM end of range found
    if(EndofRange && LastIteration && !ZIncreasesWithTime){
      EndofRangePlane = i;
      break;
    }

  }


  // Store entries from covariance matrix for use in setting track properties
  if(NIter>1) {
    if(ZIncreasesWithTime==true) {
      VtxCov[0]=C_k[0][0]; VtxCov[1]=C_k[1][1];
      VtxCov[2]=C_k[2][2]; VtxCov[3]=C_k[3][3];
      VtxCov[4]=C_k[4][4];
    }
    else {
      EndCov[0]=C_k[0][0]; EndCov[1]=C_k[1][1];
      EndCov[2]=C_k[2][2]; EndCov[3]=C_k[3][3];
      EndCov[4]=C_k[4][4];
    }
  }
  
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::GetCombiPropagator(const int Plane, const int NewPlane, const bool GoForward)
{
  // Combination propagator, essentially same as SR propagator, but
  // generation of matrix reduces calls to swimmer by 80%
  MSG("AlgFitTrackCam",Msg::kDebug) << "GetCombiPropagator" << endl;

  for (int i=0; i<5; ++i) {
    for (int j=0; j<5; ++j) {
      F_k_minus[i][j]=0;  
    }
  }

  F_k_minus[0][0]=1; F_k_minus[1][1]=1; 
  F_k_minus[2][2]=1; F_k_minus[3][3]=1;
  
  DeltaZ=fabs(TrkStripData[NewPlane][0].ZPos-TrkStripData[Plane][0].ZPos);
  DeltaPlane=abs(NewPlane-Plane);

  // Swimmer section for last column
  double PState[5];  double NState[5];  double StateVector[5];
  double Increment=0.01;
  bool SwimInc=false; bool SwimDec=false;
  int nswimfail=0;
  
  if(GoForward==true) {F_k_minus[0][2]=DeltaZ; F_k_minus[1][3]=DeltaZ;}
  else if(GoForward==false) {F_k_minus[0][2]=-DeltaZ; F_k_minus[1][3]=-DeltaZ;}
  
  
  // Give swimmer fixed number of opportunities for successful swim
  while((SwimInc==false || SwimDec==false) && nswimfail<=10) {
    
    Increment=0.05*fabs(x_k_minus[4]); 
    if(Increment<.01) {Increment=.01;}
       
    for(int j=0; j<5; ++j) {StateVector[j]=x_k_minus[j];}  
    
    // Increment then swim
    StateVector[4]+=Increment;
    SwimInc=Swim(StateVector, NState, Plane, NewPlane, GoForward);
    
    StateVector[4]=x_k_minus[4];
    
    // Decrement then swim
    StateVector[4]-=Increment;
    SwimDec=Swim(StateVector, PState, Plane, NewPlane, GoForward);
    
    // If swim failed, double momentum and swim again
    if(SwimInc==false || SwimDec==false) {
      MSG("AlgFitTrackCam",Msg::kVerbose) << "GetCombiPropagator, Swim failed - Double momentum and swim again" << endl;
      x_k_minus[4]*=0.5;
      nswimfail++; TotalNSwimFail++;
      break;
    }
    
    // Form last row of propagator matrix.  Need to transpose to get proper Kalman F_k_minus
    else {
      if(Increment!=0.) {
	for(int j=0; j<5; ++j) {
	  F_k_minus[j][4] = (NState[j]-PState[j]) / (2*Increment);
	}
      }
      else {F_k_minus[4][4]=1;}
    }
    
  } // End while statement
  
  if(nswimfail>10) {MSG("AlgFitTrackCam",Msg::kDebug) << "GetCombiPropagator, nswimfail>10, fail track" << endl; return false;}


  // Display
  if(debug) {
    cout << "Combi F_k_minus" << endl;
    for(int i=0; i<5; ++i) {  
      for(int j=0; j<5; ++j) {
 	cout << F_k_minus[i][j] << " ";
      }
      cout << endl;
    }
  }
  
  return true;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::Swim(double* StateVector, double* Output, const int Plane, 
			  const int NewPlane,const bool GoForward, double* dS, double* Range, double* dE)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "Swim" << endl;

  // Initialisations
  // customize for bfield scaling.
  BField * bf = new BField(*vldc,-1,0);
  SwimSwimmer* myswimmer = new SwimSwimmer(*vldc,bf);
  
  if(UseGeoSwimmer) GeoSwimmer::Instance()->Initialize(*vldc);
 
  double invSqrt2 = pow(1./2.,0.5);
  double charge = 0.;
  bool done = false;
    
  if(fabs(StateVector[4])>1.e-10) {
    double modp = fabs(1./StateVector[4]);
    
    // Fix, to account for fact the cosmic muons could move in direction of negative z
    if(ZIncreasesWithTime==false) {modp=-modp;}
    
    double dsdz = pow((1.+pow(StateVector[2],2)+pow(StateVector[3],2)),0.5);
    double dxdz = invSqrt2*(StateVector[2]-StateVector[3]);
    double dydz = invSqrt2*(StateVector[2]+StateVector[3]);
    
    // Set up current muon details
    if(StateVector[4]>0.) charge = 1.;
    else if(StateVector[4]<0.) charge = -1.;
    
    TVector3 position(invSqrt2*(StateVector[0]-StateVector[1]),
		      invSqrt2*(StateVector[0]+StateVector[1]),
		      SlcStripData[Plane][0].csh->GetZPos());

    TVector3 momentum(modp*(dxdz/dsdz),
		      modp*(dydz/dsdz),
		      modp/dsdz);

    TVector3 bfield = bf->GetBField(position);
    bave += TMath::Sqrt(bfield[0]*bfield[0]+bfield[1]*bfield[1]+bfield[2]*bfield[2]);
    nbfield++;

    SwimParticle muon(position,momentum);
    muon.SetCharge(charge);
    SwimZCondition zc(SlcStripData[NewPlane][0].csh->GetZPos());
    // Do the swim, accounting for direction of motion w.r.t time too
    if( (GoForward==true && ZIncreasesWithTime==true)  || (GoForward==false && ZIncreasesWithTime==false) ) {
      if(UseGeoSwimmer) {
      	done = GeoSwimmer::Instance()->SwimForward(muon,SlcStripData[NewPlane][0].csh->GetZPos());
      } else {
	done = myswimmer->SwimForward(muon,zc);
      }
    }
    else if( (GoForward==true && ZIncreasesWithTime==false)  || (GoForward==false && ZIncreasesWithTime==true) ) {
      if(UseGeoSwimmer) {
      	done = GeoSwimmer::Instance()->SwimBackward(muon,SlcStripData[NewPlane][0].csh->GetZPos());
      } else {
	done = myswimmer->SwimBackward(muon,zc);
      }
    }
    if(done==true) {
      if(muon.GetDirection().Z()!=0. && muon.GetMomentumModulus()!=0.) {
	Output[0]=invSqrt2*(muon.GetPosition().Y()+muon.GetPosition().X());
	Output[1]=invSqrt2*(muon.GetPosition().Y()-muon.GetPosition().X());
	Output[2]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())+(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[3]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())-(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[4]=muon.GetCharge()/muon.GetMomentumModulus();
	// Get range and dS from the Swimmer
	if(dS) {*dS=muon.GetS();} if(Range) {*Range=muon.GetRange();} if(dE){*dE=muon.GetMomentumModulus()-momentum.Mag();} 
      }
      else {done=false;}
    }
    
  }

  else{
    // If infinite momentum, use straight line extrapolation
    double delz = (SlcStripData[NewPlane][0].csh->GetZPos()-SlcStripData[Plane][0].csh->GetZPos());
    Output[0]=StateVector[0] + StateVector[2]*delz;
    Output[1]=StateVector[1] + StateVector[3]*delz;
    Output[2]=StateVector[2];
    Output[3]=StateVector[3];
    Output[4]=StateVector[4];

    done=true;
  }    
  
  delete myswimmer;
  delete bf;
  return done;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::Swim(double* StateVector, double* Output, const double z, 
			  const int NewPlane,const bool GoForward, double* dS, double* Range, double* dE)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "Swim, specified starting Z" << endl;

  // Initialisations
  // customize for bfield scaling.
  BField * bf = new BField(*vldc,-1,0);
  SwimSwimmer* myswimmer = new SwimSwimmer(*vldc,bf);
  
  if(UseGeoSwimmer) GeoSwimmer::Instance()->Initialize(*vldc);

  double invSqrt2 = pow(1./2.,0.5);
  double charge = 0.;
  bool done = false;
    
  if(fabs(StateVector[4])>1.e-10) {
    double modp = fabs(1./StateVector[4]);
    
    // Fix, to account for fact the cosmic muons could move in direction of negative z
    if(ZIncreasesWithTime==false) {modp=-modp;}
    
    double dsdz = pow((1.+pow(StateVector[2],2)+pow(StateVector[3],2)),0.5);
    double dxdz = invSqrt2*(StateVector[2]-StateVector[3]);
    double dydz = invSqrt2*(StateVector[2]+StateVector[3]);
    
    // Set up current muon details
    if(StateVector[4]>0.) charge = 1.;
    else if(StateVector[4]<0.) charge = -1.;
    
    TVector3 position(invSqrt2*(StateVector[0]-StateVector[1]),
		      invSqrt2*(StateVector[0]+StateVector[1]),
		      z);

    TVector3 momentum(modp*(dxdz/dsdz),
		      modp*(dydz/dsdz),
		      modp/dsdz);
    SwimParticle muon(position,momentum);
    muon.SetCharge(charge);
    SwimZCondition zc(SlcStripData[NewPlane][0].csh->GetZPos());

   
    // Do the swim, accounting for direction of motion w.r.t time too
    if( (GoForward==true && ZIncreasesWithTime==true)  || (GoForward==false && ZIncreasesWithTime==false) ) {
      if(UseGeoSwimmer) {
      	done = GeoSwimmer::Instance()->SwimForward(muon,SlcStripData[NewPlane][0].csh->GetZPos());
      } else {
	done = myswimmer->SwimForward(muon,zc);
      } 
    }
    else if( (GoForward==true && ZIncreasesWithTime==false)  || (GoForward==false && ZIncreasesWithTime==true) ) {
      if(UseGeoSwimmer) {
      	done = GeoSwimmer::Instance()->SwimBackward(muon,SlcStripData[NewPlane][0].csh->GetZPos());

      } else {
	done = myswimmer->SwimBackward(muon,zc);
      }     
    }
    if(done==true) {
      if(muon.GetDirection().Z()!=0. && muon.GetMomentumModulus()!=0.) {
	Output[0]=invSqrt2*(muon.GetPosition().Y()+muon.GetPosition().X());
	Output[1]=invSqrt2*(muon.GetPosition().Y()-muon.GetPosition().X());
	Output[2]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())+(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[3]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())-(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[4]=muon.GetCharge()/muon.GetMomentumModulus();
	// Get range and dS from the Swimmer
	if(dS) {*dS=muon.GetS();} if(Range) {*Range=muon.GetRange();} if(dE){*dE=muon.GetMomentumModulus()-momentum.Mag();} 
      }
      else {done=false;}
    }
    
  }

  else{
    // If infinite momentum, use straight line extrapolation
    double delz = (SlcStripData[NewPlane][0].csh->GetZPos()-z);
    Output[0]=StateVector[0] + StateVector[2]*delz;
    Output[1]=StateVector[1] + StateVector[3]*delz;
    Output[2]=StateVector[2];
    Output[3]=StateVector[3];
    Output[4]=StateVector[4];

    done=true;
  }    
  
  delete myswimmer;
  delete bf;

  return done;
}
////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::Swim(double* StateVector, double* Output, const int Plane, 
			  const double Zend,const bool GoForward, double* dS, double* Range, double* dE)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "Swim, specified end Z" << endl;

  // Initialisations
  // customize for bfield scaling.

  BField * bf = new BField(*vldc,-1,0);
  SwimSwimmer* myswimmer = new SwimSwimmer(*vldc,bf);

  if(UseGeoSwimmer) GeoSwimmer::Instance()->Initialize(*vldc);

  double invSqrt2 = pow(1./2.,0.5);
  double charge = 0.;
  bool done = false;
    
  if(fabs(StateVector[4])>1.e-10) {
    double modp = fabs(1./StateVector[4]);
    
    // Fix, to account for fact the cosmic muons could move in direction of negative z
    if(ZIncreasesWithTime==false) {modp=-modp;}
    
    double dsdz = pow((1.+pow(StateVector[2],2)+pow(StateVector[3],2)),0.5);
    double dxdz = invSqrt2*(StateVector[2]-StateVector[3]);
    double dydz = invSqrt2*(StateVector[2]+StateVector[3]);
  
    // Set up current muon details
    if(StateVector[4]>0.) charge = 1.;
    else if(StateVector[4]<0.) charge = -1.;
    
    TVector3 position(invSqrt2*(StateVector[0]-StateVector[1]),
		      invSqrt2*(StateVector[0]+StateVector[1]),
		      SlcStripData[Plane][0].csh->GetZPos());


    TVector3 momentum(modp*(dxdz/dsdz),
		      modp*(dydz/dsdz),
		      modp/dsdz);
    SwimParticle muon(position,momentum);
    muon.SetCharge(charge);
    SwimZCondition zc(Zend);
 
    // Do the swim, accounting for direction of motion w.r.t time too
    if( (GoForward==true && ZIncreasesWithTime==true)  || (GoForward==false && ZIncreasesWithTime==false) ) {
      if(UseGeoSwimmer){
      	done = GeoSwimmer::Instance()->SwimForward(muon,Zend);
      } else {
	done = myswimmer->SwimForward(muon,zc);
      }   
    }
    else if( (GoForward==true && ZIncreasesWithTime==false)  || (GoForward==false && ZIncreasesWithTime==true) ) {
      if(UseGeoSwimmer){
      	done = GeoSwimmer::Instance()->SwimBackward(muon,Zend);
      } else {
	done = myswimmer->SwimBackward(muon,zc);
      }    
    }
    if(done==true) {
      if(muon.GetDirection().Z()!=0. && muon.GetMomentumModulus()!=0.) {
	Output[0]=invSqrt2*(muon.GetPosition().Y()+muon.GetPosition().X());
	Output[1]=invSqrt2*(muon.GetPosition().Y()-muon.GetPosition().X());
	Output[2]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())+(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[3]=invSqrt2*((muon.GetDirection().Y()/muon.GetDirection().Z())-(muon.GetDirection().X()/muon.GetDirection().Z()));
	Output[4]=muon.GetCharge()/muon.GetMomentumModulus();
	// Get range and dS from the Swimmer
	if(dS) {*dS=muon.GetS();} if(Range) {*Range=muon.GetRange();} if(dE){*dE=muon.GetMomentumModulus()-momentum.Mag();}
      }
      else {done=false;}
    }
    
  }

  else{
    // If infinite momentum, use straight line extrapolation
    double delz = (Zend-SlcStripData[Plane][0].csh->GetZPos());
    Output[0]=StateVector[0] + StateVector[2]*delz;
    Output[1]=StateVector[1] + StateVector[3]*delz;
    Output[2]=StateVector[2];
    Output[3]=StateVector[3];
    Output[4]=StateVector[4];

    done=true;
  }    
  
  delete myswimmer;
  delete bf;
  return done;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::ResetCovarianceMatrix()
{
  // Simple method reset variables/arrays to allow propagation again

  DeltaPlane=0; DeltaZ=0; 
  GetInitialCovarianceMatrix(false);
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GetInitialCovarianceMatrix(const bool FirstIteration)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "GetInitialCovarianceMatrix" << endl;

  if(FirstIteration==true) {
    
    for(int i=0; i<5; ++i) {
      for(int j=0; j<5; ++j) {
	C_k_minus[i][j]=0.;
      }
    }
    
    // Diagonal terms
    C_k_minus[0][0]=0.25; C_k_minus[1][1]=0.25; 
    C_k_minus[2][2]=100.; C_k_minus[3][3]=100.; 
    C_k_minus[4][4]=1.;
    
    // Off diagonal terms. Taken from SR - Origin of this?
    if(ZIncreasesWithTime==true) {
      C_k_minus[0][4]=7.5e-5;  C_k_minus[1][4]=7.5e-5;
      C_k_minus[4][0]=7.5e-5;  C_k_minus[4][1]=7.5e-5;
    }
    else if(ZIncreasesWithTime==false) {
      C_k_minus[0][4]=-7.5e-5;  C_k_minus[1][4]=-7.5e-5;
      C_k_minus[4][0]=-7.5e-5;  C_k_minus[4][1]=-7.5e-5;
    }
    
    
  }

  else if(FirstIteration==false) {
    // Results are very sensitive to this multiplication. A large number means
    // that further iterations start with the same uncertainties as the first,
    // albeit with improved "track finder" strips
    for(int i=0; i<5; ++i) {C_k_minus[i][i]*=100;}
    
    
    // Make sure not larger than very first covariance elements
    C_k_minus[0][0]=min(C_k_minus[0][0],0.25); C_k_minus[1][1]=min(C_k_minus[1][1],0.25);
    C_k_minus[2][2]=min(C_k_minus[2][2],100.); C_k_minus[3][3]=min(C_k_minus[3][3],100.);
    C_k_minus[4][4]=min(C_k_minus[4][4],1.);

    double cov_xqp = 7.5e-5;             // Taken from SR - Origin of this?
    
    for(int i=0; i<2; ++i){
      if(fabs(C_k_minus[i][4])>cov_xqp) C_k_minus[i][4] = (C_k_minus[i][4] > 0 ? cov_xqp : -cov_xqp);
      if(fabs(C_k_minus[4][i])>cov_xqp) C_k_minus[4][i] = (C_k_minus[4][i] > 0 ? cov_xqp : -cov_xqp);
    }

    cov_xqp /= 0.06;                     // Taken from SR - Origin of this?

    for(int i=2; i<4; ++i){
      if(fabs(C_k_minus[i][4])>cov_xqp) C_k_minus[i][4] = (C_k_minus[i][4] > 0 ? cov_xqp : -cov_xqp);
      if(fabs(C_k_minus[4][i])>cov_xqp) C_k_minus[4][i] = (C_k_minus[4][i] > 0 ? cov_xqp : -cov_xqp);
    }
  }


  // Display
  if(debug) {
    cout << "Initial covariance matrix" << endl;
    for(int p=0; p<5; ++p){
      for(int q=0; q<5; ++q){
 	cout << C_k_minus[p][q] << " ";
      }  
      cout << endl;
    }
  }

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GetNoiseMatrix(const int Plane, const int NewPlane)
{
  // This method is essentially the same as in SR fitter
  MSG("AlgFitTrackCam",Msg::kDebug) << "GetNoiseMatrix" << endl;

  for (int p=0; p<5; ++p) {
    for (int q=0; q<5; ++q) {
      Q_k_minus[p][q]=0;    }
  }
  
  // Get gradients, etc from x_k_minus
  double dsdzSquared = 1.+pow(x_k_minus[2],2)+pow(x_k_minus[3],2);
  double dsdz = pow(dsdzSquared,0.5);

  // Implement noise matrix as in SR
  if (DeltaPlane!=-99 && DeltaZ!=-99) {  
    double qp = x_k_minus[4];
    if(fabs(qp)<0.01) qp = (qp>0 ? 0.01 : -0.01);
    int izdir = ((NewPlane-Plane)>0 ? 0 : 1);
  
    double LocalRadiationLength=(dsdz * double(DeltaPlane) * 1.47); // 1.47 radiation lengths per iron plane

    double SigmaMS=(0.0136 * fabs(qp) * pow(LocalRadiationLength,0.5)
		    * (1. + (0.038 * log(LocalRadiationLength)) ));
    double SigmaMSSquared=pow(SigmaMS,2);

    double Sigma33Squared=0.5*SigmaMSSquared*dsdzSquared*(1.+pow(x_k_minus[2],2));

    double Sigma34Squared=0.5*SigmaMSSquared*dsdzSquared*(x_k_minus[2]*x_k_minus[3]);

    double Sigma44Squared=0.5*SigmaMSSquared*dsdzSquared*(1.+pow(x_k_minus[3],2));;


    // Treat steel as discrete scatter
    SwimGeo *spil = SwimGeo::Instance(*(const_cast<VldContext*>(vldc))); // Get edges of steel
    double z1 = spil->GetSteel(TrkStripData[Plane][0].ZPos,izdir)->GetZ();
    double z2 = spil->GetSteel(z1,izdir)->GetZ();
  
    double dzscatter = fabs(TrkStripData[NewPlane][0].ZPos-0.5*(z1+z2));
    double dzscatter2 = pow(dzscatter,2);
  
    UgliGeomHandle ugh(*vldc);
    BField bf(*vldc,-1,0);
    TVector3 uvz;

    uvz(0) = x_k_minus[0];
    uvz(1) = x_k_minus[1];
    uvz(2) = ugh.GetNearestSteelPlnHandle(TrkStripData[Plane][0].ZPos).GetZ0();

    TVector3 buvz = bf.GetBField(uvz, true);
    buvz[0] *= 0.3;
    buvz[1] *= 0.3;
    buvz[2] *= 0.3;

    
    double eloss = 0.038 * double(DeltaPlane);
    double sigmaeloss2 = 0.25*eloss*dsdz*qp*qp;
    sigmaeloss2 *= sigmaeloss2;


    // Fill elements of noise matrix
    Q_k_minus[0][0]=dzscatter2*Sigma33Squared;
    Q_k_minus[0][1]=dzscatter2*Sigma34Squared;
    Q_k_minus[0][2]=dzscatter*Sigma33Squared;
    Q_k_minus[0][3]=dzscatter*Sigma34Squared;
    Q_k_minus[0][4]=dzscatter*sigmaeloss2*double(DeltaPlane)*buvz(1)*0.0254*dsdz*(1.+pow(x_k_minus[2],2));

    Q_k_minus[1][0]=dzscatter2*Sigma34Squared;
    Q_k_minus[1][1]=dzscatter2*Sigma44Squared;
    Q_k_minus[1][2]=dzscatter*Sigma34Squared;
    Q_k_minus[1][3]=dzscatter*Sigma44Squared;
    Q_k_minus[1][4]=dzscatter*sigmaeloss2*double(DeltaPlane)*buvz(0)*0.0254*dsdz*(1.+pow(x_k_minus[3],2));

    Q_k_minus[2][0]=dzscatter*Sigma33Squared;
    Q_k_minus[2][1]=dzscatter*Sigma34Squared;
    Q_k_minus[2][2]=Sigma33Squared;
    Q_k_minus[2][3]=Sigma34Squared;
    Q_k_minus[2][4]=sigmaeloss2*double(DeltaPlane)*buvz(1)*0.0254*dsdz*(1.+pow(x_k_minus[2],2));

    Q_k_minus[3][0]=dzscatter*Sigma34Squared;
    Q_k_minus[3][1]=dzscatter*Sigma44Squared;
    Q_k_minus[3][2]=Sigma34Squared;
    Q_k_minus[3][3]=Sigma44Squared;
    Q_k_minus[3][4]=sigmaeloss2*double(DeltaPlane)*buvz(0)*0.0254*dsdz*(1.+pow(x_k_minus[3],2));

    Q_k_minus[4][0]=dzscatter*sigmaeloss2*double(DeltaPlane)*buvz(1)*0.0254*dsdz*(1.+pow(x_k_minus[2],2));
    Q_k_minus[4][1]=dzscatter*sigmaeloss2*double(DeltaPlane)*buvz(0)*0.0254*dsdz*(1.+pow(x_k_minus[3],2));
    Q_k_minus[4][2]=sigmaeloss2*double(DeltaPlane)*buvz(1)*0.0254*dsdz*(1.+pow(x_k_minus[2],2));
    Q_k_minus[4][3]=sigmaeloss2*double(DeltaPlane)*buvz(0)*0.0254*dsdz*(1.+pow(x_k_minus[3],2));
    Q_k_minus[4][4]=sigmaeloss2;
  }
 

  // Display
  if(debug) {
    cout << "1e6 * Q_k_minus" << endl;
    for(int i=0; i<5; ++i) {
      for(int j=0; j<5; ++j) {
 	cout << 1e6*Q_k_minus[i][j] << " ";
      }
      cout << endl;
    }
  }

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::ExtrapCovMatrix()
{
  // C_k_intermediate = (F_k_minus * C_k_minus * F_k_minus^T) + Q_k_minus
  MSG("AlgFitTrackCam",Msg::kDebug) << "ExtrapCovMatrix" << endl;

  for (int i=0; i<5; ++i) {
    for (int j=0; j<5; ++j) {  
      C_k_intermediate[i][j]=0;
      
      for (int l=0; l<5; ++l) {
	for (int m=0; m<5; ++m) {   
	  C_k_intermediate[i][j]+=F_k_minus[i][m]*C_k_minus[m][l]*F_k_minus[j][l];
	}
      }

      C_k_intermediate[i][j]+=Q_k_minus[i][j];
    }
    
  }


  // Diagonal elements should be positive
  double covlim = 1.e-8;

  for(int i=0; i<5; ++i) {
    if(C_k_intermediate[i][i]<covlim) {
      MSG("AlgFitTrackCam",Msg::kVerbose) << "Negative diagonal element in C_k_intermediate" << endl;
      C_k_intermediate[i][i]=covlim;
    }
  }


  // Display
  if(debug) {
    cout << "C_k_intermediate" << endl;
    for(int i=0; i<5; ++i) {
      for(int j=0; j<5; ++j) {
 	cout << C_k_intermediate[i][j] << " ";
      }
      cout << endl;
    }
  }
  
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::CalcKalmanGain(const int NewPlane)
{
  // K_k = C_k_intermediate * H_k^T * ( V_k + H_k * C_k_intermediate * H_k^T )^-1
  MSG("AlgFitTrackCam",Msg::kDebug) << "CalcKalmanGain" << endl;

  double Denominator=0;

  // H_k has only one non-zero element, so we can reduce matrix multiplication required
  if(TrkStripData[NewPlane][0].PlaneView==2) {Denominator=C_k_intermediate[0][0];}
  else {Denominator=C_k_intermediate[1][1];}
  

  // Add uncertainty in measurement
  Denominator+=TrkStripData[NewPlane][0].TPosError;

  MSG("AlgFitTrackCam",Msg::kVerbose) << "V_k " << TrkStripData[NewPlane][0].TPosError 
				      << ", Kalman Gain Denominator " << Denominator << endl;

  if (Denominator!=0.) {
    for (int i=0; i<5; ++i) {
      K_k[i]=0;
      
      for (int m=0; m<5; ++m) {K_k[i]+=(C_k_intermediate[i][m])*(H_k[m]);}
      
      K_k[i]=K_k[i]/Denominator;
    }

    MSG("AlgFitTrackCam",Msg::kVerbose) << "Kalman Gain: " 
					<< K_k[0] << " " << K_k[1] << " " << K_k[2] << " "
					<< K_k[3] << " " << K_k[4] << endl;
  }
  else MSG("AlgFitTrackCam",Msg::kDebug) << "V_k + (H_k * C_k_intermediate * H_k_transpose) is zero!" << endl;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::UpdateStateVector(const int Plane, const int NewPlane, const bool GoForward)
{  
  // x_k = (F_k_minus * x_k_minus) + K_k(m_k - (H_k * F_k_minus * x_k_minus) )
  MSG("AlgFitTrackCam",Msg::kDebug) << "UpdateStateVector" << endl;


  double HFx=0;
  double m_k=0;
  double MeasurementFactor=0;
  int nswimfail=0;


  // Calculate F_k_minus * x_k_minus, using the Swimmer
  // Also get an accurate measure of dS and Range from the Swimmer
  ////////////////////////
  double StateVector[5];
  double Prediction[5];
  bool GetPrediction=false;

  for(int i=0; i<5; ++i) {StateVector[i]=x_k_minus[i];}

  // Prediction could be used without GeoSwimmer calculation. Prediction is initialized with linear extrapolation.
  for(int i=0; i<5; i++) {
    double delz = (SlcStripData[NewPlane][0].csh->GetZPos()-SlcStripData[Plane][0].csh->GetZPos());
    Prediction[0]=StateVector[0] + StateVector[2]*delz;
    Prediction[1]=StateVector[1] + StateVector[3]*delz;
    Prediction[2]=StateVector[2];
    Prediction[3]=StateVector[3];
    Prediction[4]=StateVector[4];
  }

  // Do the swim
  ////////////
  while(GetPrediction==false && nswimfail<=10) {
    MSG("AlgFitTrackCam",Msg::kVerbose) << " state " << StateVector[0] << " " 
					<< StateVector[1] << " " << StateVector[2] << " " 
					<< StateVector[3] << " " << StateVector[4] << endl;   
    
    GetPrediction=Swim(StateVector, Prediction, Plane, NewPlane, GoForward);
    
    MSG("AlgFitTrackCam",Msg::kVerbose) << " predict state " << Prediction[0] << " " 
					<< Prediction[1] << " " << Prediction[2] << " " 
					<< Prediction[3] << " " << Prediction[4] << endl;

    if(GetPrediction==false) {
      StateVector[4]*=0.5; 
      nswimfail++; TotalNSwimFail++;
      MSG("AlgFitTrackCam",Msg::kVerbose) << "UpdateStateVector, Prediction failed - Double momentum and swim again" << endl;
    }
  }

  if(nswimfail>10) {  // Swim shouldn't fail, as it succeeded to get the propagator
    MSG("AlgFitTrackCam",Msg::kDebug) << "UpdateStateVector, nswimfail>10, fail track" << endl;
    PassTrack=false;
  }
  ////////////

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


  MSG("AlgFitTrackCam",Msg::kDebug) << "UpdateStateVector, Check predicted state " << endl;
  CheckValues(Prediction, NewPlane);


  // Calculate H_k * F_k_minus * x_k_minus
  ////////////
  if(TrkStripData[NewPlane][0].PlaneView==2) {HFx=Prediction[0];}
  if(TrkStripData[NewPlane][0].PlaneView==3) {HFx=Prediction[1];}

  MSG("AlgFitTrackCam",Msg::kVerbose) << "HFx " << HFx << endl;
  ////////////

  
  // Read in measurement
  ////////////
  m_k=TrkStripData[NewPlane][0].TPos;
  MSG("AlgFitTrackCam",Msg::kVerbose) << "m_k " << TrkStripData[NewPlane][0].TPos << endl;
  
  MeasurementFactor=(m_k-HFx);
  ////////////

  
  // Calculate x_k
  ////////////
  for (int i=0; i<5; ++i) {
    x_k[i]=0.;
    x_k[i]+=Prediction[i]+(K_k[i]*MeasurementFactor);
  }
  ////////////


  MSG("AlgFitTrackCam",Msg::kDebug) << "UpdateStateVector, Check filtered state " << endl;
  CheckValues(x_k, NewPlane);


  // Care with multiple range corrections - do not want to flip sign
  // (multiple corrections mean sign changes can occur even though absolute value stays same)
  ////////////
  // JAM up to 40 from 4

  double Maxqp = 4.;
  if(fabs(x_k_minus[4])==Maxqp && 
     ( (GoForward==true && ZIncreasesWithTime==true) 
       || (GoForward==false && ZIncreasesWithTime==false) ) ) 
    {
      if(!LastIteration) x_k[4] = (x_k_minus[4]>0 ? Maxqp : -Maxqp);
      //      cout << " resetting in UpdateStateVector " <<  endl;
    }
  ////////////

  //if on last plane in forward swim, disregard sign flip
  if(x_k_minus[4]!=0){
    if ( x_k[4]/x_k_minus[4]<0 && 
	( (GoForward==true && ZIncreasesWithTime==true && NewPlane >= EndofRangePlane ) 
	  || (GoForward==false && ZIncreasesWithTime==false && NewPlane <= EndofRangePlane ) )) 
      {
	x_k[4] = -x_k[4];
      }
  }
  
  // Display
  MSG("AlgFitTrackCam",Msg::kVerbose) << "Filtered State vector: "
				      << x_k[0] << " " << x_k[1] << " " << x_k[2] << " "
				      << x_k[3] << " " << x_k[4] << endl;
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::UpdateCovMatrix()
{
  // C_k = (Identity - (K_k * H_k) ) * C_k_intermediate
  MSG("AlgFitTrackCam",Msg::kDebug) << "UpdateCovMatrix" << endl;

  for (int i=0; i<5; ++i) {
    for (int j=0; j<5; ++j) {  
      C_k[i][j]=0;
      for (int m=0; m<5; ++m) {   
	C_k[i][j]+=(Identity[i][m] - K_k[i]*H_k[m]) * C_k_intermediate[m][j];
      }
    }
  }


  // Diagonal elements should be positive
  double covlim = 1.e-8;

  for(int i=0; i<5; ++i) {
    if(C_k[i][i]<covlim) {
      MSG("AlgFitTrackCam",Msg::kVerbose) << "Negative diagonal element in C_k" << endl;
      C_k[i][i]=covlim;
    }
  }


  // Display
  if(debug) {
    cout << "Filtered Covariance matrix" << endl;
    for(int i=0; i<5; ++i) {
      for(int j=0; j<5; ++j) {
 	cout << C_k[i][j] << " ";
      }
      cout << endl;  
    }
  }

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::MoveArrays()
{
  // Move k to k-1 ready to consider next hit
  MSG("AlgFitTrackCam",Msg::kDebug) << "MoveArrays" << endl;

  for (int i=0; i<5; ++i) {
    for (int j=0; j<5; ++j) { 
      C_k_minus[i][j]=C_k[i][j];
    }
  }
  
  for (int l=0; l<5; ++l) {
    x_k_minus[l]=x_k[l];
  }
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::CheckValues(double* Input, const int NewPlane)
{
  // Make range and gradient corrections
  // Possible source of offset in q/p resolutions

  // Range check
    
  double Maxqp=4.; double Maxqpfrac=1.2;
  double Range=track->GetRange(NewPlane);

  //JAM signal end of range found
  if(fabs(Input[4])>10.0) EndofRange=true; 

   if(Range>0. && (Maxqpfrac*500/Range)<Maxqp) {Maxqp=(Maxqpfrac*500/Range);}
  MSG("AlgFitTrackCam",Msg::kVerbose) << " Range " << Range << " Maxqp " << Maxqp << endl;



  if(LastIteration) Maxqp=40;
  if(fabs(Input[4])>Maxqp){
    //    cout << " CheckValues: Range check correction " << Input[4] << " " << Maxqp << endl;
    MSG("AlgFitTrackCam",Msg::kVerbose) << "CheckValues, Range check correction" << endl;
    Input[4]=(Input[4]>0 ? Maxqp : -Maxqp);
  }
  
  
  // Gradient check
  double Maxgradient=25.;

  if(fabs(Input[2])>Maxgradient) {
    MSG("AlgFitTrackCam",Msg::kVerbose) << "CheckValues, Gradient correction, U" << endl;
    Input[2]=(Input[2]>0 ? Maxgradient : -Maxgradient);
  }

  if(fabs(Input[3])>Maxgradient) {
    MSG("AlgFitTrackCam",Msg::kVerbose) << "CheckValues, Gradient correction, V" << endl;
    Input[3]=(Input[3]>0 ? Maxgradient : -Maxgradient);
  }
  

  // Check u and v values are not rubbish
  if(fabs(Input[0])<5000. && fabs(Input[1])<5000.) {PassTrack=true;}
  else {
    PassTrack=false;
  }
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::StoreFilteredData(const int NewPlane)
{
  // Store the data required for matching Kalman output data to strips
  MSG("AlgFitTrackCam",Msg::kDebug) << "StoreFilteredData" << endl;

  FiltDataStruct temp;
  
  temp.x_k0=x_k[0]; temp.x_k1=x_k[1];
  temp.x_k2=x_k[2]; temp.x_k3=x_k[3];
  temp.x_k4=x_k[4];

  FilteredData[NewPlane].push_back(temp);
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::SetTrackProperties(CandFitTrackCamHandle &cth)
{ 

  if(nbfield>0) bave /=nbfield;
  
  // Carry out the assignment of variables to the new fitted track 
  MSG("AlgFitTrackCam",Msg::kDebug) << "SetTrackProperties" << endl;
  
  
  // Momentum, charge and error on q/p
  ///////////////////////////////////
  if(x_k[4]!=0.) {cth.SetMomentumCurve(fabs(1./x_k[4]));}
  cth.SetRangeBiasedQP(x_k4_biased);
  if(x_k[4]>0.) {cth.SetEMCharge(1.);}
  else if(x_k[4]<0.) {cth.SetEMCharge(-1.);}
  cth.SetVtxQPError(pow(VtxCov[4],0.5));

  //  cout << " biased " << x_k4_biased << " " << eqp_biased << " " << x_k[4] << " " <<  pow(VtxCov[4],0.5) << endl;
  ///////////////////////////////////
  
  
  // Positions and angles  
  ///////////////////////////////////
  int VtxPlane;
  int EndPlane;
  double dsdz;
  
  if(ZIncreasesWithTime==true) {VtxPlane=MinPlane; EndPlane=MaxPlane;}
  else {VtxPlane=MaxPlane; EndPlane=MinPlane;}
  
  // Vtx and end coordinates  
  cth.SetVtxU(FilteredData[VtxPlane][0].x_k0);
  cth.SetVtxV(FilteredData[VtxPlane][0].x_k1);
  cth.SetVtxZ(SlcStripData[VtxPlane][0].csh->GetZPos());
  cth.SetVtxPlane(VtxPlane);
  
  cth.SetEndU(FilteredData[EndPlane][0].x_k0);
  cth.SetEndV(FilteredData[EndPlane][0].x_k1);

  cth.SetEndZ(SlcStripData[EndPlane][0].csh->GetZPos());
  cth.SetEndPlane(EndPlane);
  
  
  // Vtx and end direction cosines
  dsdz=pow(1.+pow(FilteredData[VtxPlane][0].x_k2,2)+pow(FilteredData[VtxPlane][0].x_k3,2),0.5);
  if(ZIncreasesWithTime==false) {dsdz=-dsdz;}
  
  cth.SetVtxDirCosU(FilteredData[VtxPlane][0].x_k2/dsdz);
  cth.SetVtxDirCosV(FilteredData[VtxPlane][0].x_k3/dsdz);
  cth.SetVtxDirCosZ(1./dsdz);
  
  cth.SetDirCosU(FilteredData[VtxPlane][0].x_k2/dsdz);
  cth.SetDirCosV(FilteredData[VtxPlane][0].x_k3/dsdz);
  cth.SetDirCosZ(1./dsdz);
    
  dsdz=pow(1.+pow(FilteredData[EndPlane][0].x_k2,2)+pow(FilteredData[EndPlane][0].x_k3,2),0.5);
  if(ZIncreasesWithTime==false) {dsdz=-dsdz;}
  
  cth.SetEndDirCosU(FilteredData[EndPlane][0].x_k2/dsdz);
  cth.SetEndDirCosV(FilteredData[EndPlane][0].x_k3/dsdz);
  cth.SetEndDirCosZ(1./dsdz);
  
  // End q/p value
  cth.SetEndQP(FilteredData[EndPlane][0].x_k4);
    
  // Errors on vtx positions and angles  
  cth.SetVtxUError(pow(VtxCov[0],0.5));
  cth.SetVtxVError(pow(VtxCov[1],0.5));
  cth.SetVtxdUError(pow(VtxCov[2],0.5));
  cth.SetVtxdVError(pow(VtxCov[3],0.5)); 
  
  // Errors on end positions, angles and q/p
  cth.SetEndUError(pow(EndCov[0],0.5));
  cth.SetEndVError(pow(EndCov[1],0.5));
  cth.SetEnddUError(pow(EndCov[2],0.5));
  cth.SetEnddVError(pow(EndCov[3],0.5)); 
  cth.SetEndQPError(pow(EndCov[4],0.5)); 

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

  cth.SetBave(bave);

  // More variables to be set, in order to ensure compatibility
  ////////////////////////////////////
  cth.SetNTrackStrip(cth.GetNDaughters());
  cth.SetNTrackDigit(cth.GetNDigit());
  
  cth.SetNIterate(NIter);
  cth.SetNSwimFail(TotalNSwimFail);
  
  
  // Obtain "fitting data" for the final track strips
  for (int i=0; i<490; ++i) {TrkStripData[i].clear();} 
  GetFitData(MinPlane,MaxPlane);
  
  
  // Set tpos error and Calculate chi2, NDOF
  double Chi2=0; double Chi2Contrib=0; int NDOF=0; double FilteredTPos=0;
  
  for(int i=MinPlane; i<=MaxPlane; ++i) {
    if(TrkStripData[i].size()>0) {
      
      if(TrkStripData[i][0].TPosError>0.) {
	cth.SetTrackPointError(i,pow(TrkStripData[i][0].TPosError,0.5));
	
	if(TrkStripData[i][0].PlaneView==2) {FilteredTPos=FilteredData[i][0].x_k0;}
	else {FilteredTPos=FilteredData[i][0].x_k1;}
	
	Chi2Contrib = pow((TrkStripData[i][0].TPos-FilteredTPos),2) / TrkStripData[i][0].TPosError;
	cth.SetPlaneChi2(i,Chi2Contrib);	
	
	Chi2+=Chi2Contrib;
	
	NDOF++;
      }
    }
  }
  cth.SetChi2(Chi2);
  cth.SetNDOF(NDOF-5); // Number of constraints set to 5
  
  
  // Assign U, V and q/p values
  for(int i=MinPlane; i<=MaxPlane; ++i) {
    if(FilteredData[i].size()>0) {
      cth.SetU(i,FilteredData[i][0].x_k0);
      cth.SetV(i,FilteredData[i][0].x_k1);
      cth.SetPlaneQP(i,FilteredData[i][0].x_k4);
    }
  }
  
  
  // Set Trace and TraceZ
  CalculateTrace(cth);
  
  
  // Fill time and range maps
  SetT(&cth);
  SetRangeAnddS(cth);
  
  
  // Set momentum to our best estimate (range or curvature)
  cth.SetMomentum(cth.GetMomentumCurve());
  if(cth.IsContained()){
    cth.SetMomentum(cth.GetMomentumRange());
  }
  
  
  // Identify track strips inside in vertex shower
  
  TIter FitTrackStripItr = cth.GetDaughterIterator();
  while(CandStripHandle* FitTrackStrip = dynamic_cast<CandStripHandle*>(FitTrackStripItr()))
    {
      if(ShowerEntryPlane!=-99) {
	if( (ZIncreasesWithTime==true && FitTrackStrip->GetPlane()<=ShowerEntryPlane)
	    || (ZIncreasesWithTime==false && FitTrackStrip->GetPlane()>=ShowerEntryPlane) ) {
	  cth.SetInShower(FitTrackStrip,2);
	}
	else {cth.SetInShower(FitTrackStrip,0);}
      }
      else {cth.SetInShower(FitTrackStrip,0);}
    }
  
  
  // Set all time related variables
  TimingFit(cth);
  
  
  // Calibrate, to get track pulse height in MIPs, GeV, etc
  Calibrator& cal = Calibrator::Instance();
  cal.ReInitialise(*vldc);
  Calibrate(&cth);


}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::TimingFit(CandFitTrackCamHandle &cth)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "TimingFit" << endl;

  // Initialisations
  /////////////////////////////
  double s; double t; double q; double w; int n=0; 
  double Uncertainty=1.0; double MinUncertainty=1.0; 
  double MinCT=-3000.;
  
  // Time of first strip in track
  StripListTime=9.e30;

  // Create an offset such that dS=0 at the MinPlane
  double dSOffset=0.; double Sign=-1.; double dS[490];
  if(ZIncreasesWithTime==true) {dSOffset=cth.GetdS(MinPlane); Sign=1.;}

  // Store data needed in arrays. Charge is in PEs.
  double Qp[490];  double Qm[490];
  double CTp[490]; double CTm[490];
  int Skipp[490];  int Skipm[490];
  double C=3.e8;   

  double ErrorParam[3];
  ErrorParam[0]=1.; ErrorParam[1]=0.; ErrorParam[2]=0.;

  // Zero the arrays
  for(int i=0; i<490; ++i) { 
    dS[i]=0.; CTm[i]=0.; CTp[i]=0.; 
    Qp[i]=0.; Qm[i]=0.; 
    Skipp[i]=0; Skipm[i]=0;
  }
  // End of Initialisations
  /////////////////////////////


  // Copy track strips into vector
  /////////////////////////////
  vector<StripStruct> TimeFitStripData[490];

  TIter MyStripItr = cth.GetDaughterIterator();

  while(CandStripHandle* MyStrip = dynamic_cast<CandStripHandle*>(MyStripItr()))
    {    
      int MyPlane = MyStrip->GetPlane();
     
      StripStruct temp;
      temp.csh = MyStrip;

      if( cth.GetdS(MyPlane)>-0.99 ){               // distance must be greater
        TimeFitStripData[MyPlane].push_back(temp);  // than default value of -1
      }
      
    } 
  /////////////////////////////


  // Organise timing for the Far Detector
  /////////////////////////////
  if(vldc->GetDetector()==Detector::kFar) {  

    // Parameters for time fit residual vs PEs
    // (consistent with AlgTrackCam::WeightsForTimingFits() method)
    // ErrorParam[0]=0.56; ErrorParam[1]=0.50; ErrorParam[2]=-0.34; // (OLD)
    ErrorParam[0]=0.58; ErrorParam[1]=0.69; ErrorParam[2]=-0.33; // (NEW)
    MinUncertainty=ErrorParam[0];

    // Loop over all planes
    for(int i=MinPlane; i<=MaxPlane; ++i) {
        
      if(TimeFitStripData[i].size()>0) {
	dS[i]=Sign*(dSOffset-cth.GetdS(i));
	
	CTp[i]=C*cth.GetT(i,StripEnd::kPositive);
	CTm[i]=C*cth.GetT(i,StripEnd::kNegative);
	
	if(CTp[i]>MinCT && CTp[i]<StripListTime) {StripListTime=CTp[i];}
	if(CTm[i]>MinCT && CTm[i]<StripListTime) {StripListTime=CTm[i];}
	
	for(unsigned int j=0; j<TimeFitStripData[i].size(); ++j) {
	  Qp[i]+=TimeFitStripData[i][j].csh->GetCharge(StripEnd::kPositive);
	  Qm[i]+=TimeFitStripData[i][j].csh->GetCharge(StripEnd::kNegative);
	}
      }
    }

    // Subtract StripList time
    if(StripListTime<8.e30) {
      for(int i=MinPlane; i<=MaxPlane; ++i) {
	if(TimeFitStripData[i].size()>0) {
	  CTp[i]-=StripListTime;
	  CTm[i]-=StripListTime;
	}
      }
    }
    else {StripListTime=0.;}

  } // End Far Detector Section
  /////////////////////////////



  // Organise timing for the Near Detector
  /////////////////////////////
  if(vldc->GetDetector()==Detector::kNear) {
    // Parameters for PE vs time fit residual
    MinUncertainty=1.19;
    ErrorParam[0]=1.13; ErrorParam[1]=0.63; ErrorParam[2]=-0.21;

    double Index=1.77; double DistFromCentre=0.; double CTime=0.;  double FibreDist=0.;    
    double halfLength=0.; double DigChg=0.; double PlaneDigChg;

    StripEnd::StripEnd_t StpEnd = StripEnd::kUnknown;
    CandStripHandle* strip; CandDigitHandle* digit;

    UgliGeomHandle ugh = UgliGeomHandle(*vldc);
    UgliStripHandle striphandle;


    // Loop over all planes
    for(int i=MinPlane; i<=MaxPlane; ++i)
      {
	if(TimeFitStripData[i].size()>0) {dS[i]=Sign*(dSOffset-cth.GetdS(i));}
	PlaneDigChg=0.;

	// Loop over track strips on plane. Only +ve StripEnds should have charge.
	for(unsigned int j=0; j<TimeFitStripData[i].size(); ++j) {
	  strip = TimeFitStripData[i][j].csh;
	  CandDigitHandleItr digitItr(strip->GetDaughterIterator());

	  Qp[i]+=strip->GetCharge(StripEnd::kPositive);


	  // Loop over digits on strip. 
	  /////////////////
	  while( (digit = digitItr()) ) { 
	    StpEnd=digit->GetPlexSEIdAltL().GetEnd();

	    if(StpEnd==StripEnd::kPositive) {
	      FibreDist = 0.; DistFromCentre = 0.; CTime=0.; DigChg=0.;
	      UgliStripHandle stripHandle = ugh.GetStripHandle(strip->GetStripEndId());
	      halfLength = stripHandle.GetHalfLength(); 
	 
	      if(strip->GetPlaneView()==2) {DistFromCentre = cth.GetV(i);}
	      if(strip->GetPlaneView()==3) {DistFromCentre = -cth.GetU(i);}
	      
	      FibreDist = (halfLength + DistFromCentre + stripHandle.ClearFiber(StpEnd) 
			   + stripHandle.WlsPigtail(StpEnd));
	      
	      CTime = C*(strip->GetTime(StpEnd)) - Index*FibreDist;
	      DigChg=digit->GetCharge();
	      
	      PlaneDigChg+=DigChg; CTp[i]+=DigChg*CTime;

	      if(CTime>MinCT && CTime<StripListTime) {StripListTime=CTime;}
	    }
	  }
	  /////////////////
	}
	if(PlaneDigChg>0.) CTp[i]/=PlaneDigChg;
      }


    // Subtract StripList time
    if(StripListTime<8.e30) {
      for(int i=MinPlane; i<=MaxPlane; ++i) {
	if(TimeFitStripData[i].size()>0) {
	  CTp[i]-=StripListTime;
	}
      }
    }
    else {StripListTime=0.;}
    
  } // End near detector section
  /////////////////////////////
    
    

  // Carry out a simple straight line fit for T vs dS
  /////////////////////////////
  // Sqt: sum of charge*time, Sqss: sum of charge*dS*dS, etc.
  double Sqs=0; double Sqt=0; double Sqss=0; double Sqst=0; double Sqtt=0; double Sq=0;
  double TimeSlope=-999; double TimeOffset=-999; double RMS=-999;
  double CTCut = 0.; bool CalculateChi2=true;

  
  // On first iteration, carry out simple fit. Remove outlying points on subsequent passes.
  for(int itr=0; itr<3; ++itr) {
 
    for(int i=MinPlane; i<=MaxPlane; ++i) { 

      // Only consider planes where we found our final strips
      if(TimeFitStripData[i].size()>0) { 
	
	// For positive strip ends	
	s=dS[i]; q=Qp[i]; t=CTp[i];

	if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	else {Uncertainty=MinUncertainty;}
        w = 1.0/pow(Uncertainty,2.0);

	if(q>0. && t>MinCT && Skipp[i]==0) {
	  if(itr==0) {Sq+=w; Sqs+=w*s; Sqt+=w*t; Sqss+=w*s*s; Sqst+=w*s*t; Sqtt+=w*t*t; n++;}
	  
	  else if(fabs(t-TimeOffset-(s*TimeSlope)) > CTCut) {
	    Sqs-=w*s; Sqt-=w*t; Sqss-=w*s*s; Sqst-=w*s*t; Sqtt-=w*t*t; Sq-=w; n--; Skipp[i]=1;
	  }
	}


	// For negative strip ends
	s=dS[i]; q=Qm[i]; t=CTm[i];

	if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	else {Uncertainty=MinUncertainty;}
        w = 1.0/pow(Uncertainty,2.0);

	if(q>0. && t>MinCT && Skipm[i]==0) {
	  if(itr==0) {Sq+=w; Sqs+=w*s; Sqt+=w*t; Sqss+=w*s*s; Sqst+=w*s*t; Sqtt+=w*t*t; n++;}
	  
	  else if(fabs(t-TimeOffset-(s*TimeSlope)) > CTCut) {
	    Sqs-=w*s; Sqt-=w*t; Sqss-=w*s*s; Sqst-=w*s*t; Sqtt-=w*t*t; Sq-=w; n--; Skipm[i]=1;
	  }
	}

      }
    }
    
    // Calculate parameters
    if( (Sq*Sqss-Sqs*Sqs)!=0. && Sq!=0. ) {
      TimeSlope  = (Sq*Sqst-Sqs*Sqt)/(Sq*Sqss-Sqs*Sqs);
      TimeOffset = (Sqt*Sqss-Sqs*Sqst)/(Sq*Sqss-Sqs*Sqs);
      if( ((Sqtt/Sq)-((Sqt/Sq)*(Sqt/Sq)))>0. ) {
	RMS = pow((Sqtt/Sq)-((Sqt/Sq)*(Sqt/Sq)),0.5);
	CTCut = 3.+RMS;
      }
      else {CTCut = 3.5;}
    }
    else  {CalculateChi2=false; break;}
  }
  /////////////////////////////



  // Set timing properties for the fitted track
  /////////////////////////////
  if(n!=0 && CalculateChi2==true) {

    // Offset, slope and vtx/end times
    /////////////////////////////
    cth.SetTimeOffset((TimeOffset+StripListTime)/C);
    cth.SetTimeSlope(TimeSlope/C);

    if(ZIncreasesWithTime==true) {
      cth.SetVtxT((TimeOffset+StripListTime)/C); 
      cth.SetEndT((TimeOffset+StripListTime)/C+(dS[MaxPlane]*TimeSlope/C));
    }
    else {
      cth.SetEndT((TimeOffset+StripListTime)/C); 
      cth.SetVtxT((TimeOffset+StripListTime)/C+(dS[MaxPlane]*TimeSlope/C));
    }
    /////////////////////////////


    // Chi2
    /////////////////////////////
    double Residual2; double Chi2=0; 
 
    for(int i=MinPlane; i<=MaxPlane; ++i) { 
      
      if(TimeFitStripData[i].size()>0) { 
	// For positive strip ends
	s=dS[i]; q=Qp[i]; t=CTp[i];
	if(q>0. && t>MinCT && Skipp[i]==0) {
	  Residual2=pow(t-TimeOffset-(s*TimeSlope),2);

	  // From a rough parameterisation of uncertainty (in CT) vs number of PEs
	  if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	  else {Uncertainty=MinUncertainty;}

	  if(Uncertainty!=0.) {Chi2+=Residual2/pow(Uncertainty,2);}
	}


	// For negative strip ends
	q=Qm[i]; t=CTm[i];
	if(q>0. && t>MinCT && Skipm[i]==0) {
	  Residual2=pow(t-TimeOffset-(s*TimeSlope),2);

	  // From a rough parameterisation of uncertainty (in CT) vs number of PEs
	  if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	  else {Uncertainty=MinUncertainty;}
	  
	  if(Uncertainty!=0.) {Chi2+=Residual2/pow(Uncertainty,2);}
	}
      }
      
    }
    // Set these properties
    cth.SetTimeFitChi2(Chi2);
    cth.SetNTimeFitDigit(n);
  }
  /////////////////////////////



  // Now carry out fits with gradients constrained to be +/- c
  /////////////////////////////
  double CTIntercept[2]; double Csigma[2]; double Ctrunc[2]; 
  double ChiSqPositive=-999; double ChiSqNegative=-999; 
  int ChiSqNdfPos=-999; int ChiSqNdfNeg=-999;  
  double Swtt[2]; double Swt[2]; double Sw[2]; int npts[2]={0,0};

  if(Sq!=0.) {
    CTIntercept[0]=Sqt/Sq; Csigma[0]=-99999.9; Ctrunc[0]=-99999.9;
    CTIntercept[1]=Sqt/Sq; Csigma[1]=-99999.9; Ctrunc[1]=-99999.9;
    
    for(int itr=0; itr<2; ++itr)
      {
	Swtt[0]=0.; Swt[0]=0.; Sw[0]=0.; npts[0]=0;
	Swtt[1]=0.; Swt[1]=0.; Sw[1]=0.; npts[1]=0;
	
	for(int i=0; i<490; ++i)
	  {
	    // For positive strip ends
	    if(Qp[i]>0. && CTp[i]>MinCT) {
	      q=Qp[i]; 

	      if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	      else {Uncertainty=MinUncertainty;}
              w = 1.0/pow(Uncertainty,2.0);

	      t=CTp[i]-dS[i]+CTIntercept[0];
	      if(Ctrunc[0]<0. || fabs(t)<Ctrunc[0]) {Swtt[0]+=w*t*t; Swt[0]+=w*t; Sw[0]+=w; ++npts[0];}
	      
	      t=CTp[i]+dS[i]+CTIntercept[1];
	      if(Ctrunc[1]<0. || fabs(t)<Ctrunc[1]) {Swtt[1]+=w*t*t; Swt[1]+=w*t; Sw[1]+=w; ++npts[1];}
	    }
	    
	    // For negative strip ends
	    if(Qm[i]>0. && CTm[i]>MinCT) {
	      q=Qm[i]; 

	      if (q<25) {Uncertainty = ErrorParam[0]+exp(ErrorParam[1]+ErrorParam[2]*q);}
	      else {Uncertainty=MinUncertainty;}
              w = 1.0/pow(Uncertainty,2.0);

	      t=CTm[i]-dS[i]+CTIntercept[0];
	      if(Ctrunc[0]<0. || fabs(t)<Ctrunc[0]) {Swtt[0]+=w*t*t; Swt[0]+=w*t; Sw[0]+=w; ++npts[0];}
	      
	      t=CTm[i]+dS[i]+CTIntercept[1];
	      if(Ctrunc[1]<0. || fabs(t)<Ctrunc[1]) {Swtt[1]+=w*t*t; Swt[1]+=w*t; Sw[1]+=w; ++npts[1];}
	    }
	  }
	
	// Results for fit with gradient +C
	if(npts[0]>1 && Sw[0]!=0.) {
	  CTIntercept[0]=CTIntercept[0]-Swt[0]/Sw[0]; Csigma[0]=0.; 
	  if((Swtt[0]/Sw[0])-(Swt[0]/Sw[0])*(Swt[0]/Sw[0])>0.) {Csigma[0]=pow((Swtt[0]/Sw[0])-(Swt[0]/Sw[0])*(Swt[0]/Sw[0]),0.5);}
	  ChiSqPositive=Csigma[0]; ChiSqNdfPos=npts[0]-1; 
	  Ctrunc[0]=Csigma[0]+3.;
	}
	
	// Results for fit with gradient -C
	if(npts[1]>1 && Sw[1]!=0.) {
	  CTIntercept[1]=CTIntercept[1]-Swt[1]/Sw[1]; Csigma[1]=0.;
	  if((Swtt[1]/Sw[1])-(Swt[1]/Sw[1])*(Swt[1]/Sw[1])>0.) {Csigma[1]=pow((Swtt[1]/Sw[1])-(Swt[1]/Sw[1])*(Swt[1]/Sw[1]),0.5);}
	  ChiSqNegative=Csigma[1]; ChiSqNdfNeg=npts[1]-1; 
	  Ctrunc[1]=Csigma[1]+3.;
	}
	
      }
  }
  // Set these properties
  cth.SetTimeForwardFitRMS(ChiSqPositive);
  cth.SetTimeForwardFitNDOF(ChiSqNdfPos);
  cth.SetTimeBackwardFitRMS(ChiSqNegative);
  cth.SetTimeBackwardFitNDOF(ChiSqNdfNeg);
  /////////////////////////////
   
}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::SetRangeAnddS(CandFitTrackCamHandle& cth)
{

  // Set range and dS as calculated by the swimmer
  MSG("AlgFitTrackCam",Msg::kDebug) << "SetRangeAnddS from swimmer values " << endl;

  UgliGeomHandle ugh = UgliGeomHandle(*vldc);

  int ZDir; int VtxPlane; int EndPlane; int Increment;
  Detector::Detector_t detector = vldc->GetDetector();
  double dS; double dRange; double dP;


  // Start at the end of the track and calculate the required additions to range
  ///////////////////////////

  // find ending Z position (defined as Z position where muon has 100 MeV of residual energy.  This corresponds to 1/2 inch of Fe.

  // NOTE: Average dP for 1" iron is 95 MeV.
 
  if(ZIncreasesWithTime==true) {ZDir=1; EndPlane=MaxPlane; VtxPlane=MinPlane; Increment=-1;}
  else {ZDir=-1; EndPlane=MinPlane; VtxPlane=MaxPlane; Increment=1;}

  PlexPlaneId plnid(detector,EndPlane,false);
  PlexPlaneId endplnid(detector,EndPlane,false);

  double Zscint = SlcStripData[EndPlane][0].csh->GetZPos();
  double Znextscint = Zscint;

  UgliScintPlnHandle scintpln = ugh.GetScintPlnHandle(plnid); 
  double Zend = Zscint + double(ZDir)*scintpln.GetHalfThickness();

  PlexPlaneId nextscint =  endplnid.GetAdjoinScint(ZDir);
  UgliScintPlnHandle nextscintpln = ugh.GetScintPlnHandle(nextscint);
  if(nextscintpln.IsValid() && nextscint.GetPlaneView()!=PlaneView::kUnknown){
    Znextscint = nextscintpln.GetZ0();
  }
  else{
    nextscint = endplnid;
  }

  plnid = plnid.GetAdjoinSteel(ZDir);
  if(plnid.IsValid()){
    UgliSteelPlnHandle steelpln = ugh.GetSteelPlnHandle(plnid);
    Zend = steelpln.GetZ0() - double(ZDir)*steelpln.GetHalfThickness();
  }

  // add two planes of steel for the ND spectrometer
  if(detector==Detector::kNear && EndPlane>=121) {
    for(int i=0;i<2;i++){
      if(plnid.GetAdjoinSteel(ZDir).IsValid()){
	PlexPlaneId plnid_after = plnid.GetAdjoinSteel(ZDir);
	if(plnid_after.IsValid()) {
	  plnid = plnid_after;
	  UgliSteelPlnHandle steelpln = ugh.GetSteelPlnHandle(plnid);
	  Zend = steelpln.GetZ0() - double(ZDir)*steelpln.GetHalfThickness();
	}
      }
    }
  }

  // Determine whether track stops in coil
  float u_end = FilteredData[EndPlane][0].x_k0;
  float v_end = FilteredData[EndPlane][0].x_k1;
  float du_end = FilteredData[EndPlane][0].x_k2;
  float dv_end = FilteredData[EndPlane][0].x_k3;
  float delz = Znextscint-Zscint;
  float u_extrap = u_end +delz*du_end;
  float v_extrap = v_end +delz*dv_end;
  float x_extrap = 0.707*(u_extrap-v_extrap);
  float y_extrap = 0.707*(u_extrap+v_extrap);

  PlaneCoverage::PlaneCoverage_t kPC = PlaneCoverage::kComplete;
  if(detector==Detector::kNear) kPC=PlaneCoverage::kNearFull;

  bool isInOutline = fPL.IsInside(x_extrap,y_extrap,nextscint.GetPlaneView(),kPC,false);
  bool isInCoil = isInOutline && !fPL.IsInside(x_extrap,y_extrap,nextscint.GetPlaneView(),kPC,true);

  double S = 0; double Range = 0; double Prange = 0.0;
  double StateVector[5]; double Output[5];
  double chargesign = -1;
  bool GoForward = true; bool done=true; bool swimOK=true;

  // if in coil find midpoint and swim towards last hit from there
  if(isInCoil){
    float zCoil = Znextscint;
    float u_extrapC = u_extrap;
    float v_extrapC = v_extrap;
    float x_extrapC = x_extrap;
    float y_extrapC = y_extrap;
    while(isInCoil){
      zCoil -= 1.0*Munits::cm*ZDir;
      float delzC = zCoil - Zscint;
      u_extrapC = u_end +delzC*du_end;
      v_extrapC = v_end +delzC*dv_end;
      x_extrapC = 0.707*(u_extrapC-v_extrapC);
      y_extrapC = 0.707*(u_extrapC+v_extrapC);
      isInCoil = !fPL.IsInside(x_extrapC,y_extrapC,nextscint.GetPlaneView(),kPC,true) && ((zCoil>Zscint && ZDir==1) || (zCoil<Zscint && ZDir==-1)) ;        
    }
    float zMinCoil = zCoil;
    if(zMinCoil<Zscint && ZDir==1)  zMinCoil=Zscint;
    if(zMinCoil>Zscint && ZDir==-1) zMinCoil=Zscint;

    zCoil = Znextscint;
    isInCoil = true;
    while(isInCoil){
      zCoil += 1.0*Munits::cm*ZDir;
      float delzC = zCoil - Zscint;
      u_extrapC = u_end +delzC*du_end;
      v_extrapC = v_end +delzC*dv_end;
      x_extrapC = 0.707*(u_extrapC-v_extrapC);
      y_extrapC = 0.707*(u_extrapC+v_extrapC);
      float zmin; float zmax;
      ugh.GetZExtent(zmin,zmax);
      isInCoil = !fPL.IsInside(x_extrapC,y_extrapC,nextscint.GetPlaneView(),kPC,true) && ((zCoil<zmax && ZDir==1) || (zCoil>zmin && ZDir==-1));        
    }
    float zMaxCoil = zCoil;
    float zmin; float zmax;
    ugh.GetZExtent(zmin,zmax);
    if(zMaxCoil>zmax && ZDir==1)  zMaxCoil=zmax;
    if(zMaxCoil<zmin && ZDir==-1) zMaxCoil=zmin;

    // now swim from mid-coil back to endplane
    float zMidCoil = 0.5*(zMinCoil + zMaxCoil);
    float delzC  = zMidCoil -Zscint;
    u_extrapC = u_end +delzC*du_end;
    v_extrapC = v_end +delzC*dv_end;
    x_extrapC = 0.707*(u_extrapC-v_extrapC);
    y_extrapC = 0.707*(u_extrapC+v_extrapC);
    
    StateVector[0] = u_extrapC; Output[0]=StateVector[0];
    StateVector[1] = v_extrapC; Output[1]=StateVector[1];
    StateVector[2] = FilteredData[EndPlane][0].x_k2; Output[2]=StateVector[2];
    StateVector[3] = FilteredData[EndPlane][0].x_k3; Output[3]=StateVector[3];
    chargesign = -1;
    if(FilteredData[EndPlane][0].x_k4!=0.) {chargesign =  FilteredData[EndPlane][0].x_k4/fabs(FilteredData[EndPlane][0].x_k4);}
    
    GoForward = !ZIncreasesWithTime;
    StateVector[4] = 10.*chargesign; Output[4]=StateVector[4]; 

    double dsdz = pow((1. + pow(StateVector[2],2) + pow(StateVector[3],2)),0.5);    
    // set fallback to nominal energy loss in case coil swim fails
    Prange = 0.095*dsdz;
    if(detector==Detector::kNear && EndPlane>121) Prange = 0.2*dsdz;  
    Prange += 0.5*dsdz*0.1*fabs(zMaxCoil-zMinCoil)*2.357*1.97;

    swimOK = Swim(StateVector, Output, zMidCoil, EndPlane , GoForward, &dS, &dRange, &dP);
   
    if(swimOK ){
      S = dS; Range = dRange; Prange = fabs(dP);
      cth.SetdS(EndPlane,S); 
      cth.SetRange(EndPlane,Range);
    }
    if(!swimOK) {Output[4] = chargesign/Prange;}
  
  }

  else{
    // normal case - track does not end in coil
    if((Zend<Zscint && ZDir==1) || (Zend>Zscint && ZDir==-1)) {
      MSG("AlgFitTrackCam",Msg::kWarning) << " Zend on wrong side of last scint! " << endl;
      Zend=Zscint;
    }
    
    // now swim to Zend
    StateVector[0]=FilteredData[EndPlane][0].x_k0; Output[0]=StateVector[0];
    StateVector[1]=FilteredData[EndPlane][0].x_k1; Output[1]=StateVector[1];
    StateVector[2]=FilteredData[EndPlane][0].x_k2; Output[2]=StateVector[2];
    StateVector[3]=FilteredData[EndPlane][0].x_k3; Output[3]=StateVector[3];
    StateVector[4]=FilteredData[EndPlane][0].x_k4; Output[4]=StateVector[4];
    chargesign = -1;
    if(StateVector[4]!=0.) {chargesign = StateVector[4]/fabs(StateVector[4]);}
    
    GoForward = ZIncreasesWithTime;
    done = Swim(StateVector, Output, EndPlane, Zend, GoForward,  &dS, &dRange, &dP);
    
    GoForward = !ZIncreasesWithTime;
    double dsdz = pow((1. + pow(StateVector[2],2) + pow(StateVector[3],2)),0.5); 
    S = 0; Range = 10.0*dsdz; Prange = 0.095*dsdz;
    swimOK = false;
    if(done){
      for(int j=0;j<5;j++) {StateVector[j]=Output[j];}
      
      // now swim from Zend to EndPlane
      StateVector[4] = chargesign * 10.52;  // start @ P = 100 MeV (Eloss in 1/2 " Iron)
      swimOK = Swim(StateVector, Output, Zend, EndPlane , GoForward, &dS, &dRange, &dP);
      if(swimOK){
	S += dS; Range += dRange; Prange += fabs(dP);
	cth.SetdS(EndPlane,S); 
	cth.SetRange(EndPlane,Range);
      }
    }
    if(!swimOK) {Output[4] = chargesign/Prange;}
  }
  
  int thisplane = EndPlane;
  // now swim back to vertex
  bool firstplane=true;
 for(int i=EndPlane+Increment; Increment*i<=Increment*VtxPlane; i+=Increment) {
    if(FilteredData[i].size()>0) {
      double delU = FilteredData[i][0].x_k0 - StateVector[0] ;
      double delV = FilteredData[i][0].x_k1 - StateVector[1] ;
      double dSperPlane=0.;
      if(thisplane!=i) {dSperPlane = pow(delU*delU + delV*delV,0.5)/double(abs(thisplane-i));}

      // only update state vector if change in U/V is reasonable.
      if(dSperPlane < 1.5*Munits::m) {
	StateVector[0]=FilteredData[i][0].x_k0;
	StateVector[1]=FilteredData[i][0].x_k1;
	StateVector[2]=FilteredData[i][0].x_k2;
	StateVector[3]=FilteredData[i][0].x_k3;

	chargesign=-1;
	if(FilteredData[i][0].x_k4!=0.) {chargesign = FilteredData[i][0].x_k4/fabs(FilteredData[i][0].x_k4);}
      }

      StateVector[4] = chargesign * fabs(Output[4]);
      done = Swim(StateVector, Output, thisplane, i , GoForward, &dS, &dRange, &dP);
      if(done){
	S+=dS; Range+=dRange; Prange+=fabs(dP);
	cth.SetdS(i,S); cth.SetRange(i,Range);
	firstplane=false;
      }
      else {
	MSG("AlgFitTrackCam",Msg::kDebug) << " swim fail " << endl;
      }
      thisplane=i;
    }
  }

  PlexPlaneId vtxplnid(detector,VtxPlane,false);
  PlexPlaneId plnid_before = vtxplnid.GetAdjoinSteel(-ZDir);
  
  if(plnid_before.IsValid()) {
    plnid = plnid_before;
    UgliSteelPlnHandle steelpln = ugh.GetSteelPlnHandle(plnid);
    double Zstart = steelpln.GetZ0();
    StateVector[0]=FilteredData[VtxPlane][0].x_k0;
    StateVector[1]=FilteredData[VtxPlane][0].x_k1;
    StateVector[2]=FilteredData[VtxPlane][0].x_k2;
    StateVector[3]=FilteredData[VtxPlane][0].x_k3;
    StateVector[4]=Output[4];
    Swim(StateVector, Output, VtxPlane, Zstart, GoForward, &dS,&dRange,&dP);
    S+=dS; Range+=dRange; Prange+=fabs(dP);

    cth.SetRange(VtxPlane,Range);
    cth.SetdS(VtxPlane,S);
  }

  // if Prange < 21 GeV, use this value.  Otherwise, use finder track energy, which is somewhat less prone to gross errors.
 
  // apply fudge factor for nominal steel thickness in ND geometry.
  //********* !!!!!!!!!!!!! ***********
  float ecorr = 1.0;
 if(vldc->GetDetector()==Detector::kNear && vldc->GetSimFlag()==SimFlag::kData) ecorr = 1.008; 
  
  cth.SetMomentumRange(Prange*ecorr);
  CandTrackHandle* findertrack = cth.GetFinderTrack();
  if(((detector==Detector::kFar && Prange>21.) || (detector==Detector::kNear && Prange>12.)) && findertrack) {cth.SetMomentumRange(findertrack->GetMomentum());}

}
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::SetPropertiesFromFinderTrack(CandFitTrackCamHandle &cth)
{
  // This method is only called if the fit fails. We set properties from finder track.
  // This clearly does not include fitted properties such as q/p or QPVtxError.
 MSG("AlgFitTrackCam",Msg::kDebug) << "SetPropertiesFromFinderTrack" << endl;
  cth.SetDirCosU(track->GetDirCosU());
  cth.SetDirCosV(track->GetDirCosV());
  cth.SetDirCosZ(track->GetDirCosZ());
  cth.SetVtxU(track->GetVtxU());
  cth.SetVtxV(track->GetVtxV());
  cth.SetVtxZ(track->GetVtxZ());
  cth.SetVtxT(track->GetVtxT());
  cth.SetVtxPlane(track->GetVtxPlane());

  cth.SetEndDirCosU(track->GetEndDirCosU());
  cth.SetEndDirCosV(track->GetEndDirCosV());
  cth.SetEndDirCosZ(track->GetEndDirCosZ());
  cth.SetEndU(track->GetEndU());
  cth.SetEndV(track->GetEndV());
  cth.SetEndZ(track->GetEndZ());
  cth.SetEndT(track->GetEndT());
  cth.SetEndPlane(track->GetEndPlane());

  cth.SetMomentumRange(track->GetMomentum());
  cth.SetMomentum(track->GetMomentum());

  cth.SetTimeSlope(track->GetTimeSlope());
  cth.SetTimeOffset(track->GetTimeOffset());
  cth.SetTimeFitChi2(track->GetTimeFitChi2());
  cth.SetNTimeFitDigit(track->GetNTimeFitDigit());
  cth.SetTimeForwardFitRMS(track->GetTimeForwardFitRMS());
  cth.SetTimeForwardFitNDOF(track->GetTimeForwardFitNDOF());
  cth.SetTimeBackwardFitRMS(track->GetTimeBackwardFitRMS());
  cth.SetTimeBackwardFitNDOF(track->GetTimeBackwardFitNDOF());


  // Set quantities required at each plane in finder track
  int direction=1;
  if(ZIncreasesWithTime==false) {direction=-1;}

  for(int i=cth.GetVtxPlane(); i*direction<=cth.GetEndPlane()*direction; i+=direction){
    if(track->IsTPosValid(i)) {
      cth.SetTrackPointError(i,track->GetTrackPointError(i));
      cth.SetdS(i,track->GetdS(i));
      cth.SetRange(i,track->GetRange(i));
      cth.SetU(i,track->GetU(i));
      cth.SetV(i,track->GetV(i));
    }
  }

  CalculateTrace(cth);  
  SetT(&cth);

  Calibrator& cal = Calibrator::Instance();
  cal.ReInitialise(*vldc);
  Calibrate(&cth);


}
////////////////////////////////////////////////////////////////////////




////////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::GenerateNDSpectStrips(const CandSliceHandle *slice,CandContext &cx)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "GenerateNDSpectStrips" << endl;

  bool AlreadyAssigned;


  CandContext cxx(this,cx.GetMom());

  // Get singleton instance of AlgFactory
  AlgFactory &af = AlgFactory::GetInstance();
  AlgHandle stripah = af.GetAlgHandle("AlgStripSR","default");

  // Store CandStripHandles and make the strips accessible by plane number
  TIter SlcStripItr = slice->GetDaughterIterator();
  StripStruct temp;
  
  // Store all strips in slice
  while(CandStripHandle* SlcStrip = dynamic_cast<CandStripHandle*>(SlcStripItr()))  {
    if (SlcStrip->GetPlane()>120 && (SlcStrip->GetPlaneView()==PlaneView::kU || SlcStrip->GetPlaneView()==PlaneView::kV) ) {
      AlreadyAssigned=false;
      
      TIter digitItr(SlcStrip->GetDaughterIterator());
      CandDigitHandle* dig = dynamic_cast<CandDigitHandle*>(digitItr());	      
      const PlexSEIdAltL& altl = dig->GetPlexSEIdAltL();
      int siz = altl.size();
      
      for (int ind = 0; ind<siz; ++ind) {
	// Only want to make the single copy of an assigned strip
	if(AlreadyAssigned) {break;}
	
	TObjArray diglist;  
	TIter newstripdauItr(SlcStrip->GetDaughterIterator());
	
	while(CandDigitHandle* olddig = dynamic_cast<CandDigitHandle*>(newstripdauItr())) {
	  CandDigitHandle* newdig = olddig->DupHandle();
	  PlexSEIdAltL& newaltl = newdig->GetPlexSEIdAltLWritable(); 
	  
	  // Don't make any strips which have already been assigned to a 'better' track
	  if(NumFinderStrips<=newaltl.GetBestWeight()) {AlreadyAssigned=true; delete newdig;}
	  else {
	    for (int newind = 0; newind < siz; ++newind) {
	      if(newind==ind){newaltl[newind].SetWeight((float)NumFinderStrips);}
	      else{newaltl[newind].SetWeight((float)0.);}
	    }
	    
	    newdig->SetCandRecord(olddig->GetCandRecord());
	    diglist.Add(newdig);   // diglist does not own newdig. These must be explicitly deleted 
	  }
	
	}
	// Only make a new strip if we have any digits	
	if(1+diglist.GetLast()>0) {
	  cxx.SetDataIn(&diglist);
	  CandStripHandle newstrip = CandStrip::MakeCandidate(stripah,cxx);
	  newstrip.SetCandRecord(slice->GetCandRecord());
	  CandStripHandle* daustrip = new CandStripHandle(newstrip);
	  
	  for (int i=0; i<=diglist.GetLast(); ++i) {
	    CandDigitHandle* deldig =  dynamic_cast<CandDigitHandle*>(diglist.At(i));
	    delete deldig;
	  }
	  temp.csh=daustrip;      
	  SlcStripData[SlcStrip->GetPlane()].push_back(temp);
	}
      }
    }
  }
  SlcStripItr.Reset();
}

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



//////////////////////////////////////////////////////////////////////
void AlgFitTrackCam::SpectrometerSwim(CandFitTrackCamHandle &cth)
{
  MSG("AlgFitTrackCam",Msg::kDebug) << "SpectrometerSwim, improved track finding in spectrometer" << endl;

  // Initialisations
  // Sort out the lists for the ND spectrometer
  bool AddedStrip = false;
  int Plane; int NewPlane;
  double StateVector[5]; double NState[5]; double temp_x_k[5];
  bool GoForward; bool SpectSwim;
  int ActivePlanesSinceLastHit=0;
  int PlaneView; int Increment; int Counter;
  double LastHitTimes[2]={MeanTrackTime,MeanTrackTime};
  double TimeWindow=40.e-9;

  double StripDistance; double MinDistanceToStrip;
  double SwimError2;
  double StripWidth = 4.108e-2;
  double PlanePitch = 0.0594;

  bool TrackInActiveRegion;
  GoForward=true; Plane=MaxPlane; Increment=1;

  // Linear extrapolation from end of track to start of spectrometer.
  // Count number of active planes  
  while(ActivePlanesSinceLastHit<6 && Plane<121) {
    Plane += Increment;
    double u = x_k_minus[0] + x_k_minus[2]*(Plane-MaxPlane)*PlanePitch;
    double v = x_k_minus[1] + x_k_minus[3]*(Plane-MaxPlane)*PlanePitch;

    // 15 Feb 2008 - mitigate symptoms of a problem elsewhere - huge u and v can cause crash.
    if(fabs(u) > 5000 || fabs(v) > 5000){
      MSG("AlgFitTrackCam", Msg::kError) << "SpectrometerSwim - unexpectedly large u or v (u="
					 << u << " v=" << v << ") bailing out." << endl;
      return;
    }

    if (NDPlaneIsActive(Plane, u, v, 0.3)) ActivePlanesSinceLastHit++;
  }

  // If we are clearly not near spectrometer, return from method
  if(ActivePlanesSinceLastHit>5 || abs(121-MaxPlane)>=40) {return;}

  // Set initial positions for swim
  ActivePlanesSinceLastHit=0;
  Plane = MaxPlane;  NewPlane = Plane+1;

  // Continue until we reach a 8 plane gap (counting only active planes) since prior
  // hit or we reach the end of the detector
  while(ActivePlanesSinceLastHit<8 && abs(NewPlane-Plane)<=70 && NewPlane<=290 && PassTrack==true) {
    
    PlexPlaneId plexPlane(Detector::kNear,NewPlane, 0); 
    if(SlcStripData[NewPlane].size()>0 && plexPlane.IsValid() && plexPlane.GetPlaneCoverage()!=PlaneCoverage::kNoActive) {

      if(SlcStripData[NewPlane][0].csh->GetPlaneView()==2) {PlaneView=0;}
      else {PlaneView=1;}
      
      for(int i=0; i<5; ++i) {StateVector[i]=x_k_minus[i];}

      // For the purposes of spectrometer track hit finding, don't allow track to range out before
      // we have swum all the hit spectrometer planes in this slice
      SpectSwim = Swim(StateVector, NState, Plane, NewPlane, GoForward);

      // If swim has failed and there is a large gap to next hit plane, stop the spectrometer swim.
      if(!SpectSwim && (NewPlane-Plane)>=40) {
	break;}

      // If swim has failed, but there is no large gap, make a momentum correction and swim again.
      if(!SpectSwim && StateVector[4]!=0) {
	Counter=0;
	// Double momentum and attempt to swim again
	while(!SpectSwim && Counter<2) {
	  StateVector[4]*=0.5;	
	  SpectSwim = Swim(StateVector, NState, Plane, NewPlane, GoForward);
	  
	  if(!SpectSwim) {Counter++;}
	}
      }
      
      if(!SpectSwim) {break;}

      for(int i=0; i<5; ++i) {temp_x_k[i]=NState[i];}

      // Calculate error in track extrapolation, based on covariance matrix on-diagonal elements
      SwimError2 = C_k[PlaneView][PlaneView] + (C_k[PlaneView+2][PlaneView+2]*PlanePitch*PlanePitch*(NewPlane-Plane)*(NewPlane-Plane)); 
      MinDistanceToStrip = 3.0 * pow(StripWidth*StripWidth + SwimError2,0.5);
      

      // Find the closest strip (within a distance 'MinDistanceToStrip') and temporarily store CandStripHandle
      CandStripHandle* CurrentStrip = 0;      
      for(unsigned int j=0; j<SlcStripData[NewPlane].size(); ++j) {
	StripDistance = fabs(SlcStripData[NewPlane][j].csh->GetTPos()-temp_x_k[PlaneView]);

	if(StripDistance<MinDistanceToStrip 
	   && fabs(0.5*(LastHitTimes[0]+LastHitTimes[1])-NDStripBegTime(SlcStripData[NewPlane][j].csh,temp_x_k[0],temp_x_k[1]))<TimeWindow) {
	  MinDistanceToStrip=StripDistance;
	  CurrentStrip=SlcStripData[NewPlane][j].csh;
	}
      }

      // If we find a likely track strip, add it to the fit data and call the Kalman
      // update equations before repeating process to find next track strips in the shower
      if(CurrentStrip) {
	LastHitTimes[1]=LastHitTimes[0];
	LastHitTimes[0]=NDStripBegTime(CurrentStrip,temp_x_k[0],temp_x_k[1]);

	StripStruct temp;
	temp.csh = CurrentStrip;
	InitTrkStripData[NewPlane].push_back(temp);
	
	// Convert the strip to data required for Kalman fit
	GetFitData(NewPlane,NewPlane);
	
	// Carry out the Kalman fit
	if (PlaneView==1) {H_k[0]=0; H_k[1]=1; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	else {H_k[0]=1; H_k[1]=0; H_k[2]=0; H_k[3]=0; H_k[4]=0;}
	
	bool CombiPropagatorOk = GetCombiPropagator(Plane,NewPlane,GoForward);

	if(CombiPropagatorOk) {
	  GetNoiseMatrix(Plane,NewPlane);
	  ExtrapCovMatrix();
	  CalcKalmanGain(NewPlane);
	  UpdateStateVector(Plane,NewPlane,GoForward);
	  UpdateCovMatrix();
	  MoveArrays();
	  StoreFilteredData(NewPlane);
	  
	  MaxPlane=NewPlane; Plane=MaxPlane;
	  NewPlane=Plane+Increment;
	  
	  ActivePlanesSinceLastHit=0;
	}

	// Extend finder track, including the ND Spectrometer hits found in the fit
	///////////////////////////////////////////////////

       
	CandTrackHandle * findertrack = cth.GetFinderTrack();
	if(findertrack) {
	  bool SlushyOnEntry = CandHandle::IsSlushyEnabled();
	  CandHandle::SetSlushyEnabled(kTRUE);
	  AddedStrip = true;
	  findertrack->AddDaughterLink(*CurrentStrip);
	  findertrack->SetEndPlane(CurrentStrip->GetPlane());
	  findertrack->SetEndZ(CurrentStrip->GetZPos());
	  findertrack->SetEndU(FilteredData[CurrentStrip->GetPlane()][0].x_k0);
	  findertrack->SetEndV(FilteredData[CurrentStrip->GetPlane()][0].x_k1);
          double dsdz = sqrt(1+pow(FilteredData[CurrentStrip->GetPlane()][0].x_k2,2)+pow(FilteredData[CurrentStrip->GetPlane()][0].x_k3,2));
          if(!ZIncreasesWithTime) dsdz=-dsdz;
	  findertrack->SetEndDirCosU(FilteredData[CurrentStrip->GetPlane()][0].x_k2/dsdz);
	  findertrack->SetEndDirCosV(FilteredData[CurrentStrip->GetPlane()][0].x_k3/dsdz);
          findertrack->SetEndDirCosZ(1/dsdz);
	  if(!SlushyOnEntry) CandHandle::SetSlushyEnabled(kFALSE);
	}
	///////////////////////////////////////////////////
	
      }
      else {
	TrackInActiveRegion=NDPlaneIsActive(NewPlane, temp_x_k[0], temp_x_k[1], 0.3);
	if(plexPlane.IsValid() && plexPlane.GetPlaneCoverage()!=PlaneCoverage::kNoActive && TrackInActiveRegion) 
	  {ActivePlanesSinceLastHit++;}
	NewPlane+=Increment;
      }
    }
    else {
      double u = x_k_minus[0] + x_k_minus[2]*(NewPlane-Plane)*PlanePitch;
      double v = x_k_minus[1] + x_k_minus[3]*(NewPlane-Plane)*PlanePitch;
      
      TrackInActiveRegion=NDPlaneIsActive(NewPlane, u, v, 0.3);	
      
      if(plexPlane.IsValid() && plexPlane.GetPlaneCoverage()!=PlaneCoverage::kNoActive && TrackInActiveRegion) 
	{ActivePlanesSinceLastHit++;}
      NewPlane+=Increment; 
    }
  }

  
  // Sort out range and dS for finder track
  ///////////////////////////////////////////////////
  if(AddedStrip) {
    CandTrackHandle * findertrack = cth.GetFinderTrack();
    if(findertrack) {
      bool SlushyOnEntry = CandHandle::IsSlushyEnabled();
      CandHandle::SetSlushyEnabled(kTRUE);
      
      SetUVZT(findertrack);
      Double_t range = findertrack->GetRange();
      if (range>0.) {
	findertrack->SetMomentum(GetMomFromRange(range));   
      }
      if(!SlushyOnEntry) CandHandle::SetSlushyEnabled(kFALSE);
    }
  }
  ///////////////////////////////////////////////////

}
//////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////
bool AlgFitTrackCam::NDPlaneIsActive(int plane, float u, float v, float projErr)
{ 
  // Method to determine whether this u/v position is active

  PlexPlaneId plexPlane(Detector::kNear,plane, 0); 
  if(!plexPlane.IsValid()) {return false;}
  if(plexPlane.GetPlaneCoverage()==PlaneCoverage::kNoActive) {return false;}
  if(projErr<0.3)projErr=0.3;
  float x = 0.707*(u-v);
  float y = 0.707*(u+v);
  float dist,xedge,yedge;
  fPL.DistanceToEdge(x, y,
		     plexPlane.GetPlaneView(),
		     plexPlane.GetPlaneCoverage(),
		     dist, xedge, yedge);
  bool isInside = fPL.IsInside(x, y,
			       plexPlane.GetPlaneView(),
			       plexPlane.GetPlaneCoverage());
  isInside &= dist>projErr;

  return isInside;
}
//////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////
void AlgFitTrackCam::CleanNDLists(CandFitTrackHandle &cth,CandContext &cx)
{


  // Sort out the lists for the ND spectrometer

  // Delete the strip handles created in GenerateNDSpectStrips.
  // All strip handles not added to a track daughter list are deleted here.
  ///////////////////////////////////////////////////
  for(int iplane=121; iplane<=290; ++iplane){
    for(unsigned int i=0; i<SlcStripData[iplane].size(); ++i){
      CandStripHandle* delstrip = SlcStripData[iplane][i].csh;
      delete delstrip; 
    }
  }
  ///////////////////////////////////////////////////

  bool SlushyOnEntry = CandHandle::IsSlushyEnabled();
  CandHandle::SetSlushyEnabled(kTRUE);

  // Get DigitList and StripList from CandRecord
  ///////////////////////////////////////////////////
  const MomNavigator* mom = cx.GetMom();
  CandRecord* crec = dynamic_cast<CandRecord *> (mom->GetFragment("CandRecord", "PrimaryCandidateRecord"));

  // DupHandle step added by gmieg.  Must delete StripList and DigitList.
  CandStripListHandle* StripListOH = dynamic_cast<CandStripListHandle *>
    (crec->FindCandHandle("CandStripListHandle"));
  CandStripListHandle* StripList = StripListOH->DupHandle();

  CandDigitListHandle* DigitListOH = dynamic_cast<CandDigitListHandle *>
    (crec->FindCandHandle("CandDigitListHandle","canddigitlist"));
  CandDigitListHandle* DigitList = DigitListOH->DupHandle();

  CandSliceHandle* Slice = dynamic_cast<CandSliceHandle*>(cth.GetCandSliceWritable());
  ///////////////////////////////////////////////////

 
  // Compare new fitted track, with DeMuxed spectrometer strips, 
  // to StripList, Slice and DigitList
  ///////////////////////////////////////////////////////////////
  vector<CandStripHandle*> StripsToAdd;
  vector<CandStripHandle*> StripsToRemove;

  vector<CandStripHandle*> SliceStripsToAdd;
  vector<CandStripHandle*> SliceStripsToRemove;

  vector<CandDigitHandle*> DigitsToAdd;
  vector<CandDigitHandle*> DigitsToRemove;


  CandStripHandleItr TrkStripItr(cth.GetDaughterIterator());
  for(CandStripHandle* TrkStrip=TrkStripItr(); TrkStrip; TrkStrip=TrkStripItr()){

    if(TrkStrip->GetPlane()>120 ) {
      CandDigitHandleItr TrkDigitItr(TrkStrip->GetDaughterIterator());
      CandDigitHandle* TrkDigit = dynamic_cast<CandDigitHandle*>(TrkDigitItr());

      // Sort out StripList
      ///////////////////////////////////////////////////
      if(StripList) {
	bool AddedTrkStrip = false;
	CandStripHandleItr stripItr(StripList->GetDaughterIterator());
	
	for(CandStripHandle* strip=stripItr(); strip ; strip=stripItr()) {
	  if(strip->GetPlane()==TrkStrip->GetPlane()){
	    CandDigitHandleItr digitItr(strip->GetDaughterIterator());
	    CandDigitHandle* digit = dynamic_cast<CandDigitHandle*>(digitItr());
	    bool SameCandStrip = false;
	    bool SameHit = false;
	    
	    if(TrkStrip->IsEqual(strip)) {SameCandStrip = true;}
	    
	    if(digit->GetChannelId()==TrkDigit->GetChannelId() &&
      	       strip->GetCharge(CalDigitType::kNone)==TrkStrip->GetCharge(CalDigitType::kNone))
      	      {SameHit=true;}
	    
	    if(!SameCandStrip && SameHit) {
      	      StripsToRemove.push_back(strip);
	      if(!AddedTrkStrip) {StripsToAdd.push_back(TrkStrip);}
	      AddedTrkStrip=true;
	    }
	  }
	}
      }
      ///////////////////////////////////////////////////
      


      // Sort out Slice
      ///////////////////////////////////////////////////
      if(Slice){
  	bool AddedTrkStrip = false;
  	CandStripHandleItr stripItr(Slice->GetDaughterIterator());

  	for(CandStripHandle* strip=stripItr(); strip; strip=stripItr()) {
	  if(strip->GetPlane()==TrkStrip->GetPlane()){
  	    CandDigitHandleItr digitItr(strip->GetDaughterIterator());
  	    CandDigitHandle* digit = dynamic_cast<CandDigitHandle*>(digitItr());
  	    bool SameCandStrip = false;
  	    bool SameHit = false;

  	    if(TrkStrip->IsEqual(strip)) {SameCandStrip=true;}

  	    if(digit->GetChannelId()==TrkDigit->GetChannelId() &&
  	       strip->GetCharge(CalDigitType::kNone)==TrkStrip->GetCharge(CalDigitType::kNone))
	      {SameHit=true;}

  	    if(!SameCandStrip && SameHit) {
	      SliceStripsToRemove.push_back(strip);
  	      if(!AddedTrkStrip) {SliceStripsToAdd.push_back(TrkStrip);}
  	      AddedTrkStrip=true;
  	    }
  	  }
  	}
      }
      ///////////////////////////////////////////////////


      // Loop over track strip Digits, and rationalise DigitList
      ///////////////////////////////////////////////////
      if(DigitList) {
	CandDigitHandleItr TrkDigitItr2(TrkStrip->GetDaughterIterator());
	for(TrkDigit=TrkDigitItr2(); TrkDigit ; TrkDigit=TrkDigitItr2()) {
  	  bool AddedTrkDigit=false;
  	  CandDigitHandleItr DigitItr(DigitList->GetDaughterIterator());

  	  for(CandDigitHandle* digit=DigitItr(); digit; digit=DigitItr()) {
  	    bool SameCandDigit=false;
  	    bool SameHit=false;

  	    if(TrkDigit->IsEqual(digit)) {SameCandDigit=true;}

  	    if(digit->GetChannelId()==TrkDigit->GetChannelId() &&
  	       digit->GetCharge(CalDigitType::kNone)==TrkDigit->GetCharge(CalDigitType::kNone))
	      {SameHit=true;}

  	    if(!SameCandDigit && SameHit) {
	      DigitsToRemove.push_back(digit);
  	      if(!AddedTrkDigit) {DigitsToAdd.push_back(TrkDigit);}
  	      AddedTrkDigit=true;
  	    }
  	  }
  	}
      }
      ///////////////////////////////////////////////////
    
      ///////////////////////////////////////////////////
    }
  } // End loop over track strips


  // Now make the actual modifications to the lists
  ///////////////////////////////////////////////////
  for(unsigned int i=0; i<StripsToAdd.size(); ++i) {StripList->AddDaughterLink(*(StripsToAdd[i]));}
  for(unsigned int i=0; i<StripsToRemove.size(); ++i) {StripList->RemoveDaughter(StripsToRemove[i]);}
  StripsToAdd.clear();
  StripsToRemove.clear();

  for(unsigned int i=0; i<SliceStripsToAdd.size(); ++i) {Slice->AddDaughterLink(*(SliceStripsToAdd[i]));}
  for(unsigned int i=0; i<SliceStripsToRemove.size(); ++i) {Slice->RemoveDaughter(SliceStripsToRemove[i]);}
  SliceStripsToAdd.clear();
  SliceStripsToRemove.clear();

  for(unsigned int i=0; i<DigitsToAdd.size(); ++i) {DigitList->AddDaughterLink(*(DigitsToAdd[i]));}
  for(unsigned int i=0; i<DigitsToRemove.size(); ++i) {DigitList->RemoveDaughter(DigitsToRemove[i]);}
  DigitsToAdd.clear();
  DigitsToRemove.clear();
  ///////////////////////////////////////////////////////////////

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

  // Must delete DupHandle StripList and Digitlist (gmieg)
  delete StripList;
  delete DigitList;

  if(!SlushyOnEntry) CandHandle::SetSlushyEnabled(kFALSE); 
}
//////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////
double AlgFitTrackCam::NDStripBegTime(CandStripHandle* Strip, double U, double V)
{
  double BegTime=999; double Time=0;
  double Index=1.77; double DistFromCentre=0.; 
  double FibreDist=0.; double halfLength=0.;

  // Get from track. Otherwise, will have guessed using Swimmer
  if(U==0) {U=track->GetU(Strip->GetPlane());}
  if(V==0) {V=track->GetV(Strip->GetPlane());}

  StripEnd::StripEnd_t StpEnd = StripEnd::kUnknown;
  CandDigitHandle* digit;
  
  UgliGeomHandle ugh = UgliGeomHandle(*vldc);
  UgliStripHandle Striphandle;

  CandDigitHandleItr digitItr(Strip->GetDaughterIterator());


  // Loop over digits on Strip. 
  /////////////////
  while( (digit = digitItr()) ) { 
    StpEnd=digit->GetPlexSEIdAltL().GetEnd();
    
    if(StpEnd==StripEnd::kPositive) {
      FibreDist = 0.; DistFromCentre = 0.; Time=0.;
      UgliStripHandle StripHandle = ugh.GetStripHandle(Strip->GetStripEndId());
      halfLength = StripHandle.GetHalfLength(); 
      
      if(Strip->GetPlaneView()==2) {DistFromCentre = V;}
      if(Strip->GetPlaneView()==3) {DistFromCentre = -U;}
	      
      FibreDist = (halfLength + DistFromCentre + StripHandle.ClearFiber(StpEnd) 
		   + StripHandle.WlsPigtail(StpEnd));
      
      Time = digit->GetSubtractedTime(CalTimeType::kT0) - (Index/3.e8)*FibreDist;

      if(Time<BegTime) {BegTime=Time;}
    }
  }
  
  return BegTime;
}
//////////////////////////////////////////////////////////////////



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