////////////////////////////////////////////////////////////////////////
// $Id: AlgFitTrackMS.cxx,v 1.15 2005/10/26 03:38:40 rhatcher Exp $
//
// AlgFitTrackMS is a concrete FitTrackMS Algorithm class.
//
// Tom Bringley
// ttb2@duke.edu
// 6/13/2001
////////////////////////////////////////////////////////////////////////

#include <cassert>

#include "Algorithm/AlgConfig.h"
#include "Candidate/CandContext.h"
#include "CandFitTrackMS/AlgFitTrackMS.h"
#include "CandFitTrackMS/BFieldMS.h"
#include "CandFitTrackMS/CandFitTrackMSHandle.h"
#include "Conventions/Mphysical.h"
#include "Conventions/Munits.h"
#include "Conventions/PlaneView.h"
#include "MessageService/MsgService.h"
#include "MinosObjectMap/MomNavigator.h"
#include "Navigation/NavKey.h"
#include "Navigation/NavSet.h"
#include "Plex/PlexPlaneId.h"
#include "RecoBase/CandSliceHandle.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "UgliGeometry/UgliGeomHandle.h"
#include "UgliGeometry/UgliScintPlnHandle.h"
#include "Validity/VldContext.h"
// TCL.h isn't in our standard ROOT include directory - so i had to
// copy it into the CandFitTrackMS directory
//#include "TCL.h"
#include "CandFitTrackMS/TCL.h"

#include "TMath.h"
#include "TMatrixD.h"
#include "TVectorD.h"
#include "TRandom.h"
#include "TFile.h"
#include "TPolyLine3D.h"

#include "TROOT.h"
#include "TObject.h"
#include "TNtuple.h"
#include "TGraph.h" 
#include "TCanvas.h"
#include "TFile.h"
ClassImp(AlgFitTrackMS)

//______________________________________________________________________
CVSID("$Id: AlgFitTrackMS.cxx,v 1.15 2005/10/26 03:38:40 rhatcher Exp $");

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

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

  // set fCfh to ch so we can modify fCfh and return ch, the handle to the 
  // fitted track, to the FitTrackMSModule.
  // this is telling the code that it actually points to a specific
  // CandFitTrackMSHandle, not a generic CandHandle (see void above 
  assert(ch.InheritsFrom("CandFitTrackMSHandle"));
  fCfh = &dynamic_cast<CandFitTrackMSHandle &> (ch);

  InitFitHandle(cx);

  SetupAlg(ac);

  InitArrays();

  if(fNofit || (fNoBField && fNoMS) ||
     fNHits<fMinHits){

    DetermineQ();
    fCfh->SetMomentum(fCfh->GetMomentumL());
    fCfh->SetChi2(-1);
    fCfh->SetEMCharge(0);
    fCfh->SetEMChargeD(fQ);
    fCfh->SetIter(0);

    if(fFlag==1){
      fCfh->SetFlag(-3);
    }
    else{
      fCfh->SetFlag(3);
    }
  }
  else if(fNoMS!=0){

    Double_t p, L, pplus, pminus,Lplus, Lminus;

    Int_t temp1,temp2;
   
    fQ = -1.;
    temp1 = DoFitAlg(fCfh->GetMomentumL(),pminus,Lminus);

    fQ = 1.;
    temp2 = DoFitAlg(fCfh->GetMomentumL(),pplus,Lplus);

    if(Lminus < Lplus){

      p = pminus;
      L = Lminus;
      fQ = -1.;
      fCfh->SetEMCharge(-1.);
      fCfh->SetIter(temp1);
    }
    else{

      p = pplus;
      L = Lplus;
      fCfh->SetEMCharge(1.);
      fCfh->SetIter(temp2);
    }

    fCfh->SetMomentumBF(p);
    fCfh->SetChi2BF(L);
    
    if(p <= fMaxP && p >= fMinP){

      WriteFit(L,p);

      if(fFlag == 1)
        fCfh->SetFlag(-1);
      else
	fCfh->SetFlag(1);
    }
    // momentum has diverged - use the length
    else{

      fCfh->SetMomentum(fCfh->GetMomentumL());
      fCfh->SetChi2(-1);

      if(fFlag == 1)
        fCfh->SetFlag(-2);
      else
	fCfh->SetFlag(2);
    }
  }
  else if(fNoBField!=0){

    DetermineQ();
    fCfh->SetEMCharge(fQ);

    Double_t p, L;
    Int_t temp;
   
    temp = DoFitAlg(fCfh->GetMomentumL(),p,L);
  
    fCfh->SetMomentumMS(p);
    fCfh->SetChi2MS(L);
    fCfh->SetIter(temp);

    if(p <= fMaxP && p >= fMinP){

      WriteFit(L,p);

      if(fFlag == 1)
        fCfh->SetFlag(-1);
      else
	fCfh->SetFlag(1);
    }
    // momentum has diverged - use the length
    else{

      fCfh->SetMomentum(fCfh->GetMomentumL());
      fCfh->SetChi2(-1);

      if(fFlag == 1)
        fCfh->SetFlag(-2);
      else
	fCfh->SetFlag(2);
    }
  }
  else if(fFullAna==0 && fBothFit==0){
      
    Double_t p, L, pplus, pminus,Lplus, Lminus, pMS;
    Int_t temp;

    fNoMS = 1;
   
    fQ = -1.;
    DoFitAlg(fCfh->GetMomentumL(),pminus,Lminus);

    fQ = 1.;
    DoFitAlg(fCfh->GetMomentumL(),pplus,Lplus);

    if(Lminus < Lplus){

      p = pminus;
      L = Lminus;
      fQ = -1.;
      fCfh->SetEMCharge(-1.);
    }
    else{

      p = pplus;
      L = Lplus;
      fCfh->SetEMCharge(1.);
    }

    fNoMS = 0;
    fNoBField = 1;

    MakePPlanes(p);
    MakeStraightTrack();

    temp = DoFitAlg(p,pMS, L);
  
    fCfh->SetMomentumAlt(pMS);
    fCfh->SetChi2Alt(L);
    fCfh->SetIter(temp);

    if(pMS <= fMaxP && pMS >= fMinP){
    
      WriteFit(L,pMS);

      if(fFlag == 1)
        fCfh->SetFlag(-1);
      else
	fCfh->SetFlag(1);
    }
    // momentum has diverged - use the length
    else{

      fCfh->SetMomentum(fCfh->GetMomentumL());
      fCfh->SetChi2(-1);

      if(fFlag==1)
        fCfh->SetFlag(-2);
      else
	fCfh->SetFlag(2);
    }
  }
  else if(fFullAna==0 && fBothFit!=0){
      
    //    Double_t p, L, pplus, pminus,Lplus, Lminus;
    //    Int_t temp1, temp2;
    
    //    fQ = -1.;
    //    DoFitAlg(fCfh->GetMomentumL(),pminus,Lminus);

    //    fQ = 1.;
    //    DoFitAlg(fCfh->GetMomentumL(),pplus,Lplus);

    //    if(Lminus < Lplus){

    //      p = pminus;
    //      L = Lminus;
    //      fQ = -1.;
    //      fCfh->SetEMCharge(-1.);
    //      fCfh->SetIter(temp1);
    //    }
    //    else{

    //      p = pplus;
    //      L = Lplus;
    //      fCfh->SetEMCharge(1.);
    //      fCfh->SetIter(temp2);
    //    }

    Int_t temp;
    Double_t p,L;

    DetermineQ();
    fCfh->SetEMChargeD(fQ);
    fCfh->SetEMCharge(0);

    temp = DoFitAlg(fCfh->GetMomentumL(),p,L);
    fCfh->SetIter(temp);

    fCfh->SetMomentumBoth(p);
    fCfh->SetChi2Both(L);

    if(p <= fMaxP && p >= fMinP){
    
      WriteFit(L,p);

      if(fFlag == 1)
        fCfh->SetFlag(-1);
      else
	fCfh->SetFlag(1);
    }
    // momentum has diverged - use the length
    else{

      fCfh->SetMomentum(fCfh->GetMomentumL());
      fCfh->SetChi2(-1);

      if(fFlag == 1)
        fCfh->SetFlag(-2);
      else
	fCfh->SetFlag(2);
    }
  }
  else if(fFullAna != 0){

    // doing a couple different kinds of fits for this

    Double_t p, L, pMS,pminus,pplus,Lminus,Lplus;
    Int_t temp;
    
    // since this will take forever anyway -- for now, find charge
    // with DetermineQ and use that
    
    DetermineQ();
    fCfh->SetEMChargeD(fQ);

    // do a fit with BField off -- only MS.
    fNoBField = 1;
    fNoMS = 0;
    
    DoFitAlg(fCfh->GetMomentumL(),p,L);

    fCfh->SetMomentumMS(p);
    fCfh->SetChi2MS(L);

    // do a fit with BField only - no MS.
    fNoBField = 0;
    fNoMS = 1;

    fQ=-1;
    
    DoFitAlg(fCfh->GetMomentumL(),pminus,Lminus);

    fQ=1;

    DoFitAlg(fCfh->GetMomentumL(),pplus,Lplus);

    if(Lminus < Lplus){

      fCfh->SetEMCharge(-1.);
      fQ = -1;
      fCfh->SetMomentumBF(pminus);
      fCfh->SetChi2BF(Lminus);
      MakePPlanes(pminus);
      MakeStraightTrack();
      p = pminus;
    }
    else{
      fCfh->SetEMCharge(1.);
      fCfh->SetMomentumBF(pplus);
      fCfh->SetChi2BF(Lplus);
      MakePPlanes(pplus);
      MakeStraightTrack();
      p = pplus;
    }

    // fitted BField, now fit MS again with BField off (straight track 
    // stays)
    fNoBField = 1;
    fNoMS = 0;

    DoFitAlg(p,pMS,L);

    fCfh->SetMomentumAlt(pMS);
    fCfh->SetChi2Alt(L);

    // now fit both at same time
    fNoBField = 0;

    temp = DoFitAlg(fCfh->GetMomentumL(),p,L);

    fCfh->SetMomentumBoth(p);
    fCfh->SetChi2Both(L);
    fCfh->SetIter(temp);

    if(p <= fMaxP && p >= fMinP){
    
      WriteFit(L,p);

      if(fFlag == 1)
        fCfh->SetFlag(-1);
      else
	fCfh->SetFlag(1);
    }
    // momentum has diverged - use the length
    else{

      fCfh->SetMomentum(fCfh->GetMomentumL());
      fCfh->SetChi2(-1);

      if(fFlag == 1)
        fCfh->SetFlag(-2);
      else
	fCfh->SetFlag(2);
    }
  }
  else 
    MSG("FitTrackMS", Msg::kError) << "shouldn't ever get here" << endl;
}

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

