#ifndef DAFUD_H
#define DAFUD_H

  /// DAF functions for conventinal KF covariance matrix

#include "FitFunctional.h"
#include "FitUD.h"
#include "DAFBase.h"

class DAFUD: public virtual FitFunctional, public FitUD, public DAFBase {

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

inline void DAFUD::FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &V, Fvec_t *chi2) const
{
    // H = 1
    // F = C'*H' = C'

    // zeta = r - Hm
  Fvec_t zeta[5];
  for( int i = 0; i < 5; ++i )
    zeta[i] = r[i] - m[i];
  
    // C = U*D*U'= U*DU
  CovVConventional cC = C;
  CovVConventional cV = V;
  CovVConventional S = cC + cV;
  InvertCholetsky( S );

  Fvec_t zetaS[5];
  Multiply(S, zeta, zetaS);
  if(chi2) {
    for( int i = 0; i < 5; ++i ) 
      *chi2 += zeta[i]*zetaS[i];
  }

  Fvec_t dr[5];
  Multiply(cC, zetaS, dr);
  for( int i = 0; i < 5; ++i )
    r[i] -= dr[i];

    // DUH = D*U'*H' = D*U'
  const Fvec_t DUH00 = C.D00;
  const Fvec_t DUH10 = C.D11*C.U01;
  const Fvec_t DUH20 = C.D22*C.U02;
  const Fvec_t DUH30 = C.D33*C.U03;
  const Fvec_t DUH40 = C.D44*C.U04;
  const Fvec_t DUH01 = ZERO;
  const Fvec_t DUH11 = C.D11;
  const Fvec_t DUH21 = C.D22*C.U12;
  const Fvec_t DUH31 = C.D33*C.U13;
  const Fvec_t DUH41 = C.D44*C.U14;
  const Fvec_t DUH02 = ZERO;
  const Fvec_t DUH12 = ZERO;
  const Fvec_t DUH22 = C.D22;
  const Fvec_t DUH32 = C.D33*C.U23;
  const Fvec_t DUH42 = C.D44*C.U24;
  const Fvec_t DUH03 = ZERO;
  const Fvec_t DUH13 = ZERO;
  const Fvec_t DUH23 = ZERO;
  const Fvec_t DUH33 = C.D33;
  const Fvec_t DUH43 = C.D44*C.U34;
  const Fvec_t DUH04 = ZERO;
  const Fvec_t DUH14 = ZERO;
  const Fvec_t DUH24 = ZERO;
  const Fvec_t DUH34 = ZERO;
  const Fvec_t DUH44 = C.D44;


  const Fvec_t SC00 = S.C00;
  const Fvec_t SC10 = S.C10;
  const Fvec_t SC20 = S.C20;
  const Fvec_t SC30 = S.C30;
  const Fvec_t SC40 = S.C40;
  const Fvec_t SC01 = S.C10;
  const Fvec_t SC11 = S.C11;
  const Fvec_t SC21 = S.C21;
  const Fvec_t SC31 = S.C31;
  const Fvec_t SC41 = S.C41;
  const Fvec_t SC02 = S.C20;
  const Fvec_t SC12 = S.C21;
  const Fvec_t SC22 = S.C22;
  const Fvec_t SC32 = S.C32;
  const Fvec_t SC42 = S.C42;
  const Fvec_t SC03 = S.C30;
  const Fvec_t SC13 = S.C31;
  const Fvec_t SC23 = S.C32;
  const Fvec_t SC33 = S.C33;
  const Fvec_t SC43 = S.C43;
  const Fvec_t SC04 = S.C40;
  const Fvec_t SC14 = S.C41;
  const Fvec_t SC24 = S.C42;
  const Fvec_t SC34 = S.C43;
  const Fvec_t SC44 = S.C44;
  
    // DUHS = DUH*S
#define Mul(A,B,C,i,j) const Fvec_t C##i##j = A##i##0*B##0##j + A##i##1*B##1##j + A##i##2*B##2##j + A##i##3*B##3##j + A##i##4*B##4##j
  Mul(DUH,SC,DUHS,0,0);
  Mul(DUH,SC,DUHS,0,1);
  Mul(DUH,SC,DUHS,0,2);
  Mul(DUH,SC,DUHS,0,3);
  Mul(DUH,SC,DUHS,0,4);
  Mul(DUH,SC,DUHS,1,0);
  Mul(DUH,SC,DUHS,1,1);
  Mul(DUH,SC,DUHS,1,2);
  Mul(DUH,SC,DUHS,1,3);
  Mul(DUH,SC,DUHS,1,4);
  Mul(DUH,SC,DUHS,2,0);
  Mul(DUH,SC,DUHS,2,1);
  Mul(DUH,SC,DUHS,2,2);
  Mul(DUH,SC,DUHS,2,3);
  Mul(DUH,SC,DUHS,2,4);
  Mul(DUH,SC,DUHS,3,0);
  Mul(DUH,SC,DUHS,3,1);
  Mul(DUH,SC,DUHS,3,2);
  Mul(DUH,SC,DUHS,3,3);
  Mul(DUH,SC,DUHS,3,4);
  Mul(DUH,SC,DUHS,4,0);
  Mul(DUH,SC,DUHS,4,1);
  Mul(DUH,SC,DUHS,4,2);
  Mul(DUH,SC,DUHS,4,3);
  Mul(DUH,SC,DUHS,4,4);
#undef Mul

    // G = D - DUHS*DUH'
#define Mul(A,B,i,j) (A##i##0*B##j##0 + A##i##1*B##j##1 + A##i##2*B##j##2 + A##i##3*B##j##3 + A##i##4*B##j##4)
  CovVConventional G;
  G.C00 = C.D00 - Mul(DUHS,DUH,0,0);
  G.C10 =       - Mul(DUHS,DUH,1,0);
  G.C20 =       - Mul(DUHS,DUH,2,0);
  G.C30 =       - Mul(DUHS,DUH,3,0);
  G.C40 =       - Mul(DUHS,DUH,4,0);
  G.C11 = C.D11 - Mul(DUHS,DUH,1,1);
  G.C21 =       - Mul(DUHS,DUH,2,1);
  G.C31 =       - Mul(DUHS,DUH,3,1);
  G.C41 =       - Mul(DUHS,DUH,4,1);
  G.C22 = C.D22 - Mul(DUHS,DUH,2,2);
  G.C32 =       - Mul(DUHS,DUH,3,2);
  G.C42 =       - Mul(DUHS,DUH,4,2);
  G.C33 = C.D33 - Mul(DUHS,DUH,3,3);
  G.C43 =       - Mul(DUHS,DUH,4,3);
  G.C44 = C.D44 - Mul(DUHS,DUH,4,4);
#undef Mul
  
  CovV udG = G;
  
  C.D00 = udG.D00;
  C.D11 = udG.D11;
  C.D22 = udG.D22;
  C.D33 = udG.D33;
  C.D44 = udG.D44;

  C.U34 +=         udG.U34;
  C.U24 += C.U23 * udG.U34 +         udG.U24;
  C.U23 +=         udG.U23;
  C.U14 += C.U13 * udG.U34 + C.U12 * udG.U24 +         udG.U14;
  C.U13 += C.U12 * udG.U23 +         udG.U13;
  C.U12 +=         udG.U12;
  C.U04 += C.U03 * udG.U34 + C.U02 * udG.U24 + C.U01 * udG.U14 + udG.U04;
  C.U03 += C.U02 * udG.U23 + C.U01 * udG.U13 +         udG.U03;
  C.U02 += C.U01 * udG.U12 +         udG.U02;
  C.U01 +=         udG.U01;

}
#else // single

