#ifndef DAFSR_H
#define DAFSR_H

  /// DAF functions for conventinal KF covariance matrix

#include "FitFunctional.h"
#include "FitSR.h"
#include "DAFBase.h"

class DAFSR: public virtual FitFunctional, public FitSR, public DAFBase {

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

#if defined(SQUARE_ROOT1S) || defined(SQUARE_ROOT1) // Potter

inline void DAFSR::FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &sV, Fvec_t *chi2) const
{

  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]);


#ifdef SQUARE_ROOT1S // square
    
      // F = S'*H'
    Fvec_t F[5];
    F[0] = C.S00*H[0] + C.S10*H[1] + C.S20*H[2] + C.S30*H[3] + C.S40*H[4];
    F[1] = C.S01*H[0] + C.S11*H[1] + C.S21*H[2] + C.S31*H[3] + C.S41*H[4];
    F[2] = C.S02*H[0] + C.S12*H[1] + C.S22*H[2] + C.S32*H[3] + C.S42*H[4];
    F[3] = C.S03*H[0] + C.S13*H[1] + C.S23*H[2] + C.S33*H[3] + C.S43*H[4];
    F[4] = C.S04*H[0] + C.S14*H[1] + C.S24*H[2] + C.S34*H[3] + C.S44*H[4];
    
      // a = 1/(F'*F + V)
    Fvec_t FFV = V;
    for( int i = 0; i < 5; ++i )
      FFV += F[i]*F[i];
    const Fvec_t a = 1/FFV;
    
    Fvec_t aF[5];
    for( int i = 0; i < 5; ++i )
      aF[i] = a*F[i];
    
      // K = a*S*F
    Fvec_t K[5];
    K[0] = C.S00*aF[0] + C.S01*aF[1] + C.S02*aF[2] + C.S03*aF[3] + C.S04*aF[4];
    K[1] = C.S10*aF[0] + C.S11*aF[1] + C.S12*aF[2] + C.S13*aF[3] + C.S14*aF[4];
    K[2] = C.S20*aF[0] + C.S21*aF[1] + C.S22*aF[2] + C.S23*aF[3] + C.S24*aF[4];
    K[3] = C.S30*aF[0] + C.S31*aF[1] + C.S32*aF[2] + C.S33*aF[3] + C.S34*aF[4];
    K[4] = C.S40*aF[0] + C.S41*aF[1] + C.S42*aF[2] + C.S43*aF[3] + C.S44*aF[4];

    
    const Fvec_t gamma = 1/(1 + sqrt(a*V));
    
      // S = S - S*a*gamma*F*F'
    Fvec_t gaFF[5][5]; // symetric. TODO optimize
    for( int i = 0; i < 5; ++i )
      for( int j = 0; j < 5; ++j )
        gaFF[i][j] = gamma*aF[i]*F[j];

    CovV Cin = C;
#define S_UPDATE(i,j) C.S##i##j -= Cin.S##i##0*gaFF[0][j] + Cin.S##i##1*gaFF[1][j] + Cin.S##i##2*gaFF[2][j] + Cin.S##i##3*gaFF[3][j] + Cin.S##i##4*gaFF[4][j]
    S_UPDATE(0,0);
    S_UPDATE(1,0);
    S_UPDATE(2,0);
    S_UPDATE(3,0);
    S_UPDATE(4,0);
    S_UPDATE(0,1);
    S_UPDATE(1,1);
    S_UPDATE(2,1);
    S_UPDATE(3,1);
    S_UPDATE(4,1);
    S_UPDATE(0,2);
    S_UPDATE(1,2);
    S_UPDATE(2,2);
    S_UPDATE(3,2);
    S_UPDATE(4,2);
    S_UPDATE(0,3);
    S_UPDATE(1,3);
    S_UPDATE(2,3);
    S_UPDATE(3,3);
    S_UPDATE(4,3);
    S_UPDATE(0,4);
    S_UPDATE(1,4);
    S_UPDATE(2,4);
    S_UPDATE(3,4);
    S_UPDATE(4,4);
#undef S_UPDATE
    