//______________________________________________________________________
Double_t AlgFitTrackMS::ChiSquared(TMatrixD& ErrorMatrix)
{
  // calculate the Chi Squared.  Add the contribution from the x
  // points and the y points.
  Double_t chiSquared=0;
  Int_t mfactor;

  Double_t x0 = fSolnMatrix(0,0);
  Double_t xSlope = fSolnMatrix(1,0);
  Double_t y0 = fSolnMatrix(0,1);
  Double_t ySlope = fSolnMatrix(1,1);


  for(Int_t i=0; i<fNHits; i++){
    for(Int_t j=0; j<=i; j++){

      // since the error matrix is symmetric, we can get away with
      // looping only over half of it plus the diagonal.  we need to
      // account for, though, that each diagonal element should be
      // counted only once while all others should be counted twice to 
      // make up for the half of the matrix that is skipped.  hence,
      // this mfactor kludge.
      if(i!=j) mfactor=2;
      else mfactor=1;

      chiSquared +=
        mfactor*(fStraightXHits(i)-x0-xSlope*(fZHits(i)-fZHits(0)))*
	        (fStraightXHits(j)-x0-xSlope*(fZHits(j)-fZHits(0)))*
	        ErrorMatrix(i,j);
      chiSquared +=
        mfactor*(fStraightYHits(i)-y0-ySlope*(fZHits(i)-fZHits(0)))*
                (fStraightYHits(j)-y0-ySlope*(fZHits(j)-fZHits(0)))*
                ErrorMatrix(i,j);
    }
  }

  return chiSquared;
}

//______________________________________________________________________
void AlgFitTrackMS::DetermineQ()
{
  Double_t qCounter(0);
  TVector3 hit1,hit2,hit3,v1,v2,bvector,idealvector;

  // for now use BFieldMS - in the future, when normal BField is
  // working without the map, use plain old BField.
  BFieldMS bf(fCfh->GetVldContext());

  Int_t step = fNHits/10;

  if(step==0)
    step = 1;

  for(Int_t i=step;i < (fNHits-step);i++)
  {
    hit1.SetXYZ(fXHits(i-step),fYHits(i-step),fZHits(i-step));
    hit2.SetXYZ(fXHits(i),fYHits(i),fZHits(i));
    hit3.SetXYZ(fXHits(i+step),fYHits(i+step),fZHits(i+step)); 
  
    v1 = hit2 - hit1;
    v2 = hit3 - hit2;
    v2 = v2 - (v1.Dot(v2)/v1.Mag()/v1.Mag())*v1;  // Get Perp vector

    if (v1.Mag()==0)
      MSG("FitTrackMS", Msg::kInfo)  << "huh? " << i << endl;
  
    bvector = bf.GetBField(hit2);

    // this corrects for the stupid bfield being wrong!!!!
    if(fBFisFlipped)
      bvector*=-1.;
    
    idealvector = v1.Cross(bvector);

    //    if(idealvector.Dot(v2) > 0)
    //      qCounter++;
    //    else if(idealvector.Dot(v2) < 0)
    //      qCounter--;

    qCounter += idealvector.Dot(v2);
    
  }
  
  if (qCounter >= 0)
    fQ = 1.0;
  else
    fQ = -1.0;
}

