#ifndef DAFBase_H
#define DAFBase_H

  /// Common functions which is used to implement SmootherFitFunctional in every KF-approach.

#include <assert.h>
#include <math.h>
#include "FitFunctional.h"

cnst INF_p = 10000.f;

static const int NIterDAF = 4;
static const int NIterDAF_tabl = 6;
cnst T_DAF [NIterDAF_tabl] = {9, 4, 1, 0.1, 1, 0.1};
cnst T_DAFi[NIterDAF_tabl] = {ONE/(2.*T_DAF[0]),
                              ONE/(2.*T_DAF[1]),
                              ONE/(2.*T_DAF[2]),
                              ONE/(2.*T_DAF[3]),
                              ONE/(2.*T_DAF[4]),
                              ONE/(2.*T_DAF[5])};
cnst chi2_cut[NIterDAF_tabl] = {16., 16., 16., 16., 16., 16.};

class DAFBase: public virtual FitFunctional { // base class for all approaches
 public:
  
  void Smooth(TrackV &t, Station vStations[], int NStations, bool EmptyHit = 1, bool useWeights = 0) const;
  void SmoothHit(int iHit, TrackV &t, Station vStations[], int NStations, bool EmptyHit = 1) const;
  void RunDAF(TrackV &t, Station vStations[], int NStations) const;
  
 protected:
    // combain two tracks into one
  virtual void FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &V, Fvec_t *chi2 = 0) const = 0;
  void UpdateWeights(TrackV &t, Station vStations[], int NStations, int const &IterDAF) const;

#if defined(UD_FILTERING_S) || defined(SQUARE_ROOT1S) || defined(SQUARE_ROOT1)
  void Diagonalize(const CovVConventional& C, Fvec_t D[5], Fvec_t V[5][5]) const;
#endif
  
#if defined(UD_FILTERING_V) || defined(CONVENTIONAL)
  void InvertCholetsky(CovVConventional &CM) const;
  void Multiply(const CovVConventional &C, const Fvec_t *r_in, Fvec_t *r_out) const;
#endif
};