#else // triangle
    
      // F = S'*H'
    Fvec_t F[5];
    F[0] = C.S00*H[0] + C.S10*H[1] + C.S20*H[2] + C.S30*H[3] + C.S40*H[4];
    F[1] =              C.S11*H[1] + C.S21*H[2] + C.S31*H[3] + C.S41*H[4];
    F[2] =                           C.S22*H[2] + C.S32*H[3] + C.S42*H[4];
    F[3] =                                        C.S33*H[3] + C.S43*H[4];
    F[4] =                                                     C.S44*H[4];

      // a = 1/(F'*F + V)
    Fvec_t FFV = V;
    for( int i = 0; i < 5; ++i )
      FFV += F[i]*F[i];
    const Fvec_t a = 1/FFV;
    
    Fvec_t aF[5];
    for( int i = 0; i < 5; ++i )
      aF[i] = a*F[i];
    
      // K = a*S*F
    Fvec_t K[5];
    K[0] = C.S00*aF[0];
    K[1] = C.S10*aF[0] + C.S11*aF[1];
    K[2] = C.S20*aF[0] + C.S21*aF[1] + C.S22*aF[2];
    K[3] = C.S30*aF[0] + C.S31*aF[1] + C.S32*aF[2] + C.S33*aF[3];
    K[4] = C.S40*aF[0] + C.S41*aF[1] + C.S42*aF[2] + C.S43*aF[3] + C.S44*aF[4];

    
#if 0 // only for square matrix S, not triangular
    const Fvec_t gamma = 1/(1 + sqrt(a*V));
    
      // S = S - S*a*gamma*F*F'
    Fvec_t gaFF[5][5]; // symetric. TODO optimize
    for( int i = 0; i < 5; ++i )
      for( int j = 0; j < 5; ++j )
        gaFF[i][j] = gamma*aF[i]*F[j];

    const Fvec_t SagFF00 = C.S00*gaFF[0][0];
    const Fvec_t SagFF10 = C.S10*gaFF[0][0] + C.S11*gaFF[1][0];
    const Fvec_t SagFF11 = C.S10*gaFF[0][1] + C.S11*gaFF[1][1];
    const Fvec_t SagFF20 = C.S20*gaFF[0][0] + C.S21*gaFF[1][0] + C.S22*gaFF[2][0];
    const Fvec_t SagFF21 = C.S20*gaFF[0][1] + C.S21*gaFF[1][1] + C.S22*gaFF[2][1];
    const Fvec_t SagFF22 = C.S20*gaFF[0][2] + C.S21*gaFF[1][2] + C.S22*gaFF[2][2];
    const Fvec_t SagFF30 = C.S30*gaFF[0][0] + C.S31*gaFF[1][0] + C.S32*gaFF[2][0] + C.S33*gaFF[3][0];
    const Fvec_t SagFF31 = C.S30*gaFF[0][1] + C.S31*gaFF[1][1] + C.S32*gaFF[2][1] + C.S33*gaFF[3][1];
    const Fvec_t SagFF32 = C.S30*gaFF[0][2] + C.S31*gaFF[1][2] + C.S32*gaFF[2][2] + C.S33*gaFF[3][2];
    const Fvec_t SagFF33 = C.S30*gaFF[0][3] + C.S31*gaFF[1][3] + C.S32*gaFF[2][3] + C.S33*gaFF[3][3];
    const Fvec_t SagFF40 = C.S40*gaFF[0][0] + C.S41*gaFF[1][0] + C.S42*gaFF[2][0] + C.S43*gaFF[3][0] + C.S44*gaFF[4][0];
    const Fvec_t SagFF41 = C.S40*gaFF[0][1] + C.S41*gaFF[1][1] + C.S42*gaFF[2][1] + C.S43*gaFF[3][1] + C.S44*gaFF[4][1];
    const Fvec_t SagFF42 = C.S40*gaFF[0][2] + C.S41*gaFF[1][2] + C.S42*gaFF[2][2] + C.S43*gaFF[3][2] + C.S44*gaFF[4][2];
    const Fvec_t SagFF43 = C.S40*gaFF[0][3] + C.S41*gaFF[1][3] + C.S42*gaFF[2][3] + C.S43*gaFF[3][3] + C.S44*gaFF[4][3];
    const Fvec_t SagFF44 = C.S40*gaFF[0][4] + C.S41*gaFF[1][4] + C.S42*gaFF[2][4] + C.S43*gaFF[3][4] + C.S44*gaFF[4][4];

    
