////////////////////////////////////////////////////////////////////////
// $Id: KalmanPlaneSR.cxx,v 1.28 2005/03/24 19:50:36 brebel Exp $
//
// KalmanPlaneSR
//
// Author:  R. Lee 2001.04.03
// Revised: B. Rebel 2003.07.15
//
////////////////////////////////////////////////////////////////////////

#include "CandFitTrackSR/KalmanPlaneSR.h"
//#include "./KalmanPlaneSR.h"

#include <iostream>
#include <string>

#include "Algorithm/AlgConfig.h"
#include "TVector3.h"                       // ROOT TVector class
#include "CandTrackSR/TrackClusterSR.h"
#include "Conventions/PlaneView.h"
#include "Conventions/Mphysical.h"
#include "Conventions/Munits.h"
#include "MessageService/MsgFormat.h"
#include "MessageService/MsgService.h"
#include "RecoBase/CandStripHandle.h"
#include "UgliGeometry/UgliGeomHandle.h"
#include "Validity/VldContext.h"

ClassImp(KalmanPlaneSR)

//______________________________________________________________________
CVSID("$Id: KalmanPlaneSR.cxx,v 1.28 2005/03/24 19:50:36 brebel Exp $");

//______________________________________________________________________
KalmanPlaneSR::KalmanPlaneSR() : 
  fTrackCluster(0)
{
  // default ctor
  InitKalman();

}

//______________________________________________________________________
KalmanPlaneSR::KalmanPlaneSR(TrackClusterSR *tcluster)
{
  fTrackCluster = tcluster;
  InitKalman();
}

//______________________________________________________________________
KalmanPlaneSR::KalmanPlaneSR(KalmanPlaneSR *kp)
{
  fTrackCluster = kp->GetTrackCluster();
  Init(kp);
}


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

//______________________________________________________________________
void KalmanPlaneSR::InitKalman()
{
  for(Int_t i=0; i<2; ++i){
    fPreState[i].ResizeTo(5);
    fFilState[i].ResizeTo(5);
    fGain[i].ResizeTo(5);
    fPropagator[i].ResizeTo(5,5);
    fPreCovariance[i].ResizeTo(5,5);
    fFilCovariance[i].ResizeTo(5,5);
    fNoise[i].ResizeTo(5,5);
    for(Int_t j=0; j<5; ++j){
      fPropagator[i](j,j) = 1.;
      fPreCovariance[i](j,j) = 1.;
      fFilCovariance[i](j,j) = 1.;
    }
    fPredictPlane[i] = -999;
    fPreChi2[i] = 0.;
    fFilChi2[i] = 0.;
  }
  fZDir = 1;
  fRange = 0.;
}

//______________________________________________________________________
void KalmanPlaneSR::Init(KalmanPlaneSR *kp)
{
  InitKalman();
 
  for(int i=0; i<2; ++i){
    fPreState[i] = kp->fPreState[i];
    fFilState[i] = kp->fFilState[i];
    fGain[i] = kp->fGain[i];
    fPropagator[i] = kp->fPropagator[i];
    fPreCovariance[i] = kp->fPreCovariance[i];
    fFilCovariance[i] = kp->fFilCovariance[i];
    fNoise[i] = kp->fNoise[i];
    fPreChi2[i] = kp->fPreChi2[i];
    fFilChi2[i] = kp->fFilChi2[i];
    fPredictPlane[i] = kp->fPredictPlane[i];
  }
  fZDir = kp->GetZDir();
  fRange = kp->GetRange();

}

//----------------------------------------------------------------------
const VldContext *KalmanPlaneSR::GetVldContext()
{
  if(!dynamic_cast<CandStripHandle*>
     (fTrackCluster->GetStripList()->First())->GetVldContext()) 
    MSG("FitTrackSR", Msg::kWarning) << "No VldContext for KalmanPlaneSR" << endl;
  return dynamic_cast<CandStripHandle*>(fTrackCluster->GetStripList()->First())->GetVldContext();
}

//______________________________________________________________________
Int_t KalmanPlaneSR::GetPredictPlane(Int_t idir) const
{
  return fPredictPlane[idir];
}

