// MutKalmanTrack.C 
//
// Kalman track for muon tracker: implementation file
//---------------------------------------------------------------------

#include <iostream>

#include "TClonesArray.h"
#include "TMatrixD.h"
#include "TVectorD.h"
#include "TRandom.h"

#include "dMutArrayParams.h" // to be included in "dMuoTracksClass.h"????
#include "dMuoTracksClass.h"
#include "MutKalmanTrack.h"
#include "MutKalmanTrackFitParam.h"
#include "MutKalmanTrackHit.h"

#define MUPLUS 5
#define MUMINUS 6


// Prototypes for product of 3 matrices: internal methods
TMatrixD* AtBCptr(TMatrixD A, TMatrixD B, TMatrixD C);
TMatrixD AtBC(TMatrixD* A, TMatrixD* B, TMatrixD* C);
TMatrixD ABtC(TMatrixD* A, TMatrixD* B, TMatrixD* C);
TMatrixD* ABCtptr(TMatrixD A, TMatrixD B, TMatrixD C);
//TMatrixD* ABCt(TMatrixD* A, TMatrixD* B, TMatrixD* C);

ClassImp(MutKalmanTrack) // Class implementation in ROOT context

  //__________________________________________________________________________
 MutKalmanTrack::MutKalmanTrack(const MutKalmanTrack& Track)
{
  // Copy constructor
  // Track parameters at vertex
  fTrackParamAtVertex = Track.fTrackParamAtVertex;
  // TClonesArray for track hits, with 10 hits at the beginning
  fTrackHitsPtr = new TClonesArray("MutKalmanTrackHit", 10);
  (Track.fTrackHitsPtr)->Compress(); // to make easier the copy loop
  MutKalmanTrackHit *oldTrackHit;
  for (Int_t hit = 0; hit < (Track.fTrackHitsPtr)->GetEntriesFast(); hit++) {
    oldTrackHit =
      (MutKalmanTrackHit*) ((Track.fTrackHitsPtr)->UncheckedAt(hit));
    new ((*fTrackHitsPtr)[hit]) MutKalmanTrackHit(*oldTrackHit);
  }
  fChi2 = Track.fChi2;
  return;
}

  //__________________________________________________________________________
