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

#include <iostream>

#include "TVectorD.h"
#include "TMatrixD.h"

#include "MutKalmanTrackParam.h"
#include "mut_kalman_track_param_extrapP.h"

ClassImp(MutKalmanTrackParam) // Class implementation in ROOT context

  //__________________________________________________________________________
 MutKalmanTrackParam::MutKalmanTrackParam()
{
  // Default constructor
  
  fParameters = new TVectorD(5);
  fCovarianceMatrix = new TMatrixD(5, 5);
  return;
}

  //__________________________________________________________________________
 MutKalmanTrackParam::MutKalmanTrackParam(const MutKalmanTrackParam& TrackParam)
{
  // Copy constructor
  fParameters = new TVectorD(*(TrackParam.fParameters));
  fCovarianceMatrix = new TMatrixD(*(TrackParam.fCovarianceMatrix));
  return;
}

  //__________________________________________________________________________
MutKalmanTrackParam& MutKalmanTrackParam::operator=(const MutKalmanTrackParam& TrackParam)
{
  // Assignment operator
  // Delete the objects pointed to, if not empty
  if (fParameters) delete fParameters;
  if (fCovarianceMatrix) delete fCovarianceMatrix;
  // Copy the object content
  fParameters = new TVectorD(*(TrackParam.fParameters));
  fCovarianceMatrix = new TMatrixD(*(TrackParam.fCovarianceMatrix));
  // Return the pointer to the object
  return (*this);
}

  //__________________________________________________________________________
 MutKalmanTrackParam::~MutKalmanTrackParam()
{
  // Destructor
  delete fParameters;
  fParameters = NULL;
  delete fCovarianceMatrix;
  fCovarianceMatrix = NULL;
  return;
}

  //__________________________________________________________________________
 MutKalmanTrackParam* MutKalmanTrackParam::ExtrapBackward(Double_t Zin, Double_t Zout, TMatrixD* DPoutDPin)
{
  // Backward extrapolation (energy gain instead of energy loss)
  // from current MutKalmanTrackParam object ("this") at "Zin",
  // with track parameters and covariance matrix,
  // to created (returned) MutKalmanTrackParam object at "Zout",
  // with track parameters and covariance matrix,
  // including the effect of the multiple Coulomb scattering.
  // Returns also the matrix of the derivatives of the "out" parameters
  // with respect to the "in" parameters,
  // with the first index for "out" and the second index for "out".
  // The user should not forget to delete the created object
  // after using it, to avoid memory leak.
  // Call to Fortran subroutine with the right arguments
  // to allow it to call the GEANT subroutines EUFILP and ERTRAK
  // Something special for Zin=Zout (2 cathode planes)????
  MutKalmanTrackParam *param =  new MutKalmanTrackParam();
  if (Zin == Zout) {
    // Zin equal to Zout: simple extrapolation
    *param = *this;
    DPoutDPin->UnitMatrix();
  }
  else {
    // Zin equal to Zout: real extrapolation
    mut_kalman_track_param_extrap_
      (Zin,
       &((*(this->GetParameters()))(0)),
       &((*(this->GetCovarianceMatrix()))(0, 0)),
       Zout,
       &((*(param->GetParameters()))(0)),
       &((*(param->GetCovarianceMatrix()))(0, 0)),
       &((*DPoutDPin)(0, 0)));
  }
  return param;
}

  //__________________________________________________________________________
 void MutKalmanTrackParam::SetParameters(TMatrixD* Param)
{
  TVectorD *PfitV              = new TVectorD(5);
  *PfitV                       = TMatrixDColumn(*Param,0);
  *fParameters = *PfitV;
}

  //__________________________________________________________________________
 void MutKalmanTrackParam::Print()
{
  // Print method
  cout << "***** MutKalmanTrackParam::Print" << endl;
  cout << "fParameters";
  fParameters->Print();
  cout << "fCovarianceMatrix";
  fCovarianceMatrix->Print();
  return;
}