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

#include <iostream>

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

#include "dMutArrayParams.h" // to be included in "dMuoTracksClass.h"????
#include "dMuoTracksClass.h"
// #include "dMutCathodeClustersN.h"
// #include "dMutCSCGeom.h"

#include "MutKalmanTrackHit.h"

ClassImp(MutKalmanTrackHit) // Class implementation in ROOT context

  //__________________________________________________________________________
 MutKalmanTrackHit::MutKalmanTrackHit(Int_t NMeas)
{
  // Default constructor with "NMeas" measurements
  fTrack = NULL; // ????
  fZ = 0.0;
  fNMeas = NMeas;
  fMeas = new TVectorD(NMeas);
  fCovMeas = new TMatrixD(NMeas, NMeas);
  fDMeasDXY = new TMatrixD(NMeas, 2);
  fPropagatorParDeriv = new TMatrixD(5, 5);
  fTrueParam = new TVectorD(5);
  fChi2M = 0.0;
  fChi2P = 0.0;
  return;
}

  //__________________________________________________________________________
 MutKalmanTrackHit::MutKalmanTrackHit(const MutKalmanTrackHit& TrackHit)
{
  // Copy constructor
  fTrack = TrackHit.fTrack; // ????
  fZ = TrackHit.fZ;
  fNMeas = TrackHit.fNMeas;
  fMeas = new TVectorD(*(TrackHit.fMeas));
  fCovMeas = new TMatrixD(*(TrackHit.fCovMeas));
  fDMeasDXY = new TMatrixD(*(TrackHit.fDMeasDXY));
  fPropagatorParDeriv = new TMatrixD(*(TrackHit.fPropagatorParDeriv));
  for (Int_t pfs = 0; pfs < 3; pfs++) {
    fTrackParam[pfs] = TrackHit.fTrackParam[pfs];
  }
  fTrueParam = new TVectorD(*(TrackHit.fTrueParam));
  fChi2M = TrackHit.fChi2M;
  fChi2P = TrackHit.fChi2P;
  return;
}

  //__________________________________________________________________________