//---------------------------------------------------------------------
// function to run a fit - repeat until converged
Int_t AlgFitTrackMS::DoFitAlg(Double_t pInit, Double_t& pFit, Double_t& 
                             LFit)
{
  Double_t pmin,pmax,pmid;
  Double_t minDet,maxDet,midDet;
  Double_t minChi2,maxChi2,midChi2;
  Double_t minL,maxL,midL;
  Double_t logpmin,logpmid,logpmax;
  Double_t pnew, pbest,bestL;

  Bool_t converged = false;

  // begin new method
  /*
  Int_t iterations(0);

  pmid = fCfh->GetMomentumL();
    
  while(!converged && iterations < fMaxIter){

    FitTrack(pmid,midDet,midChi2);

    MSG("FitTrackMS", Msg::kDebug)
      << "tried " << pmid << " and got " << midChi2 << endl;

    pmid = pmid*TMath::Sqrt(fNHits/midChi2);

    iterations++;

  }
  if(fQ==-1){
    pbest = pmid;
    bestL = midChi2;
  }
  else{
    if(midChi2 < bestL){
      pbest = pmid;
      bestL = midChi2;
    }
  }
 
  WriteFit(bestL,pbest);
}
  */
  // end new method


  // begin old method
    
  // choose 3 beginning guesses for the momentum.  as of now, we use
  // the length estimate to generate these.
  pmin = pInit*0.5;
  pmax = pInit*1.5;
  pmid = pInit;

  //   try the fit with the 3 guessed momenta.
  FitTrack(pmin,minDet,minChi2);
  FitTrack(pmax,maxDet,maxChi2);
  FitTrack(pmid,midDet,midChi2);
  
  // calculate L for each fit.  L is a likelihood function of momentum 
  // that is calculated by summing Chi Squared and the log of the
  // determinant of the covariance matrix.  A simple minimization of
  // Chi Squared will not work here because it scales with the
  // momentum squared.  The determinant scales with one over the
  // momentum squared so, summing their logs, we get L, a quantity
  // which does not explicitly scale with momentum, so we can minimize 
  // L to get a good approximation of the true momentum.
  minL = .5 * minDet + .5 * minChi2;
  maxL = .5 * maxDet + .5 * maxChi2;
  midL = .5 * midDet + .5 * midChi2;
    
  MSG("FitTrackMS", Msg::kDebug) 
    << "tried " << pmin << " and got " << minL << endl
    << "tried " << pmid << " and got " << midL << endl
    << "tried " << pmax << " and got " << maxL << endl;
      
  if(minL < midL && minL < maxL){

    pbest = pmin;
    bestL = minL;
  }
  else if(midL < maxL){

    pbest = pmid; 
    bestL = midL;
  }
  else{

    pbest = pmax;
    bestL = maxL;
  }

  Int_t iterations(0);

  while(!converged){

    MSG("FitTrackMS", Msg::kDebug)
      << "iteration # " << iterations << endl;
      
    // use standard polynomial interpolation to fit minL, midL, and
    // maxL as a function of the logs of pmin,
    // pmid, and pmax to a parabola.  then, use exponential of the
    // minimum of the parabola to get the next test momentum.
    logpmin = TMath::Log(pmin);
    logpmid = TMath::Log(pmid);
    logpmax = TMath::Log(pmax);

    TMatrixD pmat(3,3);
    TMatrixD lmat(3,1);
    TMatrixD smat(3,1);

    pmat(0,0) = 1.0;
    pmat(1,0) = 1.0;
    pmat(2,0) = 1.0;
    pmat(0,1) = logpmin;
    pmat(1,1) = logpmid;
    pmat(2,1) = logpmax;
    pmat(0,2) = logpmin*logpmin;
    pmat(1,2) = logpmid*logpmid;
    pmat(2,2) = logpmax*logpmax;

    if(pmat.Determinant() != 0.)
      pmat.Invert();

    lmat(0,0) = minL;
    lmat(1,0) = midL;
    lmat(2,0) = maxL;

    smat.Mult(pmat,lmat);

    if(smat(2,0)!=0)
      pnew = TMath::Exp(-1*smat(1,0)/2.0/smat(2,0));
    else
      break;

    // if the (log p)^2 term of the fit is negative then the parabola
    // is convex, which means that pnew is a maximum instead of a
    // minimum.  in this case, set pnew to a non fixed value (to avoid
    // repetition of guesses) in the neighborhood of pbest.
    if(smat(2,0)<0){

      pnew = pbest*pbest/pnew;
    }

    // if pnew = infinity, set pnew to a random value in the
    // neighborhood of pbest.
    if(1.0/pnew == 0){
 
      pnew = pbest+gRandom->Rndm()*pbest/2.0 - pbest/4.0;
    }

    // if pnew is a repeat of a p already tested, set it to a random
    // value in the neighborhood of itself.
    if(pnew == pmin || pnew == pmid || pnew == pmax){

      pnew = pnew + gRandom->Rndm()*pnew/4.0 - pnew/8.0;
    }

    // if p looks like it is diverging to infinity, then it is likely
    // that the charge on the particle is wrong.  so, switch the
    // testing charge and restart the momentum search.  if the charge
    // has already been changed and p is still diverging, abort the
    // test.  if we develop a better method of determining the charge
    // of the particle, this section could be irrelevent.
    if(pmin > fMaxP && pnew > fMaxP){

      // need to do something here!
      break;
    }

    // dont allow testing of a negative or zero momentum - guess smaller
    if(pnew < fMinP){

      pnew = pmin / 2.0;
    }

    // now, determine where pnew falls in pmin, pmid, and pmax.
    // choose pnew and the 2 closest values to it to be the new pmin,
    // pmid, and pmax.  test pnew.  update pbest if necessary.
    if(pnew < pmin){

      pmax = pmid;
      pmid = pmin;
      pmin = pnew;
      maxL = midL;
      midL = minL;

      FitTrack(pmin,minDet,minChi2);

      minL = .5 * minDet + .5 * minChi2;
      MSG("FitTrackMS", Msg::kDebug) 
        << "iterate momentum of " << pmin
        <<" and got L of " << minL << endl;

      if(minL < bestL){

        pbest = pmin;
        bestL = minL;
      }
    }
    else if(pnew < pmid && pmax-pnew > pnew-pmid){

      pmax = pmid;
      pmid = pnew;
      maxL = midL;

      FitTrack(pmid,midDet,midChi2);

      midL = .5 * midDet + .5 * midChi2;

      MSG("FitTrackMS", Msg::kDebug)
        << "Tried momentum of " << pmid <<" and got L of " << midL   << endl;

      if(midL < bestL){
 
        pbest = pmid;
        bestL = minL;
      }
    }
    else if(pnew < pmid && pmax-pnew < pnew-pmid){

      pmin = pnew;

      FitTrack(pmin,minDet,minChi2);

      minL = .5 * minDet + .5 * minChi2;

      MSG("FitTrackMS", Msg::kDebug)
        << "Tried momentum of " << pmin <<" and got L of " << minL << endl;

      if(minL < bestL){
 
        pbest = pmin;
        bestL = minL;
      }
    }
    else if(pnew < pmax && pmax-pnew > pnew-pmin){

      pmax = pnew;

      FitTrack(pmax,maxDet,maxChi2);

      maxL = .5 * maxDet + .5 * maxChi2;

      MSG("FitTrackMS", Msg::kDebug)
        << "Tried momentum of " << pmax <<" and got L of " << maxL << endl;

      if(maxL < bestL){

        pbest = pmax;
        bestL = maxL;
      }
    }
    else if(pnew < pmax && pmax-pnew < pnew-pmin){

      pmin = pmid;
      pmid = pnew;
      minL = midL;
      
      FitTrack(pmid,midDet,midChi2);

      midL = .5 * midDet + .5 * midChi2;

      MSG("FitTrackMS", Msg::kDebug)
        << "Tried momentum of " << pmid <<" and got L of " << midL << endl;

      if(pmid < bestL){

          pbest = pmid;
          bestL = midL;
      }
    }
    else{

      pmin = pmid;
      pmid = pmax;
      pmax = pnew;
      minL = midL;
      midL = maxL;

      FitTrack(pmax,maxDet,maxChi2);

      maxL = .5 * maxDet + .5 * maxChi2;

      MSG("FitTrackMS", Msg::kDebug)
        << "Tried momentum of " << pmax <<" and got L of " << maxL << endl;

      if(maxL < bestL){

        pbest = pmax;
        bestL = maxL;
      }
    }

    // converging condition.
    if((TMath::Max(TMath::Max(minL,midL),maxL) -
        TMath::Min(TMath::Min(minL,midL),maxL))/
        TMath::Min(TMath::Min(minL,midL),maxL) < fLTolerance &&

       (TMath::Max(TMath::Max(pmin,pmid),pmax) -
        TMath::Min(TMath::Min(pmin,pmid),pmax))/
        TMath::Min(TMath::Min(pmin,pmid),pmax) < fPTolerance){

       converged = true;
    }

    if(iterations > fMaxIter){
      converged = true;
    }
    iterations++;
  }

  pFit = pbest;
  LFit = bestL;

  return iterations;
}