inline void DAFBase::Smooth(TrackV &t, Station vStations[], int NStations, bool EmptyHit, bool useWeights) const
{
  Fvec_t ws[MaxNStations];
  if (useWeights) { 
    for( int i = 0; i < NStations; ++i )
      ws[i] = t.vHits[i].w * t.w[i];

    for(int iT=0; iT<6; iT++)  // used in DAF, so should be already initialized
      t.T[iT] = t.Ts[NStations-1][iT];
    t.C = t.Cs[NStations-1];
  }
  else {
    for( int i = 0; i < NStations; ++i )
      ws[i] = t.vHits[i].w;
  
    GuessVec( t, vStations, NStations, 0 );
  }
    // downstream

  FieldRegion f;
  Fvec_t z0,z1,z2, dz;
  FieldVector H0, H1, H2;

  Fvec_t qp0 = t.T[4];
  Int_t i= NStations-1;
  HitV *h = &t.vHits[i];

  FilterFirst( t, *h, vStations[i] );
  AddMaterial( t, vStations[ i ], qp0 );

  z1 = vStations[ i ].z;
  vStations[i].Map.GetField(t.T[0],t.T[1], H1);
  H1.Combine( h->H, h->w );

  z2 = vStations[ i-2 ].z;
  dz = z2-z1;
  vStations[ i-2 ].Map.GetField(t.T[0]+t.T[2]*dz,t.T[1]+t.T[3]*dz,H2);
  h = &t.vHits[i-2];
  H2.Combine( h->H, h->w );

  for( --i; i>=0; i-- ){
    
    h = &t.vHits[i];
    Station &st = vStations[i];
    z0 = st.z;    
    dz = (z1-z0);
    st.Map.GetField(t.T[0]-t.T[2]*dz,t.T[1]-t.T[3]*dz,H0);
    H0.Combine( h->H, h->w );
    f.Set( H0, z0, H1, z1, H2, z2);
    ExtrapolateALight( t.T, t.C, st.zhit, qp0, f );
    if (i == 1) {
      AddMaterial( t, st, qp0, true ); // pipe
    }
    else
      AddMaterial( t, st, qp0 );

    if(EmptyHit)
    {
      for(int iT=0; iT<6; iT++)
        t.Ts[i][iT] = t.T[iT];
      t.Cs[i] = t.C;
        //      t.Chi2s[i] = t.Chi2;
        //      t.NDFs[i] = t.NDF;
    }
     
    Fvec_t u = h->x*st.UInfo.cos_phi + h->y*st.UInfo.sin_phi;
    Fvec_t v = h->x*st.VInfo.cos_phi + h->y*st.VInfo.sin_phi;
    Filter( t, st.UInfo, u, ws[i] );
    Filter( t, st.VInfo, v, ws[i] );
    H2 = H1; 
    z2 = z1;
    H1 = H0; 
    z1 = z0;
    if(!EmptyHit)
    {
      for(int iT=0; iT<6; iT++)
        t.Ts[i][iT] = t.T[iT];
      t.Cs[i] = t.C;
    }
  }
  t.fs[0] = f;

    // upstream
  
  qp0 = t.T[4];
  i = 0;
  h = &t.vHits[i];

  FilterFirst( t, *h, vStations[i] );

  AddMaterial( t, vStations[ i ], qp0 );

  z1 = vStations[ i ].z;
  vStations[i].Map.GetField(t.T[0],t.T[1], H1);
  H1.Combine( h->H, h->w );

  z2 = vStations[ i+2 ].z;
  dz = z2-z1;
  vStations[ i+2 ].Map.GetField(t.T[0]+t.T[2]*dz,t.T[1]+t.T[3]*dz,H2);
  h = &t.vHits[i+2];
  H2.Combine( h->H, h->w );

  for( ++i; i<NStations; i++ ){
      //if(i>1) continue;
    h = &t.vHits[i];
    Station &st = vStations[i];
    z0 = st.z;    
    dz = (z1-z0);
    st.Map.GetField(t.T[0]-t.T[2]*dz,t.T[1]-t.T[3]*dz,H0);
    H0.Combine( h->H, h->w );
    f.Set( H0, z0, H1, z1, H2, z2);
    t.fs[i] = f;

    ExtrapolateALight( t.T, t.C, st.zhit, qp0, f );

    if (i < NStations-1)
      if (  i >= NStations/2 ) // add short to longer
        FilterTracks(t.Ts[i], t.Cs[i], t.T, t.C, &t.Chi2s[i]);
      else {
        Fvec_t T[6];
        CovV C = t.C;
        for( int k = 0; k < 5; k++)
          T[k] = t.T[k];
        FilterTracks(T, C, t.Ts[i], t.Cs[i], &t.Chi2s[i]);
        for( int k = 0; k < 5; k++)
          t.Ts[i][k] = T[k];
        t.Cs[i] = C;
      }

    else if (EmptyHit) {
      for(int iT=0; iT<6; iT++)
        t.Ts[NStations-1][iT] = t.T[iT];
      t.Cs[NStations-1] = t.C;
    }
    
    if (i == 1)
      AddMaterial( t, st, qp0, true ); // pipe
    else
      AddMaterial( t, st, qp0 );

    Fvec_t u = h->x*st.UInfo.cos_phi + h->y*st.UInfo.sin_phi;
    Fvec_t v = h->x*st.VInfo.cos_phi + h->y*st.VInfo.sin_phi;
    Filter( t, st.UInfo, u, ws[i] );
    Filter( t, st.VInfo, v, ws[i] );
    H2 = H1; 
    z2 = z1;
    H1 = H0; 
    z1 = z0;
  }

  if (!EmptyHit) {
    for(int iT=0; iT<6; iT++)
      t.Ts[NStations-1][iT] = t.T[iT];
    t.Cs[NStations-1] = t.C;
  }
} // inline void DAFBase::Smooth