inline void DAFUD::FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &sV, Fvec_t *chi2) const
{
    // H = 1
    // F = C'*H' = C'

    // zeta = r - Hm
  Fvec_t zeta[5];
  for( int i = 0; i < 5; ++i )
    zeta[i] = r[i] - m[i];
  
  
  Fvec_t dV[5], R[5][5];
  Diagonalize( CovVConventional(sV), dV, R ); // dV = R' cV R; cV -> dV; m -> R'm; m = R'r

    // H = R'

  for( int i = 0; i < 4; ++i ) // TODO
    for( int j = i+1; j < 5; ++j ) {
      Fvec_t tmp = R[i][j];
      R[i][j] = R[j][i];
      R[j][i] = tmp;
    }

    // proceed measurements one by one
  for ( int iM = 0; iM < 5; ++iM ) {
    const Fvec_t &V = dV[iM];
    const Fvec_t *H = &(R[iM][0]);
    Fvec_t zeta = 0;
    for( int i = 0; i < 5; ++i )
      zeta += H[i]*(r[i] - m[i]);

      // F = CH' = CUDU'H'
    Fvec_t F[5];
    {
        // DU = D*U'
        // C = U*D*U'= U*DU
      CovVConventional cC = C;

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

    Fvec_t HCH = 0;
    for( int i = 0; i < 5; ++i )
      HCH += F[i]*H[i];

    const Fvec_t S = 1./( V + HCH );

    const Fvec_t zetaS = zeta * S;
    if (chi2) *chi2 += zeta*zetaS;
    for( int i = 0; i < 5; ++i )
      r[i] -= F[i]*zetaS;

      // DUH = D*U'*H' = D*U'
    const Fvec_t DUH0 = C.D00*H[0];
    const Fvec_t DUH1 = C.D11*(C.U01*H[0] + H[1]);
    const Fvec_t DUH2 = C.D22*(C.U02*H[0] + C.U12*H[1] + H[2]);
    const Fvec_t DUH3 = C.D33*(C.U03*H[0] + C.U13*H[1] + C.U23*H[2] + H[3]);
    const Fvec_t DUH4 = C.D44*(C.U04*H[0] + C.U14*H[1] + C.U24*H[2] + C.U34*H[3] + H[4]);

    const Fvec_t SDUH0 = S*DUH0;
    const Fvec_t SDUH1 = S*DUH1;
    const Fvec_t SDUH2 = S*DUH2;
    const Fvec_t SDUH3 = S*DUH3;
    const Fvec_t SDUH4 = S*DUH4;
  
      // C = D - S*DUH*DUH'
      // convert: const L1TrackParUD UD0 = C;
    C.D44 = (C.D44 - SDUH4*DUH4);
    const Fvec_t iD44 = 1.f/C.D44;
    const Fvec_t DU43 = (      - SDUH4*DUH3);
    const Fvec_t DU42 = (      - SDUH4*DUH2);
    const Fvec_t DU41 = (      - SDUH4*DUH1);
    const Fvec_t DU40 = (      - SDUH4*DUH0);
    const Fvec_t UD34 = DU43*iD44;
    const Fvec_t UD24 = DU42*iD44;
    const Fvec_t UD14 = DU41*iD44;
    const Fvec_t UD04 = DU40*iD44;

    C.D33 = (C.D33 - SDUH3*DUH3) - UD34*DU43;
    const Fvec_t iD33 = 1.f/C.D33;
    const Fvec_t DU32 = (      - SDUH3*DUH2) - UD34*DU42;
    const Fvec_t DU31 = (      - SDUH3*DUH1) - UD34*DU41;
    const Fvec_t DU30 = (      - SDUH3*DUH0) - UD34*DU40;
    const Fvec_t UD23 = DU32*iD33;
    const Fvec_t UD13 = DU31*iD33;
    const Fvec_t UD03 = DU30*iD33;

    C.D22 = (C.D22 - SDUH2*DUH2) - UD23*DU32 - UD24*DU42;
    const Fvec_t iD22 = 1.f/C.D22;
    const Fvec_t DU21 = (      - SDUH2*DUH1) - UD23*DU31 - UD24*DU41;
    const Fvec_t DU20 = (      - SDUH2*DUH0) - UD23*DU30 - UD24*DU40;
    const Fvec_t UD12 = DU21*iD22;
    const Fvec_t UD02 = DU20*iD22;

    C.D11 = (C.D11 - SDUH1*DUH1) - UD12*DU21 - UD13*DU31 - UD14*DU41;
    const Fvec_t DU10 = (      - SDUH1*DUH0) - UD12*DU20 - UD13*DU30 - UD14*DU40;
    const Fvec_t UD01 = DU10/C.D11;

    C.D00 = (C.D00 - SDUH0*DUH0) - UD01*DU10 - UD02*DU20 - UD03*DU30 - UD04*DU40;


    C.U34 +=         UD34;
    C.U24 += C.U23 * UD34 +         UD24;
    C.U23 +=         UD23;
    C.U14 += C.U13 * UD34 + C.U12 * UD24 +         UD14;
    C.U13 += C.U12 * UD23 +         UD13;
    C.U12 +=         UD12;
    C.U04 += C.U03 * UD34 + C.U02 * UD24 + C.U01 * UD14 + UD04;
    C.U03 += C.U02 * UD23 + C.U01 * UD13 +         UD03;
    C.U02 += C.U01 * UD12 +         UD02;
    C.U01 +=         UD01;
    
  } // iM
  
}
#endif // UD_FILTERING_V


#endif