//______________________________________________________________________
void AlgFitTrackMS::FitTrack(Double_t p0, Double_t &LogCovMDeterminant,
	     Double_t &chiSquared)
{  
  TMatrixD CovMatrix(fNHits,fNHits);
  TMatrixD ErrorMatrix(fNHits,fNHits);
  TMatrixD VariableCoefMatrix(2,2);
  TMatrixD ConstantCoefMatrix(2,2);

  MakePPlanes(p0);
  
  MakeCovarianceMatrix(CovMatrix, p0);

  // minimize Chi Squared by solving a matrix equation.
  // LogCovMDeterminant = TMath::Log(CovMatrix.Determinant());

  if(fPosErr!=0.)
    CovMatrix*= 1/TMath::Power(fPosErr,2);

  LogCovMDeterminant = TMath::Log(CovMatrix.Determinant());
  
  if(1/LogCovMDeterminant == 0.0){

    MSG("FitTrackMS", Msg::kDebug) << "ah!!!!!!!!!!!!!!!!!!!" << endl;

    LogCovMDeterminant = 1000000;
  }
  
  //InvertCovMatrix(CovMatrix, ErrorMatrix, LogCovMDeterminant);
    CovMatrix.Invert();          // Replaces CovMatrix.InvertPosDef();
  //gmi CovMatrix.InvertPosDef(); // InvertPosDef() eliminated in ROOT
  ErrorMatrix = CovMatrix;

  if(fPosErr!=0.){
    LogCovMDeterminant += 2*fNHits*TMath::Log(fPosErr);
    ErrorMatrix*=1/TMath::Power(fPosErr,2);
  }
  
  MakeStraightTrack();

  MakeSolnMatrices(VariableCoefMatrix,ConstantCoefMatrix,ErrorMatrix);

  if(VariableCoefMatrix.Determinant() != 0.)
    VariableCoefMatrix.Invert();

  fSolnMatrix.Mult(VariableCoefMatrix, ConstantCoefMatrix);

  // calculate Chi Squared.
  chiSquared = ChiSquared(ErrorMatrix);

  //  LogCovMDeterminant = -1*fNHits*TMath::Log(p0);
  //LogCovMDeterminant = 0;
}

//______________________________________________________________________
Double_t AlgFitTrackMS::GetSigmaMS(Double_t peloss)
{
  // this is a standard formula for calculating the mean square
  // deflection angle in multiple scattering.

  if(!fNoMS)
    return TMath::Power(.0136*TMath::Sqrt(fSteelPlnWidth/fXZero)*
                     (1.0+.038*TMath::Log(fSteelPlnWidth/fXZero))/peloss,2);
  else
    return 0;
  
}