inline void DAFBase::SmoothHit(int iHit, TrackV &t, Station vStations[], int NStations, bool EmptyHit) const
{
  GuessVec( t, vStations,NStations );

  // fit downstream to the hit we are interested in

  FieldRegion f;
  Fvec_t z0,z1,z2, dz;
  FieldVector H0, H1, H2;

  Fvec_t qp0 = t.T[4];
  Int_t i= NStations-1;
  HitV *h = &t.vHits[i];

  FilterFirst( t, *h, vStations[i] );
  AddMaterial( t, vStations[ i ], qp0 );

  z1 = vStations[ i ].z;
  vStations[i].Map.GetField(t.T[0],t.T[1], H1);
  H1.Combine( h->H, h->w );

  z2 = vStations[ i-2 ].z;
  dz = z2-z1;
  vStations[ i-2 ].Map.GetField(t.T[0]+t.T[2]*dz,t.T[1]+t.T[3]*dz,H2);
  h = &t.vHits[i-2];
  H2.Combine( h->H, h->w );

  for( --i; i>=iHit; i-- ){
    h = &t.vHits[i];
    Station &st = vStations[i];
    z0 = st.z;    
    dz = (z1-z0);
    st.Map.GetField(t.T[0]-t.T[2]*dz,t.T[1]-t.T[3]*dz,H0);
    H0.Combine( h->H, h->w );
    f.Set( H0, z0, H1, z1, H2, z2);

    ExtrapolateALight( t.T, t.C, st.zhit, qp0, f );
    if(i == 1) AddPipeMaterial( t, qp0 );
    AddMaterial( t, st, qp0 );

    if(EmptyHit && i==iHit) continue;
    Fvec_t u = h->x*st.UInfo.cos_phi + h->y*st.UInfo.sin_phi;
    Fvec_t v = h->x*st.VInfo.cos_phi + h->y*st.VInfo.sin_phi;
    Filter( t, st.UInfo, u, h->w );
    Filter( t, st.VInfo, v, h->w );

    H2 = H1; 
    z2 = z1;
    H1 = H0; 
    z1 = z0;
  }

  for(int iT=0; iT<6; iT++)
    t.Ts[iHit][iT] = t.T[iT];
  t.Cs[iHit] = t.C;
  t.fs[iHit] = t.f;

  if(iHit==0) return;
  // fit upstream to the hit, that is previous to the hit we are interested in

  GuessVec( t, vStations,NStations, 1);

  qp0 = t.T[4];
  i= 0;
  h = &t.vHits[i];

  FilterFirst( t, *h, vStations[i] );
  AddMaterial( t, vStations[ i ], qp0 );

  z1 = vStations[ i ].z;
  vStations[i].Map.GetField(t.T[0],t.T[1], H1);
  H1.Combine( h->H, h->w );

  z2 = vStations[ i+2 ].z;
  dz = z2-z1;
  vStations[ i+2 ].Map.GetField(t.T[0]+t.T[2]*dz,t.T[1]+t.T[3]*dz,H2);
  h = &t.vHits[i+2];
  H2.Combine( h->H, h->w );

  for( ++i; i<iHit; i++ ){
    h = &t.vHits[i];
    Station &st = vStations[i];
    z0 = st.z;    
    dz = (z1-z0);
    st.Map.GetField(t.T[0]-t.T[2]*dz,t.T[1]-t.T[3]*dz,H0);
    H0.Combine( h->H, h->w );
    f.Set( H0, z0, H1, z1, H2, z2);

    ExtrapolateALight( t.T, t.C, st.zhit, qp0, f );
    if(i == 2) AddPipeMaterial( t, qp0 );

    AddMaterial( t, st, qp0 );

    Fvec_t u = h->x*st.UInfo.cos_phi + h->y*st.UInfo.sin_phi;
    Fvec_t v = h->x*st.VInfo.cos_phi + h->y*st.VInfo.sin_phi;
    Filter( t, st.UInfo, u, h->w );
    Filter( t, st.VInfo, v, h->w );

    H2 = H1; 
    z2 = z1;
    H1 = H0; 
    z1 = z0;
  }
    // extrapolate obtained parameters to the hit we are interested in
  h = &t.vHits[iHit];
  Station &st = vStations[iHit];
  z0 = st.z;    
  dz = (z1-z0);
  st.Map.GetField(t.T[0]-t.T[2]*dz,t.T[1]-t.T[3]*dz,H0);
  H0.Combine( h->H, h->w );
  f.Set( H0, z0, H1, z1, H2, z2);

  ExtrapolateALight( t.T, t.C, st.zhit, qp0, f );
  if(iHit == 2) AddPipeMaterial( t, qp0 );
  if(EmptyHit && iHit ==NStations-1) 
  {
    for(int iT=0; iT<6; iT++)
      t.Ts[iHit][iT] = t.T[iT];
    t.Cs[iHit] = t.C;
    t.fs[iHit] = t.f;
    return;
  }

  if ( iHit >= NStations/2 ) // add short to longer
    FilterTracks(t.Ts[iHit], t.Cs[iHit], t.T, t.C, &t.Chi2s[iHit]);
  else {
    FilterTracks(t.T, t.C, t.Ts[iHit], t.Cs[iHit], &t.Chi2s[iHit]);
    for( int k = 0; k < 5; k++)
      t.Ts[iHit][k] = t.T[k];
    t.Cs[iHit] = t.C;
  }
  
  for(int iT=0; iT<6; iT++)
    t.T[iT] = t.Ts[iHit][iT];
  t.C = t.Cs[iHit];
  t.f = t.fs[iHit];

} // void DAFBase::SmoothHit