MutKalmanTrackHit& MutKalmanTrackHit::operator=(const MutKalmanTrackHit& TrackHit)
{
  // Assignment operator
  // Delete the objects pointed to, if not empty
  if (fMeas) delete fMeas;
  if (fCovMeas) delete fCovMeas;
  if (fDMeasDXY) delete fDMeasDXY;
  if (fPropagatorParDeriv) delete fPropagatorParDeriv;
  if (fTrueParam) delete fTrueParam;
  // Copy the object content
  fTrack = TrackHit.fTrack; // ????
  fZ = TrackHit.fZ;
  fNMeas = TrackHit.fNMeas;
  fMeas = new TVectorD(*(TrackHit.fMeas));
  fCovMeas = new TMatrixD(*(TrackHit.fCovMeas));
  fDMeasDXY = new TMatrixD(*(TrackHit.fDMeasDXY));
  fPropagatorParDeriv = new TMatrixD(*(TrackHit.fPropagatorParDeriv));
  for (Int_t pfs = 0; pfs < 3; pfs++) {
    fTrackParam[pfs] = TrackHit.fTrackParam[pfs];
  }
  fTrueParam = new TVectorD(*(TrackHit.fTrueParam));
  fChi2M = TrackHit.fChi2M;
  fChi2P = TrackHit.fChi2P;
  // Return the pointer to the object
  return (*this);
}

  //__________________________________________________________________________
 MutKalmanTrackHit::~MutKalmanTrackHit()
{
  // Destructor
  delete fMeas;
  fMeas = NULL;
  delete fCovMeas;
  fCovMeas = NULL;
  delete fDMeasDXY;
  fDMeasDXY = NULL;
  delete fPropagatorParDeriv;
  fPropagatorParDeriv = NULL;
  delete fTrueParam;
  fTrueParam = NULL;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetMeas1(Double_t Meas)
{
  // Set (first) measurement to "Meas"
  (*fMeas)(0) = Meas;
  return;
}


  //__________________________________________________________________________
 void MutKalmanTrackHit::SetMeas1Z(Double_t Meas, Double_t Z)
{
  // Set (first) measurement to "Meas" and Z coordinate to "Z"
    (*fMeas)(0) = Meas;
    fZ = Z;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetMeas2(Double_t Meas0, Double_t Meas1)
{
  // Set 2 measurements to "Meas0" and "Meas1" respectively
  (*fMeas)(0) = Meas0;
  (*fMeas)(1) = Meas1;
  return;
}


  //__________________________________________________________________________
 void MutKalmanTrackHit::SetMeas2Z(Double_t Meas0, Double_t Meas1, Double_t Z)
{
  // Set 2 measurements to "Meas0" and "Meas1" respectively,
  // and Z coordinate to "Z"
    (*fMeas)(0) = Meas0;
    (*fMeas)(1) = Meas1;
    fZ = Z;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetCovMeas(TMatrixD* CovMeas)
{
  *fCovMeas = *CovMeas;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetDMeasDXY(TMatrixD* DMeasDXY)
{
  *fDMeasDXY = *DMeasDXY;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetTrueParam(Double_t X, Double_t Y, Double_t Px, Double_t Py, Double_t Pz)
{
  TVectorD *trueParam = new TVectorD(0, 4, X, Y, Px, Py, Pz, "END");
  *fTrueParam = *trueParam;
  delete trueParam;
  return;
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetChi2P(TMatrixD* Chi2P)
{
  fChi2P = (*Chi2P)(0, 0);
}

  //__________________________________________________________________________
 void MutKalmanTrackHit::SetChi2M(TMatrixD* Chi2M)
{
  fChi2M = (*Chi2M)(0, 0);
}

  //__________________________________________________________________________
// void MutKalmanTrackHit::FillWithHitFromDMuoTracksClass(dMuoTracksClass* MuoTrack, Int_t stNumber, Int_t stHitNumber)
// {
//   // Fill this MutKalmanTrackHit with information from
//   // hit number "stHitNumber"
//   // in station number "stNumber"
//   // in dMuoTracksClass object pointed to by "MuoTrack"
//   Double_t cathAngle, cosCathAngle, offsetCath, sinCathAngle;
//   Int_t planeIndex;
//   // arm (0-1)
//   Int_t arm = MuoTrack->arm;
//   // octant (0-7)
//   Int_t octant = MuoTrack->TrackHits.octant[stHitNumber][stNumber];
//   // halfoctant (0-1)
//   Int_t halfoctant = MuoTrack->TrackHits.halfoctant[stHitNumber][stNumber];
//   // gap number (0-2) in station
//   Int_t gap = MuoTrack->TrackHits.gap[stHitNumber][stNumber];
//   // cathode plane (0-1) in gap
//   Int_t cathPlane = MuoTrack->TrackHits.cath[stHitNumber][stNumber];
//   // index for cathode cluster
//   Int_t iClus = MuoTrack->TrackHits.ihit[stHitNumber][stNumber];
//   // index for fitted subcluster
//   Int_t iFit = MuoTrack->TrackHits.ifit[stHitNumber][stNumber];
// //   // pointer to "dMutCathodeClustersN" structure
// //   CathodeClust_ST *clus =
// //     dMutCathodeClustersN[arm][stNumber][octant][halfoctant][gap][cathPlane]
// //     ->clust[iClus];
// //   // Z
// //   planeIndex = arm * 18 + stNumber * 6 + (octant % 2) * 3 + gap;
// //   fZ = dMutCSCGeom[0].plane_zpos[planeIndex];
// //   // "good" measurement
// //   offsetCath = clus->offset[iFit];
// //   if (clus->xyflag){
// //     // reference line y = m * x + b
// //     (*fMeas)(0) = offsetCath * cosCathAngle;
// //   }
// //   else{ 
// //     // reference line x = m * y + b
// //     (*fMeas)(0) = -offsetCath * sinCathAngle;
// //   }
// //   // covariance of "good" measurement
// //   (*fCovMeas)(0) = clus->res[iFit];
// //   // derivatives of "good" measurement with respect to X and Y
// //   (*fDMeasDXY)(0, 0) = -sinCathAngle;
// //   (*fDMeasDXY)(0, 1) = cosCathAngle;
//   return;
// }

  //__________________________________________________________________________
 void MutKalmanTrackHit::Print()
{
  // Print method
  cout << "***** MutKalmanTrackHit::Print" << endl;
  cout << "fTrack = " << fTrack << endl;
  cout << "fZ = " << fZ << endl;
  cout << "fMeas";
  fMeas->Print();
  cout << "fCovMeas";
  fCovMeas->Print();
  cout << "fDMeasDXY";
  fDMeasDXY->Print();
  cout << "fPropagatorParDeriv";
  fPropagatorParDeriv->Print();
  cout << "fTrackParam[0]";
  fTrackParam[0].Print();
  cout << "fTrackParam[1]";
  fTrackParam[1].Print();
  cout << "fTrackParam[2]";
  fTrackParam[2].Print();
  cout << "fTrueParam";
  fTrueParam->Print();
  cout << "fChi2M = " << fChi2M << endl;
  cout << "fChi2P = " << fChi2P << endl;
  return;
}