//______________________________________________________________________
void AlgFitTrackMS::InitArrays()
{
  MSG("FitTrackMS", Msg::kDebug) << "InitArray" << endl;

  UgliGeomHandle ugh(*fCfh->GetVldContext());
  
  Int_t sMod(-1);
  Double_t pathlength(0);

  Int_t bPlane = TMath::Min(fCfh->GetBegPlane(),fCfh->GetEndPlane());
  Int_t ePlane = TMath::Max(fCfh->GetBegPlane(),fCfh->GetEndPlane());

  // preliminarily resize these vectors to the number of
  // possible hits
  fXHits.ResizeTo(ePlane-bPlane+1);
  fYHits.ResizeTo(ePlane-bPlane+1);
  fZHits.ResizeTo(ePlane-bPlane+1);
  fZPlanes.ResizeTo(ePlane-bPlane+1);

  fXHits.Zero();
  fYHits.Zero();
  fZHits.Zero();
  fZPlanes.Zero();

  fNHits = 0;
  fNPlanes = 0;

  Double_t timetemp(0);
  
  for(Int_t i=bPlane;i<=ePlane;i++){

    // check if you are in the supermodule -- if you are, record which 
    // hit you are at so the supermodule gap can be subtracted from
    // pathlength later
    if(i==fSuperModSkippedPlane)
      sMod = fNHits;
    
    // get the scintilator and steel plane numbered i.  a couple of
    // problems here:  it assumes that the scintilator and steel
    // planes of the same number are always next to eachother - it
    // assumes the planes are in ascending z order (which is true for
    // now, but wouldnt have to be)
    PlexPlaneId planeid(fDetector, i);
    UgliScintPlnHandle scintp = ugh.GetScintPlnHandle(planeid);
    planeid.SetIsSteel(true);
    UgliSteelPlnHandle steelp = ugh.GetSteelPlnHandle(planeid);

    if(scintp.IsValid()){            

      fZHits(fNHits) = scintp.GetZ0() + .5*fScintPlnWidth;

      // as of now, this whole algorithm uses x,y coordinates.  should 
      // be able to switch u,v without trouble though.  have to switch 
      // the b field is why i haven't gotten around to it yet.
      ugh.uv2xy(fCfh->GetU(i),fCfh->GetV(i),fXHits(fNHits),fYHits(fNHits));

      // get the time so (for now) can test if this is a real hit or
      // an interpolated hit.  if interpolated GetT will return
      // -99999.

      timetemp = fCfh->GetT(i);

      if(fXHits(fNHits)>-90000 && fYHits(fNHits)>-900000
         && timetemp>-90000 && fNHits<fMaxHits){

        fNHits++;
      }
    }

    if(steelp.IsValid() && (steelp.GetZ0() > fZHits(0))&& i!=ePlane){

      fZPlanes(fNPlanes) = steelp.GetZ0() + .5*fSteelPlnWidth;
      
      fNPlanes++;
    }    
  }

  Float_t zmin, zmax;
  
  // this calculates the momentum based on the path length and stores
  // it with the SetMomentumL method.  the geometry stuff is there to
  // determine if the last hit of the track is the last plane in the
  // detector - i.e. supposedly if the muon exited the detector.  it
  // doesnt check, though, to see if the muon exited out of the sides
  // of the detector instead of the back, or if for some reason it
  // didnt hit the last plane but exited anyway.
  ugh.GetZExtent(zmin,zmax);
  PlexPlaneId maxPlane = ugh.GetPlaneIdFromZ(zmax);

  // determine pathlength
  for(int i=1;i<fNHits;i++){
    pathlength +=
      TMath::Sqrt((fXHits(i)-fXHits(i-1))*(fXHits(i)-fXHits(i-1)) +
                  (fYHits(i)-fYHits(i-1))*(fYHits(i)-fYHits(i-1)) +
                  (fZHits(i)-fZHits(i-1))*(fZHits(i)-fZHits(i-1)));
  }
		  
  // subtract the supermodule
  if(fDetector == Detector::kFar && sMod!=-1 &&
     sMod!=0 && sMod!=fNHits){

    pathlength-= fSuperModGapSize/(fZHits(sMod)-fZHits(sMod-1))*
      TMath::Sqrt(TMath::Power(fXHits(sMod)-fXHits(sMod-1),2) + 
                  TMath::Power(fYHits(sMod)-fYHits(sMod-1),2) + 
                  TMath::Power(fZHits(sMod)-fZHits(sMod-1),2));

    if(pathlength<0){
      MSG("FitTrackMS", Msg::kDebug) << "supermodule problem!" << endl;

      pathlength = 1.;
      fFlag=1;
    }
  }

  // flag track if last hit is within .2 of the
  // end of the detector or last hit is outside a makeshift octagon
  // shaped area.  Use u-v coordinates.

  Double_t lastu, lastv;
  
  ugh.xy2uv(fXHits(fNHits-1),fYHits(fNHits-1),lastu, lastv);
  
  if(maxPlane.GetPlane() - 5 <= ePlane ||
     TMath::Abs(lastu) > 3.8 ||
     TMath::Abs(lastv) > 3.8 ||
     TMath::Abs(lastu) + TMath::Abs(lastv) > 5.3)
  {
    fFlag = 1;
  }

  fCfh->SetMomentumL(pathlength*fDedx*fSteelPlnWidth/fTotalPlnWidth);
  
  // rhb log p 94, my calculation of mean dE/dx replaces Tom's
  // I ignore fdedx
  //  if(ePlane != maxPlane.GetPlane()){
    // the 21/18 is a total fudge -- it's in my log book.  Other people
    // are also seeing a similar discrepancy in dE/dx, which is just a total 
    //fudge at this point.  Ne fuss pas.
    //fCfh->SetMomentumL((21/18.)*pathlength*.021/.0594);

    // it's not working, so i'm restoring my method of calculating
    // this
    //    fCfh->SetMomentumL(fdedx*pathlength*.0254/.0594);
  //    fCfh->SetMomentumL(pathlength);
  //  }
  //  else{
    /* this occurs if the track leaves the back of the detector
          Tom tries 0.5, 1, and 1.5 times this momentum.  But this can
	  be totally unreasonable. Start with three */
    //      fCfh->SetMomentumL(3.*pathlength*.021/.0594);
  //    fCfh->SetMomentumL(-1);
  //  }
  // resize the vectors correctly now

  MSG("FitTrackMS", Msg::kDebug) << "nHits = " << fNHits << endl;
  
  fXHits.ResizeTo(fNHits);
  fYHits.ResizeTo(fNHits);
  fZHits.ResizeTo(fNHits);
  fZPlanes.ResizeTo(fNPlanes);
  fPPlanes.ResizeTo(fNPlanes);

  fStraightXHits.ResizeTo(fNHits);
  fStraightYHits.ResizeTo(fNHits);
  fStraightXHits = fXHits;
  fStraightYHits = fYHits;
}

//______________________________________________________________________
void AlgFitTrackMS::InitFitHandle(CandContext &cx)
{
  // this method copies the data in the CandTrack to the new
  // CandFitTrackMS.  something like this really ought to be included in 
  // the CandFitTrack abstract base class.
  assert(cx.GetDataIn());

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

  const CandTrackHandle *track0 = 
    dynamic_cast<const CandTrackHandle*>(cx.GetDataIn());

  fCfh-> SetCandSlice(track0->GetCandSlice());
  fCfh->SetDirCosU(track0->GetDirCosU());
  fCfh->SetDirCosV(track0->GetDirCosV());
  fCfh->SetDirCosZ(track0->GetDirCosZ());
  fCfh->SetVtxU(track0->GetVtxU());
  fCfh->SetVtxV(track0->GetVtxV());
  fCfh->SetVtxZ(track0->GetVtxZ());
  fCfh->SetVtxT(track0->GetVtxT());
  fCfh->SetVtxPlane(track0->GetVtxPlane());
  fCfh->SetTimeSlope(track0->GetTimeSlope());

  fCfh->SetMomentum(0.);
  fCfh->SetMomentumL(0.);
  fCfh->SetMomentumBF(0.);
  fCfh->SetMomentumMS(0.);
  fCfh->SetMomentumBoth(0.);
  fCfh->SetMomentumAlt(0.);
  fCfh->SetChi2(0.);
  fCfh->SetChi2L(0.);
  fCfh->SetChi2BF(0.);
  fCfh->SetChi2MS(0.);
  fCfh->SetChi2Both(0.);
  fCfh->SetChi2Alt(0.);

  fCfh->SetEMCharge(0.);
  fCfh->SetEMChargeD(0.);
  
  CandStripHandleItr stripItr(track0->GetDaughterIterator());
  CandStripHandleKeyFunc *stripKf = stripItr.CreateKeyFunc();
  stripKf->SetFun(CandStripHandle::KeyFromPlane);
  stripItr.GetSet()->AdoptSortKeyFunc(stripKf);
  stripKf = 0;
  
  CandStripHandle * strip=0;

  while ((strip=stripItr())) {

    if(fCfh->IsInShower(strip)<=fInShower){
      fCfh->AddDaughterLink(*strip);
    }
  }
  fCfh->SetCandRecord(track0->GetCandRecord());

  SetUVZT(fCfh);

  Calibrate(fCfh);
  
  //  for(Int_t i=0;i<500;i++){

  //    fCfh->SetU(i,track0->GetU(i));
  //    fCfh->SetV(i,track0->GetV(i));
  //    fCfh->SetT(i,StripEnd::kNegative,track0->GetT(i,StripEnd::kNegative));
  //    fCfh->SetT(i,StripEnd::kPositive,track0->GetT(i,StripEnd::kPositive));
  //  }       
}