#define S_UPDATE(i,j) C.S##i##j -= SagFF##i##j
    S_UPDATE(0,0);
    S_UPDATE(1,0);
    S_UPDATE(2,0);
    S_UPDATE(3,0);
    S_UPDATE(4,0);
    S_UPDATE(1,1);
    S_UPDATE(2,1);
    S_UPDATE(3,1);
    S_UPDATE(4,1);
    S_UPDATE(2,2);
    S_UPDATE(3,2);
    S_UPDATE(4,2);
    S_UPDATE(3,3);
    S_UPDATE(4,3);
    S_UPDATE(4,4);
#undef S_UPDATE
    
#else // diverge ... FIXME
      // G = I - a*F*F'
    CovVConventional G;
    G.C00 = 1 - aF[0]*F[0];
    G.C10 =   - aF[1]*F[0];
    G.C20 =   - aF[2]*F[0];
    G.C30 =   - aF[3]*F[0];
    G.C40 =   - aF[4]*F[0];
    G.C11 = 1 - aF[1]*F[1];
    G.C21 =   - aF[2]*F[1];
    G.C31 =   - aF[3]*F[1];
    G.C41 =   - aF[4]*F[1];
    G.C22 = 1 - aF[2]*F[2];
    G.C32 =   - aF[3]*F[2];
    G.C42 =   - aF[4]*F[2];
    G.C33 = 1 - aF[3]*F[3];
    G.C43 =   - aF[4]*F[3];
    G.C44 = 1 - aF[4]*F[4];

      // S -> S*sqrt(G)
    C *= CovV(G);
#endif

#endif // SQUARE_ROOT1S

    for( int i = 0; i < 5; ++i )
      r[i] -= K[i]*zeta;
    
    if( chi2 ) *chi2 +=  zeta * a * zeta;
  }
}

#else // Squareroot1