inline void DAFBase::UpdateWeights(TrackV &t, Station vStations[], int NStations, int const &IterDAF) const
{

#ifdef VC
  float_m Check;
#else
  Fvec_t Check;
#endif

    
  for(int iSt=0; iSt<NStations; iSt++) {
    
      // find P = 1 + exp ( ( chi2 - chi2_cut ) / 2T )
    Fvec_t C00 = vStations[iSt].XYInfo.C00 + t.Cs[iSt].GetC00();
    Fvec_t C11 = vStations[iSt].XYInfo.C11 + t.Cs[iSt].GetC11();
    Fvec_t C10 = vStations[iSt].XYInfo.C10 + t.Cs[iSt].GetC10();
    Fvec_t det = C00*C11 - C10*C10;

    Fvec_t dx = (t.vHits[iSt].x - t.Ts[iSt][0]);
    Fvec_t dy = (t.vHits[iSt].y - t.Ts[iSt][1]);
    Fvec_t dx2 = dx*dx;
    Fvec_t dy2 = dy*dy;
    Fvec_t dxdy = dx*dy;

    Fvec_t Chi2 = (C11*dx2 - 2.f*C10*dxdy + C00*dy2)/det;

    Fvec_t P = Chi2;
#ifdef VC
    Check = abs(ZERO - t.vHits[iSt].w) < 0.001;
    P(Check) = ZERO;
#else
    Check = Fvec_t( ZERO < t.vHits[iSt].w );
    P = Check & P;
#endif
    P -= chi2_cut[IterDAF];
    P *= T_DAFi[IterDAF];
#ifdef VC
    float_v::Memory mem;
    for (int iexp = 0; iexp < float_v::Size; ++iexp) {
     mem[iexp] = static_cast<Single_t>(exp(p[iexp]));
    }
    P.load(mem);
#else
    P = exp( P );
#endif
    P += ONE;

    t.w[iSt] = 1.f/min( P, INF_p ); 
  }
  
  
} // SmoothDAF