//______________________________________________________________________
void AlgFitTrackMS::InvertCovMatrix(TMatrixD &CovMatrix,
       TMatrixD &ErrorMatrix, Double_t& LogDet)
{
  // this method uses the TCL functions to efficiently invert a
  // symmetric matrix.  we ought to have a symmetric matrix class that 
  // does this sort of thing.
  Int_t counter(0);

  // we need fortran style matrices for TCL.  CovMatrix is written to
  // a fortran matrix, which is inverted and whose inverse is written
  // to another fortram style matrix which is then written to
  // ErrorMatrix.

  /* try another scheme */

//rwh:  Double_t mat[fNHits*fNHits];
//rwh:  Double_t tempmat[fNHits*fNHits];
//rwh:  C++ forbids variable-size array  --- need new/delete[]
  Double_t *mat = new Double_t[fNHits*fNHits];
  Double_t *tempmat = new Double_t[fNHits*fNHits];

  MSG("FitTrackMS", Msg::kDebug) << "Cov Matrix" << endl;
  
  for(Int_t i=0;i<fNHits;i++){
    //    for(Int_t j=0;j<=i;j++){
    for(Int_t j=0;j<fNHits;j++){

      tempmat[counter]=CovMatrix(i,j);
      counter++;
      MSG("FitTrackMS", Msg::kDebug) 
        << i << "  " << j << "  " << CovMatrix(i,j) << endl; 
    }
  }

  TCL::trchlu(tempmat,mat,fNHits);

  LogDet = 0;

  MSG("FitTrackMS", Msg::kDebug) << "diag" << endl;
  
  for(Int_t i=0;i<fNHits;i++) {

    MSG("FitTrackMS", Msg::kDebug) << i << "   " << mat[i*fNHits+i] << endl;
    
    if(mat[i*fNHits+i] > 0){
      LogDet += 2*TMath::Log((Double_t)mat[i*fNHits+i]);
    }
    else if(mat[i*fNHits+i] == 0){
      MSG("FitTrackMS", Msg::kDebug) << "zero exactly" << endl;
    }
    else{
      MSG("FitTrackMS", Msg::kDebug) << "less than zero" << endl;
    }
  }

  MSG("FitTrackMS", Msg::kDebug) << "log det = " << LogDet << endl;
  
  TCL::trinv(tempmat,mat,fNHits);

  counter=0;

  MSG("FitTrackMS", Msg::kDebug) << "Error Matrix" << endl;
  
  for(Int_t i=0;i<fNHits;i++) {
    for(Int_t j=0;j<fNHits;j++) {

      ErrorMatrix(i,j)=(Double_t)mat[counter];
      //      ErrorMatrix(j,i)=ErrorMatrix(i,j);
      counter++;

      MSG("FitTrackMS", Msg::kDebug) 
        << i << "   " << j << "   " << ErrorMatrix(i,j) << endl;
    }
  }

  //   Double_t determinant = CovMatrix.TMatrixD::Determinant();
  Double_t determinant = 1.0;
  
  if (determinant <=0)	          
  {
    for(Int_t i=0;i<fNHits;i++) {
      for(Int_t j=0;j<=i;j++) {

        if (i==j)
        {
          ErrorMatrix(i,j) = 1;
          CovMatrix(i,j) = 1.;
        }
        if (i!=j)
        {
          ErrorMatrix(i,j) = 0.;
          CovMatrix(i,j) = 0.;
        }
      }
    }
  }


//rwh: delete allocated array
  delete [] mat;
  delete [] tempmat;

}
    
//______________________________________________________________________
void AlgFitTrackMS::MakeCovarianceMatrix(TMatrixD& CovMatrix, 
                                         Double_t /* p0 */)
{
  
  Double_t sigma2MS, dzi, dzj;
  Int_t k;

  CovMatrix.Zero();

  // make the covariance matrix -- loop over half of the matrix
  // because it is symmetric.
  for(Int_t i=0; i<fNHits; i++){
    for(Int_t j=0; j<=i; j++){

      // add the uncertainty on position to diagonal elements
      if(i==j)
      {
        CovMatrix(i,j) += fPosErr*fPosErr;
      }
      if (1.)
      {
        k=0;
        // loop over all planes between the first hit to the jth hit.  j
        // is always smaller than i.
        while(k<fNPlanes&&fZPlanes(k)<fZHits(j)){

          dzi = fZHits(i) - fZPlanes(k);
          dzj = fZHits(j) - fZPlanes(k);

          // get the square RMS scattering angle
          sigma2MS = GetSigmaMS(fPPlanes(k));

          // add the covariance from the scattering
          CovMatrix(i,j) += sigma2MS*(fSteelPlnWidth*fSteelPlnWidth/3.0
	                 + fSteelPlnWidth*(dzi+dzj)/2.0 + dzi*dzj);
      	  k++;
        }
      }
      // the matrix is symmetric
      CovMatrix(j,i) = CovMatrix(i,j);
    }
  }
}

//______________________________________________________________________
void AlgFitTrackMS::MakeStraightTrack()
{

  if(!fNoBField){
    fStraightXHits.ResizeTo(fNHits);
    fStraightYHits.ResizeTo(fNHits);

    fStraightXHits = fXHits;
    fStraightYHits = fYHits;

    // for now use BFieldMS - in the future, when normal BField is
    // working without the map, use plain old BField.
    BFieldMS bf(fCfh->GetVldContext());
    TVector3 temp,bfield;

    Double_t xslope,yslope, sinThetaDx, sinThetaDy, pz, dist;
  
    Int_t i(0), k(0);

    // loop over steel planes -- keep track of the 3 hits surrounding.
    while(i<fNHits && k<fNPlanes){

      while(k < fNPlanes && fZHits(i) < fZPlanes(k) &&
            fZPlanes(k) < fZHits(i+1)){

        // estimate the slope the particle had while going into the
        // plane.  if no hits are missing, this is roughly the average of
        // the slopes of lines drawn connecting the hit before the plane
        // to the previous
        // hit and the hit before the plane to the following hit.  If
        // planes are missing,
        // this gets more complicated.  If hits are missing, use a
        // weighted average.

        if(i!=0){
      
          xslope = ((fZHits(i+1)-fZHits(i))*(fXHits(i) - fXHits(i-1)) /
                    (fZHits(i) - fZHits(i-1)) +
                   (fZHits(i)-fZHits(i-1))*(fXHits(i+1) - fXHits(i)) /
                    (fZHits(i+1) - fZHits(i))) /
                   (fZHits(i+1) - fZHits(i-1));

          yslope = ((fZHits(i+1)-fZHits(i))*(fYHits(i) - fYHits(i-1)) /
                    (fZHits(i) - fZHits(i-1)) +
                   (fZHits(i)-fZHits(i-1))*(fYHits(i+1) - fYHits(i)) /
                    (fZHits(i+1) - fZHits(i))) /
                   (fZHits(i+1) - fZHits(i-1));
        }
        else{

	  xslope = (fXHits(1)-fXHits(0))/(fZHits(1)-fZHits(0));
          yslope = (fYHits(1)-fYHits(0))/(fZHits(1)-fZHits(0));
        }

        // calculate p component in z direction.  for now, ignore p in x 
        // and y directions

        pz = fPPlanes(k)*TMath::Cos(TMath::ATan(TMath::Sqrt(
                       xslope*xslope + yslope*yslope)));

        // calculate the actual amount of distance travelled through the 
        // steel plane

        dist = fSteelPlnWidth*TMath::Sqrt(1 + TMath::Power(xslope,2) +
                                        TMath::Power(yslope,2));
    
        // calculate the BField deflection angle in plane k

        temp.SetXYZ(fXHits(i) + xslope*(fZPlanes(k)-fZHits(i)),
                    fYHits(i) + yslope*(fZPlanes(k)-fZHits(i)),
                    fZPlanes(k));

        bfield = bf.GetBField(temp);
      
        // once again, correct for bfield being wrong
	if(fBFisFlipped)
          bfield*=-1.;
      
        sinThetaDx = -fQ*.3*bfield.Y()*dist/pz;
        sinThetaDy = fQ *.3*bfield.X()*dist/pz;
        //      sinThetaDx = 0;
        //      sinThetaDy = 0;

        // sum the bfield effects. 
        for (Int_t j=i+1;j<fNHits;j++){
    
          fStraightXHits(j) -= sinThetaDx*(fZHits(j)-fZPlanes(k));
	  fStraightYHits(j) -= sinThetaDy*(fZHits(j)-fZPlanes(k));
        }
        k++;
      }
      i++;
    }
  }
  // if fStraightXHits not been initialized and fNoBField!=0 then
  // initialize them
  else if(fStraightXHits.GetNrows() == 0){
    fStraightXHits.ResizeTo(fNHits);
    fStraightYHits.ResizeTo(fNHits);

    fStraightXHits.ResizeTo(fNHits);
    fStraightYHits.ResizeTo(fNHits);
    fStraightXHits = fXHits;
    fStraightYHits = fYHits;

    MSG("FitTrackMS", Msg::kError) 
      << "Shouldn't ever get here!!!!!!!!!!!!!!!!!" << endl;
  }
}

