#ifndef DAFC_H
#define DAFC_H

  /// DAF functions for conventinal KF covariance matrix

#include "FitFunctional.h"
#include "FitC.h"
#include "DAFBase.h"

class DAFC: public virtual FitFunctional, public FitC, public DAFBase {

 protected:
  void FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &V, Fvec_t *chi2 = 0) const;

 private:
  void Multiply(const CovV &C, const CovV &V, MatV &K) const;
  void Multiply(const MatV &C, const CovV &V, CovV &K) const;
};

inline void DAFC::Multiply(const CovV &C, const CovV &V, MatV &K) const
{
  K.C[0][0] = C.C00*V.C00 + C.C10*V.C10 + C.C20*V.C20 + C.C30*V.C30 + C.C40*V.C40;
  K.C[0][1] = C.C00*V.C10 + C.C10*V.C11 + C.C20*V.C21 + C.C30*V.C31 + C.C40*V.C41;
  K.C[0][2] = C.C00*V.C20 + C.C10*V.C21 + C.C20*V.C22 + C.C30*V.C32 + C.C40*V.C42;
  K.C[0][3] = C.C00*V.C30 + C.C10*V.C31 + C.C20*V.C32 + C.C30*V.C33 + C.C40*V.C43;
  K.C[0][4] = C.C00*V.C40 + C.C10*V.C41 + C.C20*V.C42 + C.C30*V.C43 + C.C40*V.C44;

  K.C[1][0] = C.C10*V.C00 + C.C11*V.C10 + C.C21*V.C20 + C.C31*V.C30 + C.C41*V.C40;
  K.C[1][1] = C.C10*V.C10 + C.C11*V.C11 + C.C21*V.C21 + C.C31*V.C31 + C.C41*V.C41;
  K.C[1][2] = C.C10*V.C20 + C.C11*V.C21 + C.C21*V.C22 + C.C31*V.C32 + C.C41*V.C42;
  K.C[1][3] = C.C10*V.C30 + C.C11*V.C31 + C.C21*V.C32 + C.C31*V.C33 + C.C41*V.C43;
  K.C[1][4] = C.C10*V.C40 + C.C11*V.C41 + C.C21*V.C42 + C.C31*V.C43 + C.C41*V.C44;

  K.C[2][0] = C.C20*V.C00 + C.C21*V.C10 + C.C22*V.C20 + C.C32*V.C30 + C.C42*V.C40;
  K.C[2][1] = C.C20*V.C10 + C.C21*V.C11 + C.C22*V.C21 + C.C32*V.C31 + C.C42*V.C41;
  K.C[2][2] = C.C20*V.C20 + C.C21*V.C21 + C.C22*V.C22 + C.C32*V.C32 + C.C42*V.C42;
  K.C[2][3] = C.C20*V.C30 + C.C21*V.C31 + C.C22*V.C32 + C.C32*V.C33 + C.C42*V.C43;
  K.C[2][4] = C.C20*V.C40 + C.C21*V.C41 + C.C22*V.C42 + C.C32*V.C43 + C.C42*V.C44;

  K.C[3][0] = C.C30*V.C00 + C.C31*V.C10 + C.C32*V.C20 + C.C33*V.C30 + C.C43*V.C40;
  K.C[3][1] = C.C30*V.C10 + C.C31*V.C11 + C.C32*V.C21 + C.C33*V.C31 + C.C43*V.C41;
  K.C[3][2] = C.C30*V.C20 + C.C31*V.C21 + C.C32*V.C22 + C.C33*V.C32 + C.C43*V.C42;
  K.C[3][3] = C.C30*V.C30 + C.C31*V.C31 + C.C32*V.C32 + C.C33*V.C33 + C.C43*V.C43;
  K.C[3][4] = C.C30*V.C40 + C.C31*V.C41 + C.C32*V.C42 + C.C33*V.C43 + C.C43*V.C44;

  K.C[4][0] = C.C40*V.C00 + C.C41*V.C10 + C.C42*V.C20 + C.C43*V.C30 + C.C44*V.C40;
  K.C[4][1] = C.C40*V.C10 + C.C41*V.C11 + C.C42*V.C21 + C.C43*V.C31 + C.C44*V.C41;
  K.C[4][2] = C.C40*V.C20 + C.C41*V.C21 + C.C42*V.C22 + C.C43*V.C32 + C.C44*V.C42;
  K.C[4][3] = C.C40*V.C30 + C.C41*V.C31 + C.C42*V.C32 + C.C43*V.C33 + C.C44*V.C43;
  K.C[4][4] = C.C40*V.C40 + C.C41*V.C41 + C.C42*V.C42 + C.C43*V.C43 + C.C44*V.C44;
}