inline void DAFBase::RunDAF(TrackV &t, Station vStations[], int NStations) const
{
  Smooth(t, vStations, NStations, 1);
  for(int i=0; i<NIterDAF; i++) {
    UpdateWeights(t, vStations, NStations, i);
    Smooth(t, vStations, NStations, 1, 1);
  }
}

#if defined(UD_FILTERING_S) || defined(SQUARE_ROOT1S) || defined(SQUARE_ROOT1)
  // based on NumericalRecipiesInC, p.467
  // Computes all eigenvalues and eigenvectors of a real symmetric matrix a[5][5]. On output, elements of a above the diagonal are destroyed. d[5] returns the eigenvalues of @a. v[5][5] is a matrix whose columns contain, on output, the normalized eigenvectors of @a.  VDV' = A
inline void DAFBase::Diagonalize( const CovVConventional& C, Fvec_t d[5], Fvec_t v[5][5] ) const
{

  Fvec_t a[5][5] = {
    { C.C00, C.C10, C.C20, C.C30, C.C40 },
    { C.C10, C.C11, C.C21, C.C31, C.C41 },
    { C.C20, C.C21, C.C22, C.C32, C.C42 },
    { C.C30, C.C31, C.C32, C.C33, C.C43 },
    { C.C40, C.C41, C.C42, C.C43, C.C44 }  };
  
#define ROTATE(a,i,j,k,l)                       \
  g = a[i][j];                                  \
  h = a[k][l];                                  \
  a[i][j] = g - s*(h + g*tau);                  \
  a[k][l] = h + s*(g - h*tau);
  
  const int n = 5; // matrix dim
  int j,iq,ip; // iterators
  Fvec_t b[n],z[n];
    // Initialize to the identity matrix.
  for (ip=0;ip<n;ip++) { 
    for (iq=0;iq<n;iq++)
      v[ip][iq]=0.f;
    v[ip][ip]=1.f;
  }
    // Initialize b and d to the diagonal of a.
  for (ip=0;ip<n;ip++) { 
    b[ip]=d[ip]=a[ip][ip]; 
    z[ip]=0.0; // This vector will accumulate terms of the form tapq
  }
  for (int i = 0; i < 4; i++) { // TODO optimize. 2? 4 is enouph for 10e-16 precision in double
    Fvec_t sm = 0.0; // Sum off-diagonal elements.
    for (ip=0;ip<n-1;ip++) { 
      for (iq=ip+1;iq<n;iq++)
        sm += fabs(a[ip][iq]);
    }
    // // if (sm == 0.0) // The normal return, which relies on quadratic convergence to  machine underflow. // TODO
    //   return;
    
    // Fvec_t tresh; // threshhold
    // if (i < 4)
    //   tresh=0.2*sm/(n*n); //...on the first three sweeps.
    // else
    //   tresh=0.0; // ...thereafter.
      // loops over upper triangle
    for (ip=0;ip<n-1;ip++) { 
      for (iq=ip+1;iq<n;iq++) { 
        Fvec_t g = 100.0*fabs(a[ip][iq]);
          
        // if (   i > 4 // After four sweeps, skip the rotation if the off-diagonal element is small. TODO
        //     && (fabs(d[ip])+g == fabs(d[ip]))
        //     && (fabs(d[iq])+g == fabs(d[iq])) )
        //   a[ip][iq]=0.0;
        // else 
        // if ( NotEmpty(fabs(a[ip][iq]) > tresh) )
        {
          Fvec_t h = d[iq] - d[ip];
          Fvec_t t; // tangens(theta)
          // if (fabs(h)+g == fabs(h))// TODO
          //   t = (a[ip][iq])/h; // t = 1/(2theta)
          // else 
          {
            const Fvec_t theta = 0.5*h/(a[ip][iq]); 
            t = 1.0/( fabs(theta)+sqrt(1.0+theta*theta) );
            t = if3( theta < 0.0, -t, t );
          }
          const Fvec_t c = 1.f/sqrt(1.f + t*t);
          const Fvec_t s = t*c;
          const Fvec_t tau = s/(1.f + c);
          h = t*a[ip][iq];
          z[ip] -= h;
          z[iq] += h;
          d[ip] -= h;
          d[iq] += h;
          a[ip][iq]=0.0;
          for (j=0;j<=ip-1;j++) { // Case of rotations 1 <= j < p.
            ROTATE(a,j,ip,j,iq);
          }
          for (j=ip+1;j<=iq-1;j++) { // Case of rotations p < j < q.
            ROTATE(a,ip,j,j,iq);
          }
          for (j=iq+1;j<n;j++) { // Case of rotations q < j <= n.
            ROTATE(a,ip,j,iq,j);
          }
          for (j=0;j<n;j++) {
            ROTATE(v,j,ip,j,iq);
          }
        }
      }
    }
    for (ip=0;ip<n;ip++) {
      b[ip] += z[ip];
      d[ip]=b[ip]; // Update d with the sum of tapq,
      z[ip]=0.0; // and reinitialize z.
    }
  }
#undef ROTATE

#if 0
  { // check of Diagonalize procedure
    // cout << " d " << endl;
    // for (int i = 0; i < 5; i++ )
    //   cout << d[i][0] << " ";
    // cout << endl;
    // cout << " v " << endl;
    // for (int i = 0; i < 5; i++ ) {
    //   for (int j = 0; j < 5; j++ )
    //     cout << v[i][j][0] << " ";
    //   cout << endl;
    // }
    // cout << endl;
    
    Fvec_t A_new[5][5], I[5][5];
    for (int i = 0; i < 5; i++ )
      for (int j = 0; j < 5; j++ ) {
        A_new[j][i] = 0;
        I[j][i] = 0;
        for (int k = 0; k < 5; k++ ) {
          A_new[j][i] += v[i][k]*d[k]*v[j][k];
          I[j][i] += v[i][k]*v[j][k];
        }
      }
    Fvec_t A_old[5][5] = {
    { C.C00, C.C10, C.C20, C.C30, C.C40 },
    { C.C10, C.C11, C.C21, C.C31, C.C41 },
    { C.C20, C.C21, C.C22, C.C32, C.C42 },
    { C.C30, C.C31, C.C32, C.C33, C.C43 },
    { C.C40, C.C41, C.C42, C.C43, C.C44 }  };

    
    cout << " dC/C+1 " << endl;
    for (int i = 0; i < 5; i++ ) {
      for (int j = 0; j < 5; j++ )
        cout << A_new[i][j][0]/A_old[i][j][0] << " ";
      cout << endl;
    }
    cout << " I+1 " << endl;
    for (int i = 0; i < 5; i++ ) {
      for (int j = 0; j < 5; j++ )
        cout << I[i][j][0]+1 << " ";
      cout << endl;
    }
    cout << endl;
  }
#endif // 0
} // inline void DAFBase::Diagonalize