MutKalmanTrack& MutKalmanTrack::operator=(const MutKalmanTrack& Track)
{
  // Assignment operator
  // Delete the objects pointed to, if not empty
  if (fTrackHitsPtr) {fTrackHitsPtr->Delete(); delete fTrackHitsPtr;}
  // Track parameters at vertex
  fTrackParamAtVertex = Track.fTrackParamAtVertex;
  // TClonesArray for track hits, with 10 hits at the beginning
  fTrackHitsPtr = new TClonesArray("MutKalmanTrackHit", 10);
  (Track.fTrackHitsPtr)->Compress(); // to make easier the copy loop
  MutKalmanTrackHit *oldTrackHit;
  for (Int_t hit = 0; hit < (Track.fTrackHitsPtr)->GetEntriesFast(); hit++) {
    oldTrackHit =
      (MutKalmanTrackHit*) ((Track.fTrackHitsPtr)->UncheckedAt(hit));
    new ((*fTrackHitsPtr)[hit]) MutKalmanTrackHit(*oldTrackHit);
  }
  fChi2 = Track.fChi2;
  // Return the pointer to the object
  return (*this);
}

  //__________________________________________________________________________
 MutKalmanTrack::~MutKalmanTrack()
{
  // Destructor
  fTrackHitsPtr->Delete();
  delete fTrackHitsPtr;
  fTrackHitsPtr = NULL;
  return;
}

  //__________________________________________________________________________
 MutKalmanTrack::MutKalmanTrack(Int_t TrackNumber, TABLE_HEAD_ST* Mumhits_h, MUMHITS_ST* Mumhits, Double_t SigmaR, Double_t SigmaRPhi)
{
  // Constructor from the Geant track with track number "TrackNumber".
  // One needs the PISA hit table, and more explicitly:
  // * a pointer to the header of this table, "Mumhits_h",
  // * a pointer to this table, "Mumhits",
  // * the Gaussian error for R, "SigmaR",
  // * R times the Gaussian error for Phi, "SigmaRPhi"
  Double_t cosPhi, phi, phiTrue, r, rTrue, sCur, sinPhi,
    xPrime, xTrue, yPrime, yTrue;
  Int_t charge = 0;
  Int_t numberOfPisaHits, pisaHitNumber, trackHitNumber;
  MUMHITS_ST* pGeantHit;
  MutKalmanTrackHit *trackHit;
  TMatrixD covMeas(2,2);
  TMatrixD dMeasDXY(2,2);
  Int_t printLevel = (MutKalmanTrackFitParam::Instance())->GetPrintLevel();
  // The field "fTrackParamAtVertex" is constructed automatically.
  // TClonesArray for track hits, with 10 hits at the beginning
  fTrackHitsPtr = new TClonesArray("MutKalmanTrackHit", 10);
  // Loop over the hits in the table
  // access to "Mumhits_h" data through "get" methods????
  numberOfPisaHits = Mumhits_h->nok; // number of hits in the table
  if (printLevel >= 1) {
    cout << "Geant track number : " << TrackNumber
	 << " from PISA " << numberOfPisaHits << " hits" << endl;
  }
  trackHitNumber = 0;
  for (pisaHitNumber = 0; pisaHitNumber < numberOfPisaHits; pisaHitNumber++) {
    pGeantHit = &(Mumhits[pisaHitNumber]);
    // access to "pGeantHit" data through "get" methods????
    // Test whether the hit belongs to the track and the track is a muon track
    if ((pGeantHit->track == TrackNumber) &&
	((pGeantHit->pid == MUPLUS) || (pGeantHit->pid == MUMINUS))) {
      // Yes: one adds one hit to the track
      if (pGeantHit->pid == MUPLUS) charge = 1;
      if (pGeantHit->pid == MUMINUS) charge = -1;
      trackHit = new ((*fTrackHitsPtr)[trackHitNumber]) MutKalmanTrackHit(2);
      // with the right data members
      trackHit->SetTrack(this);
      // Z
      trackHit->SetZ(pGeantHit->x[2]);
      // X and Y from X1=R and Y1=RPhi with random values around the true ones
      // according to the covariance matrix
      xTrue = pGeantHit->x[0];
      yTrue = pGeantHit->x[1];
      rTrue = TMath::Sqrt(xTrue * xTrue + yTrue * yTrue);
      phiTrue = TMath::ATan2(yTrue, xTrue);
      r = gRandom->Gaus(rTrue, SigmaR);
      phi = gRandom->Gaus(phiTrue, (SigmaRPhi / rTrue));
      cosPhi = TMath::Cos(phi);
      sinPhi = TMath::Sin(phi);
//       trackHit->SetMeas(r * cosPhi, r * sinPhi);
      // Measurements = Y1, X1
      Double_t y1 = r * (sinPhi * TMath::Cos(phiTrue) -
			 cosPhi * TMath::Sin(phiTrue));
      Double_t x1 = r * (sinPhi * TMath::Sin(phiTrue) +
			 cosPhi * TMath::Cos(phiTrue));
//       trackHit->SetMeas(rTrue * (phi-phiTrue), r);
      trackHit->SetMeas2(y1, x1);
      // Covariance matrix for measurements
//       // use matrix multiplication????
//       covMeas(0,0) = (xTrue * xTrue * SigmaR * SigmaR +
// 		    yTrue * yTrue * SigmaRPhi * SigmaRPhi) / rTrue / rTrue;
//       covMeas(0,1) = covMeas(1,0) = xTrue * yTrue *
// 	(SigmaR * SigmaR - SigmaRPhi * SigmaRPhi) / rTrue / rTrue;
//       covMeas(1,1) = (yTrue * yTrue * SigmaR * SigmaR +
// 		    xTrue * xTrue * SigmaRPhi * SigmaRPhi) / rTrue / rTrue;
      covMeas(0,0) = SigmaRPhi * SigmaRPhi;
      covMeas(0,1) = covMeas(1,0) = 0.0;
      covMeas(1,1) = SigmaR * SigmaR;
      trackHit->SetCovMeas(&covMeas);
      // Matrix for derivatives of measurements with respect to X and Y
      dMeasDXY(0,0) = -TMath::Sin(phiTrue); // d(R*Phi)/dX
      dMeasDXY(0,1) = TMath::Cos(phiTrue); // d(R*Phi)/dY
      dMeasDXY(1,0) = dMeasDXY(0,1); // dR/dX
      dMeasDXY(1,1) = -dMeasDXY(0,0); // dR/dY
      trackHit->SetDMeasDXY(&dMeasDXY);
      // True parameters
      sCur = 1.0 / (charge * TMath::Sqrt(pGeantHit->p[0] * pGeantHit->p[0] +
					 pGeantHit->p[1] * pGeantHit->p[1] +
					 pGeantHit->p[2] * pGeantHit->p[2]));
      xPrime = pGeantHit->p[0] / pGeantHit->p[2];
      yPrime = pGeantHit->p[1] / pGeantHit->p[2];
      trackHit->SetTrueParam(sCur, xPrime, yPrime, xTrue, yTrue);
      if (printLevel >= 1) {
	cout << "KalmanTrack hit number : " << trackHitNumber
	     << " from PISA hit number : " << pisaHitNumber << endl;
	cout << "X = " << pGeantHit->x[0]
	     << "; Y = " << pGeantHit->x[1]
	     << "; Z = " << pGeantHit->x[2] << endl;
	cout << "t = " << pGeantHit->t << "; e = " << pGeantHit->e << endl;
	cout << "pX = " << pGeantHit->p[0]
	     << "; pY = " << pGeantHit->p[1]
	     << "; pZ = " << pGeantHit->p[2] << endl;
	cout << "track = " << pGeantHit->track
	     << "; pid = " << pGeantHit->pid
	     << "; plane(A:1N-2S/O:1-8/S:1-3/P:1-3) = "
	     << pGeantHit->plane << endl;
      }
      // increment trackHitNumber
      trackHitNumber++;
    }
  }
  fChi2 = 0.0;
  return;
}

  //__________________________________________________________________________
 MutKalmanTrack::MutKalmanTrack(long int NHits, float Xmeas[MAXPLANES][3], float CathodeAngle[MAXPLANES], float Res[MAXPLANES][2])
{
  // Constructor from the same information
  // as given to Fortran subroutine "mum_geafit"
  Double_t cosCathAngle, sig2, sinCathAngle;
  Int_t hitNumber;
  MutKalmanTrackHit *trackHit;
  TMatrixD *covMeas, *dMeasDXY;
  // TClonesArray for track hits, with "NHits" hits
  fTrackHitsPtr = new TClonesArray("MutKalmanTrackHit", NHits);
  // Loop over hits
  for (hitNumber = 0; hitNumber < NHits; hitNumber++) {
    // track hit added to the track, with 2 measurements
    trackHit = new ((*fTrackHitsPtr)[hitNumber]) MutKalmanTrackHit(2);
    // with the right data members
    trackHit->SetTrack(this);
    // Z
    trackHit->SetZ(Double_t(Xmeas[hitNumber][2]));
    // measurements, in the same order as for "mum_geafit",
    // i.e. index = 0 (1) for "bad" ("good") measurement
    trackHit->SetMeas2(Double_t(Xmeas[hitNumber][0]),
		       Double_t(Xmeas[hitNumber][1]));
    // variance of measurements
    covMeas = trackHit->GetCovMeas();
    sig2 = Double_t(Res[hitNumber][0]);
    (*covMeas)(0, 0) = sig2 * sig2;
    sig2 = Double_t(Res[hitNumber][1]);
    (*covMeas)(1, 1) = sig2 * sig2;
    // derivatives of measurements with respect to X and Y????
    dMeasDXY = trackHit->GetDMeasDXY();
    cosCathAngle = TMath::Cos(Double_t(CathodeAngle[hitNumber]));
    sinCathAngle = TMath::Sin(Double_t(CathodeAngle[hitNumber]));
    (*dMeasDXY)(0, 0) = cosCathAngle;
    (*dMeasDXY)(0, 1) = sinCathAngle;
    (*dMeasDXY)(1, 0) = -sinCathAngle;
    (*dMeasDXY)(1, 1) = cosCathAngle;
    // true parameters????
  }
  fChi2 = 0.0;
  return;
}

  //__________________________________________________________________________