//______________________________________________________________________
void AlgFitTrackMS::MakePPlanes(Double_t p0)
{
  fPPlanes.Zero();
  
  // The function makes a vector PPlanes which stores the value of p
  // for each plane, taking into account Dedx energy loss.
  //
  // Since the value of p you input into the function is actually the
  // p when the muon is created, it is the p approximately half way
  // through the steel plane before the first hit.  In contrast,
  // fPPlanes(0) is the value of p half way through the plane after
  // the first hit.

  Int_t i(0),k(0);
  Double_t pcounter(0);

  while(i<fNHits-1 && k<fNPlanes){

    while(k < fNPlanes && fZHits(i) < fZPlanes(k) &&
          fZPlanes(k) < fZHits(i+1)){

      // The dist travelled in steel from one plane to another is
      // calculated  by summing the dist travelled in the second half of
      // the last plane with the dist travelled in the first half of the
      // plane k.  The direction is extrapolated from surrounding hit
      // data.

      // this is done in such a way that the formula is simply...
      Double_t dz = fSteelPlnWidth/(fZHits(i+1)-fZHits(i))*
        (TMath::Sqrt(TMath::Power(fXHits(i+1)-fXHits(i),2) +
                     TMath::Power(fYHits(i+1)-fYHits(i),2) +
                     TMath::Power(fZHits(i+1)-fZHits(i),2)));

      pcounter += dz*fDedx;

      if(k!=0){
	
        fPPlanes(k) = fPPlanes(k-1) - dz*fDedx;
	if(fPPlanes(k) < 0)
	  fPPlanes(k) = fPPlanes(k-1)/2.;
      }
      else{
	
	fPPlanes(k) = p0 - dz*fDedx;
        if(fPPlanes(k) < 0)
	  fPPlanes(k) = p0/2.;
      }
      
      k++;
    }
    i++;
  }
  assert(k==fNPlanes);
}


//______________________________________________________________________
void AlgFitTrackMS::MakeSolnMatrices(TMatrixD& VariableCoefMatrix,
          TMatrixD& ConstantCoefMatrix, TMatrixD& ErrorMatrix)
{

  Double_t z0 = fZHits(0);

  VariableCoefMatrix.Zero();
  ConstantCoefMatrix.Zero();

  // this is somewhat complicated.  you can derive what goes into
  // these by taking partials of the Chi squared and solving - or you
  // can take my word for it.

  // further complications arise because i foolishly insist on only
  // looping over half the matrix.  and we're doing this for x and y.

  for(Int_t i=0; i<ErrorMatrix.GetNrows(); i++){
    for(Int_t j=0; j<=i; j++){
      
      if(i==j){
        VariableCoefMatrix(0,0) += ErrorMatrix(i,j);
        VariableCoefMatrix(1,0) +=
          (fZHits(i)-z0)*ErrorMatrix(i,j);
        VariableCoefMatrix(1,1) +=
          (fZHits(i)-z0)*(fZHits(j)-z0)*ErrorMatrix(i,j);
      
        ConstantCoefMatrix(0,0) +=
          fStraightXHits(i)*ErrorMatrix(i,j);
        ConstantCoefMatrix(1,0) +=
          fStraightXHits(i)*(fZHits(j)-z0)*ErrorMatrix(i,j);
        ConstantCoefMatrix(0,1) +=
          fStraightYHits(i)*ErrorMatrix(i,j);
        ConstantCoefMatrix(1,1) +=
          fStraightYHits(i)*(fZHits(j)-z0)*ErrorMatrix(i,j);
      }

      else{
        VariableCoefMatrix(0,0)+=2*ErrorMatrix(i,j);
        VariableCoefMatrix(1,0)+=(fZHits(i)+fZHits(j)-2*z0)*ErrorMatrix(i,j);
        VariableCoefMatrix(1,1) +=
          2*(fZHits(i)-z0)*(fZHits(j)-z0)*ErrorMatrix(i,j);

        ConstantCoefMatrix(0,0) += (fStraightXHits(i) +
          fStraightXHits(j))*ErrorMatrix(i,j);
        ConstantCoefMatrix(1,0) +=
          (fStraightXHits(i)*(fZHits(j)-z0)+
           fStraightXHits(j)*(fZHits(i)-z0))*ErrorMatrix(i,j);
        ConstantCoefMatrix(0,1) += (fStraightYHits(i) +
          fStraightYHits(j))*ErrorMatrix(i,j);
        ConstantCoefMatrix(1,1) +=
          (fStraightYHits(i)*(fZHits(j)-z0)+
           fStraightYHits(j)*(fZHits(i)-z0))*ErrorMatrix(i,j);
      }
    }
  }
  VariableCoefMatrix(0,1) = VariableCoefMatrix(1,0);
}