#endif // UD_S\SR1

#if defined(UD_FILTERING_V) || defined(CONVENTIONAL)

inline void DAFBase::InvertCholetsky(CovVConventional &CM) const
{
  Fvec_t *a = &CM.C00; // a[i][j] = Cji -> a[j*(j+1)/2+i]; a[i][i] -> a[i*(i+3)/2]

  Fvec_t d[5], uud, u[5][5];
  for(int i=0; i<5; i++) 
  {
    d[i]=ZERO;
    for(int j=0; j<5; j++) 
      u[i][j]=0.;
  }

  for(int i=0; i<5; i++)
  {
    uud=0.;
    for(int j=0; j<i; j++) 
      uud += u[j][i]*u[j][i]*d[j];
    uud = a[i*(i+3)/2] - uud; //a[i][i]
#ifdef VC
    float_m IsInf = abs(uud)<1.e-8;
    uud(IsInf) = 1.e-8;
    d[i] = abs(uud)/uud;
    u[i][i] = sqrt(abs(uud));
#else
    d[i] = sgn(uud);
    u[i][i] = sqrt(fabs(uud));
#endif
    for(int j=i+1; j<5; j++) 
    {
      uud = 0.;
      for(int k=0; k<i; k++)
        uud += u[k][i]*u[k][j]*d[k][k];
      uud = a[j*(j+1)/2+i] - uud; //a[i][j]
      u[i][j] = d[i]/u[i][i]*uud;
    }
  }

    // invert u
  Fvec_t u1[5];
  for(int i=0; i<5; i++)
  {
    u1[i] = u[i][i];
    u[i][i] = ONE/u[i][i];
  }
  for(int i=0; i<4; i++)
  {
    u[i][i+1] = - u[i][i+1]*u[i][i]*u[i+1][i+1];
  }
  for(int i=0; i<3; i++)
  {
    u[i][i+2] = u[i][i+1]*u1[i+1]*u[i+1][i+2]-u[i][i+2]*u[i][i]*u[i+2][i+2];
  }
  for(int i=0; i<2; i++)
  {
    u[i][i+3] = u[i][i+2]*u1[i+2]*u[i+2][i+3] - u[i][i+3]*u[i][i]*u[i+3][i+3];
    u[i][i+3] -= u[i][i+1]*u1[i+1]*(u[i+1][i+2]*u1[i+2]*u[i+2][i+3] - u[i+1][i+3]);
  }
  u[0][4] = u[0][2]*u1[2]*u[2][4] - u[0][4]*u[0][0]*u[4][4];
  u[0][4] += u[0][1]*u1[1]*(u[1][4] - u[1][3]*u1[3]*u[3][4] - u[1][2]*u1[2]*u[2][4]);
  u[0][4] += u[3][4]*u1[3]*(u[0][3] - u1[2]*u[2][3]*(u[0][2] - u[0][1]*u1[1]*u[1][2]));

    // a = udu
  for(int i=0; i<5; i++)
    a[i+10] = u[i][4]*d[4]*u[4][4];
  for(int i=0; i<4; i++)
    a[i+6] = u[i][3]*u[3][3]*d[3] + u[i][4]*u[3][4]*d[4];
  for(int i=0; i<3; i++)
    a[i+3] = u[i][2]*u[2][2]*d[2] + u[i][3]*u[2][3]*d[3] + u[i][4]*u[2][4]*d[4];
  for(int i=0; i<2; i++)
    a[i+1] = u[i][1]*u[1][1]*d[1] + u[i][2]*u[1][2]*d[2] + u[i][3]*u[1][3]*d[3] + u[i][4]*u[1][4]*d[4];
  a[0] = u[0][0]*u[0][0]*d[0] + u[0][1]*u[0][1]*d[1] + u[0][2]*u[0][2]*d[2] + u[0][3]*u[0][3]*d[3] + u[0][4]*u[0][4]*d[4];
}


inline void DAFBase::Multiply(const CovVConventional &C, const Fvec_t *r_in, Fvec_t *r_out) const 
{
  r_out[0] = r_in[0]*C.C00 + r_in[1]*C.C10 + r_in[2]*C.C20 +r_in[3]*C.C30 + r_in[4]*C.C40;
  r_out[1] = r_in[0]*C.C10 + r_in[1]*C.C11 + r_in[2]*C.C21 +r_in[3]*C.C31 + r_in[4]*C.C41;
  r_out[2] = r_in[0]*C.C20 + r_in[1]*C.C21 + r_in[2]*C.C22 +r_in[3]*C.C32 + r_in[4]*C.C42;
  r_out[3] = r_in[0]*C.C30 + r_in[1]*C.C31 + r_in[2]*C.C32 +r_in[3]*C.C33 + r_in[4]*C.C43;
  r_out[4] = r_in[0]*C.C40 + r_in[1]*C.C41 + r_in[2]*C.C42 +r_in[3]*C.C43 + r_in[4]*C.C44;
}


#endif // conv || ud

#endif