inline void DAFSR::FilterTracks(Fvec_t *r, CovV &C, const Fvec_t *m, const CovV &V, Fvec_t *chi2) const
{
    // H = 1
    // F = C'*H' = C'
    CovVConventional cC = C; // dbg
    // zeta = r - Hm
  Fvec_t zeta[5];
  for( int i = 0; i < 5; ++i ) zeta[i] = r[i] - m[i];
  
    // solve [ (VV'+F'F)^1/2' (K*(VV'+F'F)^1/2')' ] = [ W ] = [ Wtl Wtr ] = T * [ V' 0 ] // X = X^1/2 * X^1/2'
    //       [      0              C_new'         ]         = [  0  Wbr ]       [ F  C']
  
    // A = [ V' 0 ]
    //     [ F  C']


    //      ? ? ? ? ? 0 0 0 0 0
    //      0 ? ? ? ? 0 0 0 0 0
    //      0 0 ? ? ? 0 0 0 0 0
    //      0 0 0 ? ? 0 0 0 0 0
    //      0 0 0 0 ? 0 0 0 0 0
    //      ? ? ? ? ? ? ? ? ? ?
    // A  = 0 ? ? ? ? 0 ? ? ? ?
    //      0 0 ? ? ? 0 0 ? ? ?
    //      0 0 0 ? ? 0 0 0 ? ?
    //      0 0 0 0 ? 0 0 0 0 ?
  

    // Atl
  Fvec_t  A00 = V.S00;
  Fvec_t  A01 = V.S10;
  Fvec_t  A02 = V.S20;
  Fvec_t  A03 = V.S30;
  Fvec_t  A04 = V.S40;
  Fvec_t  A10 = ZERO;
  Fvec_t  A11 = V.S11;
  Fvec_t  A12 = V.S21;
  Fvec_t  A13 = V.S31;
  Fvec_t  A14 = V.S41;
  Fvec_t  A20 = ZERO;
  Fvec_t  A21 = ZERO;
  Fvec_t  A22 = V.S22;
  Fvec_t  A23 = V.S32;
  Fvec_t  A24 = V.S42;
  Fvec_t  A30 = ZERO;
  Fvec_t  A31 = ZERO;
  Fvec_t  A32 = ZERO;
  Fvec_t  A33 = V.S33;
  Fvec_t  A34 = V.S43;

  Fvec_t  A40 = ZERO;
  Fvec_t  A41 = ZERO;
  Fvec_t  A42 = ZERO;
  Fvec_t  A43 = ZERO;
  Fvec_t  A44 = V.S44;
    // Atr
  Fvec_t  A05 = ZERO;
  Fvec_t  A06 = ZERO;
  Fvec_t  A07 = ZERO;
  Fvec_t  A08 = ZERO;
  Fvec_t  A09 = ZERO;
  Fvec_t  A15 = ZERO;
  Fvec_t  A16 = ZERO;
  Fvec_t  A17 = ZERO;
  Fvec_t  A18 = ZERO;
  Fvec_t  A19 = ZERO;
  Fvec_t  A25 = ZERO;
  Fvec_t  A26 = ZERO;
  Fvec_t  A27 = ZERO;
  Fvec_t  A28 = ZERO;
  Fvec_t  A29 = ZERO;
  Fvec_t  A35 = ZERO;
  Fvec_t  A36 = ZERO;
  Fvec_t  A37 = ZERO;
  Fvec_t  A38 = ZERO;
  Fvec_t  A39 = ZERO;
  Fvec_t  A45 = ZERO;
  Fvec_t  A46 = ZERO;
  Fvec_t  A47 = ZERO;
  Fvec_t  A48 = ZERO;
  Fvec_t  A49 = ZERO;
    // Abl
  Fvec_t  A50 = C.S00;
  Fvec_t  A51 = C.S10;
  Fvec_t  A52 = C.S20;
  Fvec_t  A53 = C.S30;
  Fvec_t  A54 = C.S40;
  Fvec_t  A60 = ZERO;
  Fvec_t  A61 = C.S11;
  Fvec_t  A62 = C.S21;
  Fvec_t  A63 = C.S31;
  Fvec_t  A64 = C.S41;
  Fvec_t  A70 = ZERO;
  Fvec_t  A71 = ZERO;
  Fvec_t  A72 = C.S22;
  Fvec_t  A73 = C.S32;
  Fvec_t  A74 = C.S42;
  Fvec_t  A80 = ZERO;
  Fvec_t  A81 = ZERO;
  Fvec_t  A82 = ZERO;
  Fvec_t  A83 = C.S33;
  Fvec_t  A84 = C.S43;
  Fvec_t  A90 = ZERO;
  Fvec_t  A91 = ZERO;
  Fvec_t  A92 = ZERO;
  Fvec_t  A93 = ZERO;
  Fvec_t  A94 = C.S44;
    // Abr
  Fvec_t  A55 = C.S00;
  Fvec_t  A56 = C.S10;
  Fvec_t  A57 = C.S20;
  Fvec_t  A58 = C.S30;
  Fvec_t  A59 = C.S40;
  Fvec_t  A65 = ZERO;
  Fvec_t  A66 = C.S11;
  Fvec_t  A67 = C.S21;
  Fvec_t  A68 = C.S31;
  Fvec_t  A69 = C.S41;
  Fvec_t  A75 = ZERO;
  Fvec_t  A76 = ZERO;
  Fvec_t  A77 = C.S22;
  Fvec_t  A78 = C.S32;
  Fvec_t  A79 = C.S42;
  Fvec_t  A85 = ZERO;
  Fvec_t  A86 = ZERO;
  Fvec_t  A87 = ZERO;
  Fvec_t  A88 = C.S33;
  Fvec_t  A89 = C.S43;
  Fvec_t  A95 = ZERO;
  Fvec_t  A96 = ZERO;
  Fvec_t  A97 = ZERO;
  Fvec_t  A98 = ZERO;
  Fvec_t  A99 = C.S44;

    // Wtl
  Fvec_t W00;
  Fvec_t W01;
  Fvec_t W02;
  Fvec_t W03;
  Fvec_t W04;
  Fvec_t W11;
  Fvec_t W12;
  Fvec_t W13;
  Fvec_t W14;
  Fvec_t W22;
  Fvec_t W23;
  Fvec_t W24;
  Fvec_t W33;
  Fvec_t W34;
  Fvec_t W40;
  Fvec_t W44;
    //  Wtr
  Fvec_t W05;
  Fvec_t W06;
  Fvec_t W07;
  Fvec_t W08;
  Fvec_t W09;
  Fvec_t W15;
  Fvec_t W16;
  Fvec_t W17;
  Fvec_t W18;
  Fvec_t W19;
  Fvec_t W25;
  Fvec_t W26;
  Fvec_t W27;
  Fvec_t W28;
  Fvec_t W29;

  Fvec_t W35;
  Fvec_t W36;
  Fvec_t W37;
  Fvec_t W38;
  Fvec_t W39;
  Fvec_t W45;
  Fvec_t W46;
  Fvec_t W47;
  Fvec_t W48;
  Fvec_t W49;
    // Wbr
  Fvec_t& W55 = C.S00;
  Fvec_t& W56 = C.S10;
  Fvec_t& W66 = C.S11;
  Fvec_t& W57 = C.S20;
  Fvec_t& W67 = C.S21;
  Fvec_t& W77 = C.S22;
  Fvec_t& W58 = C.S30;
  Fvec_t& W68 = C.S31;
  Fvec_t& W78 = C.S32;
  Fvec_t& W88 = C.S33;
  Fvec_t& W59 = C.S40;
  Fvec_t& W69 = C.S41;
  Fvec_t& W79 = C.S42;
  Fvec_t& W89 = C.S43;
  Fvec_t& W99 = C.S44;

  
#define S_DEFINE( k )                                                   \
  const Fvec_t s##k = sqrt( A0##k*A0##k + A1##k*A1##k + A2##k*A2##k + A3##k*A3##k + A4##k*A4##k + A5##k*A5##k + A6##k*A6##k + A7##k*A7##k + A8##k*A8##k + A9##k*A9##k ); \
  const Fvec_t is##k = ONE/s##k

#define W_DEFINE( k, j ) W##k##j = is##k*( A0##k*A0##j + A1##k*A1##j + A2##k*A2##j + A3##k*A3##j + A4##k*A4##j + A5##k*A5##j + A6##k*A6##j + A7##k*A7##j + A8##k*A8##j + A9##k*A9##j )
    
#define A_UPDATE( k, j, l ) A##l##j -= is##k * W##k##j * A##l##k
    
#define A_UPDATE_COLUMN( k, j )                 \
  A_UPDATE( k, j, 0 );                          \
  A_UPDATE( k, j, 1 );                          \
  A_UPDATE( k, j, 2 );                          \
  A_UPDATE( k, j, 3 );                          \
  A_UPDATE( k, j, 4 );                          \
  A_UPDATE( k, j, 5 );                          \
  A_UPDATE( k, j, 6 );                          \
  A_UPDATE( k, j, 7 );                          \
  A_UPDATE( k, j, 8 );                          \
  A_UPDATE( k, j, 9 );        
    
  S_DEFINE( 0 );
  W00 = s0;
  W_DEFINE( 0, 1 );
  W_DEFINE( 0, 2 );
  W_DEFINE( 0, 3 );
  W_DEFINE( 0, 4 );
  W_DEFINE( 0, 5 );
  W_DEFINE( 0, 6 );
  W_DEFINE( 0, 7 );
  W_DEFINE( 0, 8 );
  W_DEFINE( 0, 9 );
  A_UPDATE_COLUMN( 0, 1 );
  A_UPDATE_COLUMN( 0, 2 );
  A_UPDATE_COLUMN( 0, 3 );
  A_UPDATE_COLUMN( 0, 4 );
  A_UPDATE_COLUMN( 0, 5 );
  A_UPDATE_COLUMN( 0, 6 );
  A_UPDATE_COLUMN( 0, 7 );
  A_UPDATE_COLUMN( 0, 8 );
  A_UPDATE_COLUMN( 0, 9 );
  
  S_DEFINE( 1 );
  W11 = s1;
  W_DEFINE( 1, 2 );
  W_DEFINE( 1, 3 );
  W_DEFINE( 1, 4 );
  W_DEFINE( 1, 5 );
  W_DEFINE( 1, 6 );
  W_DEFINE( 1, 7 );
  W_DEFINE( 1, 8 );
  W_DEFINE( 1, 9 );
  A_UPDATE_COLUMN( 1, 2 );
  A_UPDATE_COLUMN( 1, 3 );
  A_UPDATE_COLUMN( 1, 4 );
  A_UPDATE_COLUMN( 1, 5 );
  A_UPDATE_COLUMN( 1, 6 );
  A_UPDATE_COLUMN( 1, 7 );
  A_UPDATE_COLUMN( 1, 8 );
  A_UPDATE_COLUMN( 1, 9 );
  
  S_DEFINE( 2 );
  W22 = s2;
  W_DEFINE( 2, 3 );
  W_DEFINE( 2, 4 );
  W_DEFINE( 2, 5 );
  W_DEFINE( 2, 6 );
  W_DEFINE( 2, 7 );
  W_DEFINE( 2, 8 );
  W_DEFINE( 2, 9 );
  A_UPDATE_COLUMN( 2, 3 );
  A_UPDATE_COLUMN( 2, 4 );
  A_UPDATE_COLUMN( 2, 5 );
  A_UPDATE_COLUMN( 2, 6 );
  A_UPDATE_COLUMN( 2, 7 );
  A_UPDATE_COLUMN( 2, 8 );
  A_UPDATE_COLUMN( 2, 9 );
  
  S_DEFINE( 3 );
  W33 = s3;
  W_DEFINE( 3, 4 );
  W_DEFINE( 3, 5 );
  W_DEFINE( 3, 6 );
  W_DEFINE( 3, 7 );
  W_DEFINE( 3, 8 );
  W_DEFINE( 3, 9 );
  A_UPDATE_COLUMN( 3, 4 );
  A_UPDATE_COLUMN( 3, 5 );
  A_UPDATE_COLUMN( 3, 6 );
  A_UPDATE_COLUMN( 3, 7 );
  A_UPDATE_COLUMN( 3, 8 );
  A_UPDATE_COLUMN( 3, 9 );
  
  S_DEFINE( 4 );
  W44 = s4;
  W_DEFINE( 4, 5 );
  W_DEFINE( 4, 6 );
  W_DEFINE( 4, 7 );
  W_DEFINE( 4, 8 );
  W_DEFINE( 4, 9 );
  A_UPDATE_COLUMN( 4, 5 );
  A_UPDATE_COLUMN( 4, 6 );
  A_UPDATE_COLUMN( 4, 7 );
  A_UPDATE_COLUMN( 4, 8 );
  A_UPDATE_COLUMN( 4, 9 );

  S_DEFINE( 5 );
  W55 = s5;
  W_DEFINE( 5, 6 );
  W_DEFINE( 5, 7 );
  W_DEFINE( 5, 8 );
  W_DEFINE( 5, 9 );
  A_UPDATE_COLUMN( 5, 6 );
  A_UPDATE_COLUMN( 5, 7 );
  A_UPDATE_COLUMN( 5, 8 );
  A_UPDATE_COLUMN( 5, 9 );

  S_DEFINE( 6 );
  W66 = s6;
  W_DEFINE( 6, 7 );
  W_DEFINE( 6, 8 );
  W_DEFINE( 6, 9 );
  A_UPDATE_COLUMN( 6, 7 );
  A_UPDATE_COLUMN( 6, 8 );
  A_UPDATE_COLUMN( 6, 9 );

  S_DEFINE( 7 );
  W77 = s7;
  W_DEFINE( 7, 8 );
  W_DEFINE( 7, 9 );
  A_UPDATE_COLUMN( 7, 8 );
  A_UPDATE_COLUMN( 7, 9 );

  S_DEFINE( 8 );
  W88 = s8;
  W_DEFINE( 8, 9 );
  A_UPDATE_COLUMN( 8, 9 );
  
  S_DEFINE( 9 );
  W99 = s9;
    
#undef A_UPDATE_COLUMN
#undef A_UPDATE
#undef W_DEFINE
#undef S_DEFINE
  
    // Get state vector update

  //   // IWtl = Wtl^-1
  const Fvec_t IW00 = ONE/W00;
  const Fvec_t IW11 = ONE/W11;
  const Fvec_t IW22 = ONE/W22;
  const Fvec_t IW33 = ONE/W33;
  const Fvec_t IW44 = ONE/W44;
  const Fvec_t IW01 = - IW00*W01*IW11;
  const Fvec_t IW12 = - IW11*W12*IW22;
  const Fvec_t IW23 = - IW22*W23*IW33;
  const Fvec_t IW34 = - IW33*W34*IW44;
  const Fvec_t IW02 = - IW00*(W01*IW12 + W02*IW22);
  const Fvec_t IW13 = - IW11*(W12*IW23 + W13*IW33);
  const Fvec_t IW24 = - IW22*(W23*IW34 + W24*IW44);
  const Fvec_t IW03 = - IW00*(W01*IW13 + W02*IW23 + W03*IW33 );
  const Fvec_t IW14 = - IW11*(W12*IW24 + W13*IW34 + W14*IW44 );
  const Fvec_t IW04 = - IW00*(W01*IW14 + W02*IW24 + W03*IW34 + W04*IW44 );
  
  // IW00 = ONE/W00;
  // IW11 = ONE/W11;
  // IW22 = ONE/W22;
  // IW33 = ONE/W33;
  // IW44 = ONE/W44;
  // IW01 = - IW00*W01*IW11;
  // IW12 = - IW11*W12*IW22;
  // IW23 = - IW22*W23*IW33;
  // IW34 = - IW33*W34*IW44;
  // IW02 = IW01*W11*IW12 - W02*IW00*IW22;
  // IW13 = IW12*W22*IW23 - W13*IW11*IW33;
  // IW24 = IW23*W33*IW34 - W24*IW22*IW44;
  // IW03 = IW02*W22*IW23 - W03*IW00*IW33 - IW01*W11*( IW12*W22*IW23 - IW13 );
  // IW14 = IW13*W33*IW34 - W14*IW11*IW44 - IW12*W22*( IW23*W33*IW34 - IW24 );
  // IW04 = IW02*W22*IW24 - W04*IW00*IW44 + IW01*W11*( IW14 - IW13*W33*IW34 - IW12*W22*IW24 ) + IW34*W33*(IW03 - IW23*W22*(IW02 - IW01*W11*IW12));

    // const Fvec_t zetaA = IWtl'*zeta;
  const Fvec_t zetaA0 = IW00*zeta[0];
  const Fvec_t zetaA1 = IW01*zeta[0] + IW11*zeta[1];
  const Fvec_t zetaA2 = IW02*zeta[0] + IW12*zeta[1] + IW22*zeta[2];
  const Fvec_t zetaA3 = IW03*zeta[0] + IW13*zeta[1] + IW23*zeta[2] + IW33*zeta[3];
  const Fvec_t zetaA4 = IW04*zeta[0] + IW14*zeta[1] + IW24*zeta[2] + IW34*zeta[3] + IW44*zeta[4];
    // dr = - K*zeta = - Wtr'*zetaA
  r[0] -= W05*zetaA0 + W15*zetaA1 + W25*zetaA2 + W35*zetaA3 + W45*zetaA4;
  r[1] -= W06*zetaA0 + W16*zetaA1 + W26*zetaA2 + W36*zetaA3 + W46*zetaA4;
  r[2] -= W07*zetaA0 + W17*zetaA1 + W27*zetaA2 + W37*zetaA3 + W47*zetaA4;
  r[3] -= W08*zetaA0 + W18*zetaA1 + W28*zetaA2 + W38*zetaA3 + W48*zetaA4;
  r[4] -= W09*zetaA0 + W19*zetaA1 + W29*zetaA2 + W39*zetaA3 + W49*zetaA4;

  
    // dchi2 = zetaA'*zetaA
  if(chi2) *chi2 += zetaA0*zetaA0 + zetaA1*zetaA1 + zetaA2*zetaA2 + zetaA3*zetaA3 + zetaA4*zetaA4;

}
#endif // else SQUARE_ROOT1










#endif