//______________________________________________________________________
void KalmanPlaneSR::SetPredictPlane(Int_t plane, Int_t idir)
{
  fPredictPlane[idir] = plane;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetPreStateValue(Int_t stateIndex, Int_t idir) const
{
  return fPreState[idir](stateIndex);
}
  
//______________________________________________________________________
void KalmanPlaneSR::SetPreStateValue(Int_t stateIndex, Int_t idir, 
									 Double_t stateValue)
{
  fPreState[idir](stateIndex) = stateValue;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetFilStateValue(Int_t stateIndex, Int_t idir) const
{
  return fFilState[idir](stateIndex);
}
  
//______________________________________________________________________
void KalmanPlaneSR::SetFilStateValue(Int_t stateIndex, Int_t idir,
				     Double_t stateValue)
{
  fFilState[idir](stateIndex) = stateValue;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetGainValue(Int_t gainIndex, Int_t idir) const
{
  return fGain[idir](gainIndex);
}
  
//______________________________________________________________________
void KalmanPlaneSR::SetGainValue(Int_t gainIndex, Int_t idir, 
				 Double_t gainValue)
{
  fGain[idir](gainIndex) = gainValue;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetPropagatorMatrixValue(Int_t idir, Int_t row,
						  Int_t column) const
{
  return fPropagator[idir](row, column);
}

//______________________________________________________________________
void KalmanPlaneSR::SetPropagatorMatrixValue(Int_t idir, Int_t row, 
					     Int_t column, Double_t value)
{
  fPropagator[idir](row,column) = value;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetPreCovarianceMatrixValue(Int_t idir, Int_t row,
						     Int_t column) const
{
  return fPreCovariance[idir](row, column);
}

//______________________________________________________________________
void KalmanPlaneSR::SetPreCovarianceMatrixValue(Int_t idir, Int_t row, 
						Int_t column, Double_t value)
{
  fPreCovariance[idir](row,column) = value;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetFilCovarianceMatrixValue(Int_t idir, Int_t row,
						     Int_t column) const
{
  return fFilCovariance[idir](row, column);
}

//______________________________________________________________________
void KalmanPlaneSR::SetFilCovarianceMatrix(Int_t idir, Double_t setMatrix[][5])
{
  
  for(Int_t i = 0; i<5; ++i){
    for(Int_t j = 0; j<5; ++j){
      fFilCovariance[idir](i,j) = setMatrix[i][j];
    }
  }

  return;
}

//______________________________________________________________________
void KalmanPlaneSR::SetFilCovarianceMatrixValue(Int_t idir, Int_t row, 
						Int_t column, Double_t value)
{
  fFilCovariance[idir](row,column) = value;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetNoiseMatrixValue(Int_t idir, Int_t row, 
					     Int_t column) const
{
  return fNoise[idir](row, column);
}

//______________________________________________________________________
void KalmanPlaneSR::SetNoiseMatrixValue(Int_t idir, Int_t row, 
					Int_t column, Double_t value)
{
  fNoise[idir](row,column) = value;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetPreChi2(Int_t idir) const
{
  return fPreChi2[idir];
}

//______________________________________________________________________
void KalmanPlaneSR::SetPreChi2(Double_t chi2, Int_t idir)
{
  fPreChi2[idir] = chi2;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetFilChi2(Int_t idir) const
{
  return fFilChi2[idir];
}

//______________________________________________________________________
void KalmanPlaneSR::SetFilChi2(Double_t chi2, Int_t idir)
{
  fFilChi2[idir] = chi2;
  return;
}

//______________________________________________________________________
Int_t KalmanPlaneSR::GetZDir() const
{
  return fZDir;
}

//______________________________________________________________________
void KalmanPlaneSR::SetZDir(Int_t idir)
{
  fZDir = idir;
  return;
}

//______________________________________________________________________
Double_t KalmanPlaneSR::GetRange() const
{
  return fRange;
}

//______________________________________________________________________
void KalmanPlaneSR::SetRange(Double_t range)
{
  fRange = range;
  return;
}

//______________________________________________________________________
TrackClusterSR* KalmanPlaneSR::GetTrackCluster() const
{
  return fTrackCluster;
}

//______________________________________________________________________
void KalmanPlaneSR::SetTrackCluster(TrackClusterSR *cluster)
{
  fTrackCluster = cluster;
  return;
}


//______________________________________________________________________
#define KP_TEST_ARRAY_EQUALITY(__rhs, __arr_member, __arr_len)      \
{                                                                   \
  for(int __i=0; __i < __arr_len; __i++) {                          \
    if( !(this->__arr_member[__i] == __rhs->__arr_member[__i]) ) {  \
      return false;                                                 \
    }                                                               \
  }                                                                 \
}

#define KP_TEST_EQUALITY(__rhs, __member)                           \
{                                                                   \
  if( !(this->__member == __rhs->__member) ) {                      \
    return false;                                                   \
  }                                                                 \
}

//______________________________________________________________________
Bool_t KalmanPlaneSR::IsEquivalent(const TObject *rkp) const
{
  const KalmanPlaneSR *rhs = dynamic_cast<const KalmanPlaneSR*>(rkp);

  KP_TEST_ARRAY_EQUALITY(rhs, fPreState,      2);
  KP_TEST_ARRAY_EQUALITY(rhs, fFilState,      2);
  KP_TEST_ARRAY_EQUALITY(rhs, fGain,          2);
  KP_TEST_ARRAY_EQUALITY(rhs, fPropagator,    2);
  KP_TEST_ARRAY_EQUALITY(rhs, fPreCovariance, 2);
  KP_TEST_ARRAY_EQUALITY(rhs, fFilCovariance, 2);
  KP_TEST_ARRAY_EQUALITY(rhs, fNoise,         2);
  KP_TEST_ARRAY_EQUALITY(rhs, fPreChi2,       2);
  KP_TEST_ARRAY_EQUALITY(rhs, fFilChi2,       2);
  KP_TEST_ARRAY_EQUALITY(rhs, fPredictPlane,  2);
  KP_TEST_EQUALITY(      rhs, fZDir            );
  KP_TEST_EQUALITY(      rhs, fRange           );

  return true;
}

XXXITRIMP(KalmanPlaneSR)