inline void DAFC::Multiply(const MatV &C, const CovV &V, CovV &K) const
{
  K.C00 = C.C[0][0]*V.C00 + C.C[0][1]*V.C10 + C.C[0][2]*V.C20 + C.C[0][3]*V.C30 + C.C[0][4]*V.C40;

  K.C10 = C.C[1][0]*V.C00 + C.C[1][1]*V.C10 + C.C[1][2]*V.C20 + C.C[1][3]*V.C30 + C.C[1][4]*V.C40;
  K.C11 = C.C[1][0]*V.C10 + C.C[1][1]*V.C11 + C.C[1][2]*V.C21 + C.C[1][3]*V.C31 + C.C[1][4]*V.C41;

  K.C20 = C.C[2][0]*V.C00 + C.C[2][1]*V.C10 + C.C[2][2]*V.C20 + C.C[2][3]*V.C30 + C.C[2][4]*V.C40;
  K.C21 = C.C[2][0]*V.C10 + C.C[2][1]*V.C11 + C.C[2][2]*V.C21 + C.C[2][3]*V.C31 + C.C[2][4]*V.C41;
  K.C22 = C.C[2][0]*V.C20 + C.C[2][1]*V.C21 + C.C[2][2]*V.C22 + C.C[2][3]*V.C32 + C.C[2][4]*V.C42;

  K.C30 = C.C[3][0]*V.C00 + C.C[3][1]*V.C10 + C.C[3][2]*V.C20 + C.C[3][3]*V.C30 + C.C[3][4]*V.C40;
  K.C31 = C.C[3][0]*V.C10 + C.C[3][1]*V.C11 + C.C[3][2]*V.C21 + C.C[3][3]*V.C31 + C.C[3][4]*V.C41;
  K.C32 = C.C[3][0]*V.C20 + C.C[3][1]*V.C21 + C.C[3][2]*V.C22 + C.C[3][3]*V.C32 + C.C[3][4]*V.C42;
  K.C33 = C.C[3][0]*V.C30 + C.C[3][1]*V.C31 + C.C[3][2]*V.C32 + C.C[3][3]*V.C33 + C.C[3][4]*V.C43;

  K.C40 = C.C[4][0]*V.C00 + C.C[4][1]*V.C10 + C.C[4][2]*V.C20 + C.C[4][3]*V.C30 + C.C[4][4]*V.C40;
  K.C41 = C.C[4][0]*V.C10 + C.C[4][1]*V.C11 + C.C[4][2]*V.C21 + C.C[4][3]*V.C31 + C.C[4][4]*V.C41;
  K.C42 = C.C[4][0]*V.C20 + C.C[4][1]*V.C21 + C.C[4][2]*V.C22 + C.C[4][3]*V.C32 + C.C[4][4]*V.C42;
  K.C43 = C.C[4][0]*V.C30 + C.C[4][1]*V.C31 + C.C[4][2]*V.C32 + C.C[4][3]*V.C33 + C.C[4][4]*V.C43;
  K.C44 = C.C[4][0]*V.C40 + C.C[4][1]*V.C41 + C.C[4][2]*V.C42 + C.C[4][3]*V.C43 + C.C[4][4]*V.C44;
}

inline void DAFC::FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &V, Fvec_t *chi2) const
{
    // TODO check the difference between Cij and Vij. If it is huge, the algorithm is unstable

    // H = 1
    // S = (V + HCH')^-1
  CovV S = V + C;
  InvertCholetsky(S);

    // K = CH'S
  MatV K;
  Multiply(C,S,K);

   // dzeta = m - r
  Fvec_t dzeta[5];
  for(int i=0; i<5; i++)
    dzeta[i] = m[i] - r[i];

   // C -> C - KHC
  CovV KC;
  Multiply(K,C,KC);
  C -= KC;

    // r -> r + K*dzeta
  Fvec_t kd;
  for(int i=0; i<5; i++)
  {
    kd = ZERO;
    for(int j=0; j<5; j++)
      kd += K.C[i][j]*dzeta[j];
    r[i] += kd;
  }

   // chi2 -> chi2 + dzeta'*S*dzeta
  if(chi2)
  {
    Fvec_t S_dzeta[5];
    DAFBase::Multiply(S, dzeta, S_dzeta);
    *chi2 = dzeta[0]*S_dzeta[0] + dzeta[1]*S_dzeta[1] + dzeta[2]*S_dzeta[2] + dzeta[3]*S_dzeta[3] + dzeta[4]*S_dzeta[4];
  }
}






#endif