void AlgFitTrackMS::SetupAlg(AlgConfig &ac)
{
  fQ = 0;
  fFlag = 0;
  
  fSolnMatrix.ResizeTo(2,2);

  // for now - hard code in a lot of these constants.  much of this
  // will be changed later so it can be accessed from JobC.

  UgliGeomHandle ugh(*fCfh->GetVldContext());

  // fPosErr is the uncertainty on a position measurement.  this is
  // tricky because it would be easier to tell this uncertainty in a
  // u,v coordinate system.  and there are different uncertainties
  // depending on how the scintilator plane is oriented.  more work is 
  // needed here.
  fPosErr = ac.GetDouble("PosErr");
  
  // fXZero is radiation length of the steel
  fXZero = ac.GetDouble("XZero");
  
  // fdedx is the approximate energy loss per meter in steel.
  fDedx = ac.GetDouble("Dedx");
  
  // fSuperModGapSize is distance between 2 supermodules
  fSuperModGapSize = ac.GetDouble("SuperModGapSize");

  // fSuperModSkippedPlane is the plane skipped by the supermodule
  // (currently 250) and is used to test if the particle passes
  // through the supermodule.  Should the situation change to where
  // the supermodule does not skip any planes, the code would need to
  // be reworked slightly.  Should it change to where the supermodule
  // skips multiple planes, setting fSuperModSkippedPlane to any one of
  // the skipped planes will do.
  fSuperModSkippedPlane = ac.GetInt("SuperModSkippedPlane");

  // set fBFisFlipped != 0 (the default) if the BField map was made
  // with the current going the wrong way, making the magnetic field
  // wrong.  this is currently the case.
  fBFisFlipped = ac.GetInt("BFisFlipped");

  // if fNofit != 0, the actual fit is skipped for testing purposes.
  fNofit = ac.GetInt("Nofit");

  // if fNoBField !=0, the fit is done with bfield identically zero.
  fNoBField = ac.GetInt("NoBField");

  // if fNoMS !=0, the fit is done assuming no multiple scattering.
  fNoMS = ac.GetInt("NoMS");

  // if fBothFit !=0, do MS and BField at same time, otherwise do
  // seperately.
  fBothFit = ac.GetInt("BothFit");

  // if fFullAna !=0, fit in many different ways and save all kinds.
  fFullAna = ac.GetInt("FullAna");

  // runs out of memory over a certain number - fit only the first
  // MaxHits hits if the track has more.
  fMaxHits = ac.GetInt("MaxHits");

  // too few hits causes impossible fit - use length.
  fMinHits = ac.GetInt("MinHits");

  // very unlikely that an event with p above maxp would occur.  kill
  // fit if recommended momentum is above it
  fMaxP = ac.GetDouble("MaxP");

  // kill also if p drops below minp
  fMinP = ac.GetDouble("MinP");

  // Maximum number of iterations
  fMaxIter = ac.GetInt("MaxIter");

  // To what extent cut bad hits - high number keeps all - zero keeps 
  // fewest - 1 is more reasonable.
  fInShower = ac.GetInt("InShower");

  // tolerance of L before converges
  fLTolerance = ac.GetDouble("LTolerance"); 

  // tolerance of P before converges
  fPTolerance = ac.GetDouble("PTolerance");

  // set the detector type.
  fDetector = fCfh->GetVldContext()->GetDetector();

  Int_t done(0);
  
  PlexPlaneId planeid1(fDetector,1);
  PlexPlaneId splaneid(fDetector,1);
  splaneid.SetIsSteel(true);
  PlexPlaneId planeid2(fDetector,2);
  
  while(!done){

    UgliScintPlnHandle scintp1 = ugh.GetScintPlnHandle(planeid1);
    UgliScintPlnHandle scintp2 = ugh.GetScintPlnHandle(planeid2);
    
    if(scintp1.IsValid() && scintp2.IsValid()){

      done = true;

      fScintPlnWidth = scintp1.GetHalfThickness()*2.;
      fTotalPlnWidth = scintp2.GetZ0() - scintp1.GetZ0();
    }
    planeid1 = planeid2;
    planeid2 = planeid2.GetNext();
  }
  done = false;

  while(!done){

    UgliSteelPlnHandle steelp = ugh.GetSteelPlnHandle(splaneid);
 
    if(steelp.IsValid()){

      done = true;

      fSteelPlnWidth = steelp.GetHalfThickness()*2.;
    }
    splaneid = splaneid.GetNext();
  }
}
  
//______________________________________________________________________
void AlgFitTrackMS::WriteFit(Double_t ChiSquared, Double_t p0)
{
  // write the fit to the screen and to the objects

  // right now this doesnt always work because we fit for x and y and
  // take the best, while this always uses the last SolnMatrix
  // tested.  we should use something like a BestSolnMatrix.
  /*  
  MSG("FitTrackMS", Msg::kInfo) << "# of hits     = " <<
     fNHits << endl;
  MSG("FitTrackMS", Msg::kInfo) << "x intercept   = " <<
     fSolnMatrix(0,0) << endl;
  MSG("FitTrackMS", Msg::kInfo) << "x slope       = " <<
     fSolnMatrix(1,0) << endl;
  MSG("FitTrackMS", Msg::kInfo) << "y intercept   = " <<
     fSolnMatrix(0,1) << endl;
  MSG("FitTrackMS", Msg::kInfo) << "y slope       = " <<
     fSolnMatrix(1,1) << endl;
.q  MSG("FitTrackMS", Msg::kInfo) << "chi squared   = " <<
     ChiSquared << endl << endl;
  MSG("FitTrackMS", Msg::kInfo) << "tom's momentum      = " <<
     p0*Munits::GeV << " GeV" << endl << endl;
  */
  fCfh->SetDirCosU(fSolnMatrix(1,0));
  fCfh->SetVtxU(fSolnMatrix(0,0));
  fCfh->SetDirCosV(fSolnMatrix(1,1));
  fCfh->SetVtxV(fSolnMatrix(0,1));
  fCfh->SetChi2(ChiSquared/fNHits);
  fCfh->SetDirCosZ(1.);
  fCfh->SetVtxZ(fZHits(0));
  fCfh->SetVtxT(0.);
  fCfh->SetVtxPlane(0);
  fCfh->SetMomentum(p0);
  fCfh->SetChi2(ChiSquared);
  //  fCfh->SetEMCharge(fQ);

  MSG("FitTrackMS", Msg::kDebug) << "reco p = " << p0 << endl;
  
  // all this stuff writes TPolyLine3Ds to the fitTrackMS.root file so 
  // you can see what the fits and tracks are looking like.
  TPolyLine3D plHits(fNHits);
  TPolyLine3D plFit(fNHits);

  //  plFit.SetPoint(0,fCfh.GetVtxU(),fCfh.GetVtxV(),fZHits(0));

  Double_t x0, y0, mx, my;

  x0 = fCfh->GetVtxU();
  y0 = fCfh->GetVtxV();
  mx = fCfh->GetDirCosU();
  my = fCfh->GetDirCosV();

  for(Int_t i=0;i<fNHits;i++){
    
    plHits.SetPoint(i,fXHits(i),fYHits(i),fZHits(i));

    //    plFit.SetPoint(i,x0-fDThetas(i,0)+mx*(fZHits(i)-fZHits(0)),
    //		     y0-fDThetas(i,1)+my*(fZHits(i)-fZHits(0)),fZHits(i));
  }
  
  
  //    plFit.SetPoint(1,fCfh.GetVtxU()+(fZHits(fNHits-1)-fZHits(0))*fCfh.GetDirCosU(),
  //                     fCfh.GetVtxV()+(fZHits(fNHits-1)-fZHits(0))*fCfh.GetDirCosV(),
  //                     fZHits(fNHits-1));
  

//rwh: push this into a switchable code block
  bool writeit = false;
  if (writeit) {
    MSG("FitTrackMS",Msg::kInfo) 
      << "writing fitTrackMS.root file, "
      << " fNHits = " << fNHits << endl;
    TFile* f = new TFile("fitTrackMS.root","UPDATE");
    plHits.Write("hits");
    plFit.Write("fit");
    f->Close();
    MSG("FitTrackMS",Msg::kInfo) 
      << "done writing fitTrackMS.root file." << endl;
  }

}