// MutKalmanTrack::MutKalmanTrack(dMuoTracksClass* MuoTrack)
// // MutKalmanTrack::MutKalmanTrack(dMuoTracksClass* MuoTrack, dMutCathodeClustersN_ST* dMutCathodeClustersN[NumberOfArms][NumberOfStations][NumberOfOctants][NumberOfHalfOctants][3][NumberOfCathodePlanes], DMUTCSCGEOM_ST* dMutCSCGeom)
// {
//   // Constructor from one "dMuoTracksClass" object
//   // Problems if one wants to use dMutCathodeClustersN in argument!!!!
//   Int_t arm, nMuoHits, kalmanHitNumber;
//   MutKalmanTrackHit *kalmanHit;
//   // pointer to structure for TrackHits
//   TrackHits_ST* muoHits = &(MuoTrack->TrackHits);
//   // number of hits in MuoTrack
//   nMuoHits = muoHits->nhitstot;
//   // What to do if not enough hits,
//   // as compared to dMuoFindTracksPar->min_track_hits????
//   // Arm number
//   arm = MuoTrack->arm;
//   // What to do if invalid arm number????
//   // TClonesArray for track hits, with 16 hits at the beginning
//   fTrackHitsPtr = new TClonesArray("MutKalmanTrackHit", 16);
//   // Loop over hits in MuoTrack
//   kalmanHitNumber = 0;
//   // loop over stations
//   for (Int_t st = 0; st < NumberOfStations; st++) {
//     // loop over track hits in station
//     Int_t nStHits = MuoTrack->TrackHits.nhits[st];
//     cout << nStHits << " hits in station " << st << endl;
//     for (Int_t stHit = 0; stHit < nStHits; stHit++) {
//       // nothing if not cathode signal
//       if (MuoTrack->TrackHits.signaltype[stHit][st] != 1) continue;
//       // Add a new MutKalmanTrackHit
//       kalmanHit = new ((*fTrackHitsPtr)[kalmanHitNumber]) MutKalmanTrackHit(1);
//       // with the right data members
//       kalmanHit->SetTrack(this);
//       // filled from this track hit in this station in MuoTrack
//       kalmanHit->FillWithHitFromDMuoTracksClass(MuoTrack, st, stHit);
//       kalmanHitNumber++;
//     }
//   }
//   fChi2 = 0.0;
//   return;
// }

  //__________________________________________________________________________
 void MutKalmanTrack::Fit(MutKalmanTrackParam *StartParam, Double_t StartZ)
{
  // Complete fit of the current MutKalmanTrack
  // Argument for starting parameters????
  // Return argument for success/failure????
  // Assumes that the MutKalmanTrack hits are sorted
  // according to increasing values of abs(Z)
  Filter(StartParam, StartZ); // Filter
  Smoother(); // Smoother
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrack::Filter(MutKalmanTrackParam *StartParam, Double_t StartZ)
{
  // Kalman filter on the current MutKalmanTrack
  // Argument for starting parameters????
  //    (parameters and covariance/weight matrix)
  // Return argument for success/failure????
  // Assumes that the MutKalmanTrack hits are sorted
  // according to increasing values of abs(Z)

  Int_t printLevel = (MutKalmanTrackFitParam::Instance())->GetPrintLevel();
  fChi2 = 0.0; // important for successive fits
  MutKalmanTrackHit *hit = this->LastHit();
  // Loop over track hits from the last one
  // sorting hits according to abs(Z)????
  MutKalmanTrackParam *previousParam = StartParam;
  Double_t previousZ = StartZ;
  if (printLevel >= 2) {
    cout << endl;
    cout << "**MutKalmanTrack::Filter" << endl;
    cout << "StartZ = " << StartZ << endl;
    cout << "StartParam" << endl;
    StartParam->Print();
  }

  while (hit) {
    // Predicted parameters,
    //   and first derivatives or covariance matrix,
    // should be returned by something like ERTRAK

    MutKalmanTrackHit* prevHit = hit->Prev();
    if (!hit) continue;
    // extrapolation
    MutKalmanTrackParam* extrapParam = (previousParam)->
      ExtrapBackward(previousZ, hit->GetZ(),
		     hit->GetPropagatorParDeriv());
    
    // assuming that we have the full covariance matrix
    
    ////////////////////////////
    // Prediction
    // Ppred = F * Pfit (Pfit of the previous plan)
    // C_(k/k-1) = F_(k-1) * C_(k-1) * F_(k-1)^{Trans} + MS_(k-1) calculeted in MutKalmanTrackHit
    TMatrixD Pfit(5, 1); // Fitted state vector
    TMatrixDColumn(Pfit,0)       = *(previousParam->GetParameters()); // Convertion type TVector in type TMatrix
    TMatrixD Ppred(5, 1); // Predicted state vector
    TMatrixDColumn(Ppred,0)       = *(extrapParam->GetParameters()); // Convertion type TVector in type TMatrix
    (hit->GetTrackParam(0))->SetParameters(&Ppred); // transfert of the predicted state vector in MutKalmanTrackHit
    (hit->GetTrackParam(0))->SetCovarianceMatrix(extrapParam->GetCovarianceMatrix()); // transfert of the predicted state vector in MutKalmanTrackHit

    //////////////////////////////////////////////////////////  
    // Theorical Fitted state vector (Weighted means formalism)
    //   Pfit = (C_(k/k-1)^{-1} + H^{Trans} * G * H)^{-1} * (C_(k/k-1)^{-1} * Ppred + H^{Trans} * G * Meas)
    TMatrixD CInv (TMatrixD::kInverted,
		   *(hit->GetTrackParam(0)->GetCovarianceMatrix())); // Inverse of the transport covariance Matrix with multi scattering
    TMatrixD Meas (hit->GetNMeas(), 1); // Measured vector
    TMatrixDColumn(Meas,0)       = *(hit->GetMeas());
    TMatrixD H (hit->GetNMeas(), 5);   // H Linear Matrix which ought to passe from the work space to measur space, 
//     H(0, 3) = 1.0;
//     H(1, 4) = 1.0;
    // simpler with submatrix????
    // faster if 0 in H taken into account explicitly in formal calculation????
    TMatrixD *dMeasDXY = hit->GetDMeasDXY();
    for (Int_t meas = 0; meas < hit->GetNMeas(); meas++) {
      H(meas, 3) = (*dMeasDXY)(meas, 0);
      H(meas, 4) = (*dMeasDXY)(meas, 1);
      //     H(0, 3) = (*dMeasDXY)(0, 0);
      //     H(0, 4) = (*dMeasDXY)(0, 1);
      //     H(1, 3) = (*dMeasDXY)(1, 0);
      //     H(1, 4) = (*dMeasDXY)(1, 1);
    }
    TMatrixD G (TMatrixD::kInverted, *(hit->GetCovMeas())); // Inverse of the covariance matrix
    TMatrixD CP (CInv, TMatrixD::kMult, Ppred);
    TMatrixD HGM                 = AtBC (&H, &G, &Meas);
    CP                          += HGM;
    TMatrixD HGH = AtBC (&H, &G, &H);
    HGH                         += CInv;
    TMatrixD *Cnew               = new TMatrixD(TMatrixD::kInverted, HGH); // Update of the covariance for the next step
    (hit->GetTrackParam(1))->SetCovarianceMatrix(Cnew); // transfert of the update convariance matrix in MutKalmanTrackHit
    Pfit                         = TMatrixD(*Cnew, TMatrixD::kMult, CP);
    (hit->GetTrackParam(1))->SetParameters(&Pfit); // transfert of the fitted state vector in MutKalmanTrackHit
 
    /////////////////////////////////////////////////////
    // Theorical value of the Chi2 incrementation
    //  DeltaChi2 = (Pfit - Pexp)^{Trans}     * C^{-1} *  (Pfit - Pexp)
    //            + (H * Pfit - Meas)^{Trans} *   G    *  (H * Pfit - Meas);
    TMatrixD DiffPP(5, 1);
    DiffPP                       = Pfit;
    DiffPP                      -= Ppred;
    TMatrixD HP (H, TMatrixD::kMult, Pfit);
    TMatrixD DiffPM (hit->GetNMeas(), 1);
    DiffPM                       =  HP;
    DiffPM                      -= Meas;
    TMatrixD *Chi2P          = AtBCptr (DiffPP, CInv, DiffPP);
    hit->SetChi2P(Chi2P);
    TMatrixD *Chi2M          = AtBCptr (DiffPM, G, DiffPM);
    hit->SetChi2M(Chi2M);
    AddToChi2(hit->GetChi2M(), hit->GetChi2P());

    // Prepare next step
    previousParam                = hit->GetTrackParam(1);
    previousZ                    = hit->GetZ();
    hit                          = prevHit;

    delete Cnew;
    Cnew =  NULL;
    delete extrapParam;
    extrapParam = NULL;
  }
  if (printLevel >= 2) {
    cout << endl;
    cout << "**MutKalmanTrack::Filter" << endl;
    cout << "MutKalmanTrack printout after Filter, before Smoother" << endl;
    this->Print();
  }
  return;
}

//__________________________________________________________________________
 void MutKalmanTrack::Smoother()
{
  // Kalman smoother on the current MutKalmanTrack
  // Return argument for success/failure????
  // Assumes that the MutKalmanTrack hits are sorted
  // according to increasing values of abs(Z)

  MutKalmanTrackHit *prevHit = this->FirstHit();
  // For first hit: smoother result = filter result
  MutKalmanTrackParam *prevParam = prevHit->GetTrackParam(2);
  *prevParam = *(prevHit->GetTrackParam(1));
  MutKalmanTrackHit *hit = prevHit->Next();

  // We are in the hit (k+1) and we are going to the hit k
  while (hit) {

    ///////////////////////////////
    // Smooth state vector
    // Psmo_{k} = Pfit_{k} + Cnew * FTrans * C_{k/k+1}^{-1} * (Psmo_{k+1} - Ppred_{k/k+1})
    // Cn/k = Cnew + Ak * (Cn/k+1 - Ck/k+1)  Ak^{Trans}
    // Ak = Cnew * FTrans * Cnew^{-1}
    TMatrixD Pfit(5, 1); // Fitted state vector
    TMatrixDColumn(Pfit,0)       = *(hit->GetTrackParam(1))->GetParameters(); // Convertion type TVector in type TMatrix
    TMatrixD CInv (TMatrixD::kInverted, *(prevHit->GetTrackParam(0)->GetCovarianceMatrix())); // Inverse of the transport covariance Matrix with multi scattering
    TMatrixD Psmo (5, 1); // Smooth state vector
    TMatrixDColumn(Psmo,0)       = *((prevHit->GetTrackParam(2))->GetParameters()); // Convertion type TVector in type TMatrix
    
    TMatrixD Ppred (5, 1);
    TMatrixDColumn(Ppred,0) = *((prevHit->GetTrackParam(0))->GetParameters()); // Predicted value of the state vector
    TMatrixD CFC = ABtC (((hit->GetTrackParam(1))->GetCovarianceMatrix()), (prevHit->GetPropagatorParDeriv()), &CInv);

    TMatrixD DiffPP (5, 1);
    DiffPP                       = Psmo;
    DiffPP                      -= Ppred;
    TMatrixD CFCDiffPP (CFC, TMatrixD::kMult, DiffPP);
    
    Psmo                         = Pfit;
    Psmo                        += CFCDiffPP;
    
//     TVectorD *PsmoV = new TVectorD(5);
//     *PsmoV                       = TMatrixDColumn(Psmo,0);
    (hit->GetTrackParam(2))->SetParameters(&Psmo); // Update of the smoothe state vector

    TMatrixD *DiffMat = new TMatrixD(*prevParam->GetCovarianceMatrix());
    *DiffMat -= *(prevHit->GetTrackParam(0))->GetCovarianceMatrix();
    TMatrixD *Cov = ABCtptr (CFC, *DiffMat, CFC);
    *Cov += *(hit->GetTrackParam(1))->GetCovarianceMatrix();
    (hit->GetTrackParam(2))->SetCovarianceMatrix(Cov); // Update of the covariance matrix of the smoothed state vector
    
    // Prepare next step
    prevParam                    = hit->GetTrackParam(2);
    prevHit = hit;
    hit                          = hit->Next();

    delete DiffMat;
    DiffMat = NULL;
    delete Cov;
    Cov = NULL;
  }

  return;
}

  //__________________________________________________________________________
 void MutKalmanTrack::Print()
{
  // Print method
  cout << "***** MutKalmanTrack::Print" << endl;
  cout << "TrackParamAtVertex";
  fTrackParamAtVertex.Print();
  cout << "fChi2 = " << fChi2 << endl;
  // Loop over track hits
  MutKalmanTrackHit *hit = this->FirstHit();
  while (hit) {
    hit->Print();
    hit = hit->Next();
  }
  return;
}

  //__________________________________________________________________________
TMatrixD AtBC(TMatrixD* A, TMatrixD* B, TMatrixD* C)
{
  TMatrixD AtB (*A, TMatrixD::kTransposeMult, *B);
  return (TMatrixD (AtB, TMatrixD::kMult, *C));
}

TMatrixD* AtBCptr(TMatrixD A, TMatrixD B, TMatrixD C)
{
  TMatrixD AtB (A, TMatrixD::kTransposeMult, B);
  return (new TMatrixD (AtB, TMatrixD::kMult, C));
}

//   //__________________________________________________________________________
// TMatrixD* AtBC(TMatrixD *A, TMatrixD *B, TMatrixD *C)
// {
//   TMatrixD *AtB  = new TMatrixD(*A, TMatrixD::kTransposeMult, *B);
//   return (new TMatrixD(*AtB, TMatrixD::kMult, *C));
// }

  //__________________________________________________________________________
TMatrixD ABtC(TMatrixD* A, TMatrixD* B, TMatrixD* C)
{
  TMatrixD BtC(*B, TMatrixD::kTransposeMult, *C);
  return (TMatrixD (*A, TMatrixD::kMult, BtC));
}


  //__________________________________________________________________________
//TMatrixD* ABCt(TMatrixD *A, TMatrixD *B, TMatrixD *C)
//{
//  TMatrixD *Ct  = new TMatrixD(TMatrixD::kTransposed, *C);
//  TMatrixD *AB = new TMatrixD(*A, TMatrixD::kMult, *B);
//  return (new TMatrixD(*AB, TMatrixD::kMult, Ct));
//}

TMatrixD* ABCtptr(TMatrixD A, TMatrixD B, TMatrixD C)
{
  TMatrixD Ct  (TMatrixD::kTransposed, C);
  TMatrixD BCt  (B, TMatrixD::kMult, Ct);
  return (new TMatrixD (A, TMatrixD::kMult, BCt));
}