#ifndef FitUD_H
#define FitUD_H

  /// conventinal KF approach
#include "FitBase.h"

class FitUD: public FitBase {
 public:
    void ExtrapolateALight( Fvec_t T[], CovV &C,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Fvec_t w = ZERO) const;
  
 protected:
  void Filter( TrackV &track, HitInfo &info, Fvec_t &u, Fvec_t w = ONE ) const;
  void FilterFirst( TrackV &track, HitV &hit, Station &st ) const;

  void ExtrapolateWithMaterial( TrackV &track, const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe = 0, Fvec_t w = 0) const;

 private:
  void AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const;
};

//inline // --> causes a runtime overhead and problems for the MS compiler (error C2603)
void FitUD::ExtrapolateALight
( 
 Fvec_t T [], // input track parameters (x,y,tx,ty,Q/p)
 CovV &C,     // input covariance matrix
 const Fvec_t &z_out  , // extrapolate to this z position
 Fvec_t       &qp0    , // use Q/p linearisation at this value
 FieldRegion &F,
 Fvec_t w
 ) const
{
   //
  //  Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
  //
  Jacobian_t J;
  ExtrapolateJ( T, z_out, qp0, F, J, w );
  
 //          covariance matrix transport 

    // get W
  Fvec_t W[5][5]; // W = J*U

    //     1 ? ? ? ?
    //     0 1 ? ? ?
    // W = 0 0 ? ? ?
    //     0 0 ? ? ?
    //     0 0 0 0 1
  
  for ( int i = 0; i < 5; ++i )
    for ( int j = 0; j < 5; ++j )
      W[i][j] = 0;

  W[0][0] = 1;
  W[0][1] = C.U01;
  W[0][2] = C.U02 + J(0,2);
  W[0][3] = C.U03 + J(0,2)*C.U23 + J(0,3);
  W[0][4] = C.U04 + J(0,2)*C.U24 + J(0,3)*C.U34 + J(0,4);
  W[1][1] = 1;
  W[1][2] = C.U12 + J(1,2);
  W[1][3] = C.U13 + J(1,2)*C.U23 + J(1,3);
  W[1][4] = C.U14 + J(1,2)*C.U24 + J(1,3)*C.U34 + J(1,4);
  W[2][2] = J(2,2);
  W[2][3] = J(2,2)*C.U23 + J(2,3);
  W[2][4] = J(2,2)*C.U24 + J(2,3)*C.U34 + J(2,4);
  W[3][2] = J(3,2);
  W[3][3] = J(3,2)*C.U23 + J(3,3);
  W[3][4] = J(3,2)*C.U24 + J(3,3)*C.U34 + J(3,4);
  W[4][4] = 1;

    // Get U and v 
  CovV UD = C; // new C.U

  Fvec_t v[5][5]; // W = U*v; v = W-(U-1)*v
  for ( int i = 0; i < 5; ++i )
    for ( int j = 0; j < 5; ++j )
      v[i][j] = 0;

  v[4][4] = W[4][4];

  UD.U34 = W[3][4]; // iU4 = 1/W[4][4] = 1;
  
  v[3][2] = W[3][2] - UD.U34*v[4][2];
  v[3][3] = W[3][3] - UD.U34*v[4][3];
  v[3][4] = W[3][4] - UD.U34*v[4][4];

  const Fvec_t iU3 = ONE/(v[3][2]*C.D22*v[3][2] + v[3][3]*C.D33*v[3][3] + v[3][4]*C.D44*v[3][4]);
  UD.U23 = (W[2][2]*C.D22*v[3][2] + W[2][3]*C.D33*v[3][3] + W[2][4]*C.D44*v[3][4])*iU3;
  UD.U24 = W[2][4];

  v[2][2] = W[2][2] - UD.U23*v[3][2] - UD.U24*v[4][2];
  v[2][3] = W[2][3] - UD.U23*v[3][3] - UD.U24*v[4][3];
  v[2][4] = W[2][4] - UD.U23*v[3][4] - UD.U24*v[4][4];

  const Fvec_t iU2 = ONE/(v[2][2]*C.D22*v[2][2] + v[2][3]*C.D33*v[2][3] + v[2][4]*C.D44*v[2][4]);
  UD.U12 = (W[1][2]*C.D22*v[2][2] + W[1][3]*C.D33*v[2][3] + W[1][4]*C.D44*v[2][4])*iU2;
  UD.U13 = (W[1][2]*C.D22*v[3][2] + W[1][3]*C.D33*v[3][3] + W[1][4]*C.D44*v[3][4])*iU3;
  UD.U14 = W[1][4];

  v[1][1] = 1; // W[1][1];
  v[1][2] = W[1][2] - UD.U12*v[2][2] - UD.U13*v[3][2] - UD.U14*v[4][2];
  v[1][3] = W[1][3] - UD.U12*v[2][3] - UD.U13*v[3][3] - UD.U14*v[4][3];
  v[1][4] = W[1][4] - UD.U12*v[2][4] - UD.U13*v[3][4] - UD.U14*v[4][4];

  const Fvec_t iU1 = ONE/( C.D11 + v[1][2]*C.D22*v[1][2] + v[1][3]*C.D33*v[1][3] + v[1][4]*C.D44*v[1][4]);
  UD.U01 = (W[0][1]*C.D11*v[1][1] + W[0][2]*C.D22*v[1][2] + W[0][3]*C.D33*v[1][3] + W[0][4]*C.D44*v[1][4])*iU1;
  UD.U02 = (W[0][1]*C.D11*v[2][1] + W[0][2]*C.D22*v[2][2] + W[0][3]*C.D33*v[2][3] + W[0][4]*C.D44*v[2][4])*iU2;
  UD.U03 = (W[0][1]*C.D11*v[3][1] + W[0][2]*C.D22*v[3][2] + W[0][3]*C.D33*v[3][3] + W[0][4]*C.D44*v[3][4])*iU3;
  UD.U04 = W[0][4];

  v[0][0] = 1;  // W[0][0];
  v[0][1] = W[0][1] - UD.U01*v[1][1]; // W[1][1];
  v[0][2] = W[0][2] - UD.U01*v[1][2] - UD.U02*v[2][2] - UD.U03*v[3][2] - UD.U04*v[4][2];
  v[0][3] = W[0][3] - UD.U01*v[1][3] - UD.U02*v[2][3] - UD.U03*v[3][3] - UD.U04*v[4][3];
  v[0][4] = W[0][4] - UD.U01*v[1][4] - UD.U02*v[2][4] - UD.U03*v[3][4] - UD.U04*v[4][4];

    // get D = v*D*v'
  UD.D00 =         C.D00         + v[0][1]*C.D11*v[0][1] + v[0][2]*C.D22*v[0][2] + v[0][3]*C.D33*v[0][3] + v[0][4]*C.D44*v[0][4]; // v[0][0] = 1;
  UD.D11 =         C.D11         + v[1][2]*C.D22*v[1][2] + v[1][3]*C.D33*v[1][3] + v[1][4]*C.D44*v[1][4]; // // v[1][1] = 1;
  UD.D22 = v[2][2]*C.D22*v[2][2] + v[2][3]*C.D33*v[2][3] + v[2][4]*C.D44*v[2][4];
  UD.D33 = v[3][2]*C.D22*v[3][2] + v[3][3]*C.D33*v[3][3] + v[3][4]*C.D44*v[3][4];
  UD.D44 = v[4][4]*C.D44*v[4][4];
  
  C = UD;
}

inline void FitUD::Filter( TrackV &track, HitInfo &info, Fvec_t &u, Fvec_t w ) const
{
  const Fvec_t p = ONE/w;
  cnst w_th = 0.001f; // max w to filter measurement
  Fvec_m_t mask = w > w_th;
  
      // convert input
  Fvec_t *T = track.T;
  CovV   &C = track.C;
  
  const Fvec_t V = info.sigma2*p;
  register Fvec_t S, zeta, zetaS, HCH;
  register Fvec_t F0, F1, F2, F3, F4;

  const Fvec_t &H0 = info.cos_phi;
  const Fvec_t &H1 = info.sin_phi;
  zeta = H0*T[0] + H1*T[1] - u;

  { // F = CH' = CUDU'H'

      // DU = D*U'
      // C = U*D*U'= U*DU
#define DU_DEF(i,j) const Fvec_t DU##i##j = C.D##i##i*C.U##j##i
    DU_DEF(4,3);
    DU_DEF(4,2);
    DU_DEF(4,1);
    DU_DEF(4,0);
    DU_DEF(3,2);
    DU_DEF(3,1);
    DU_DEF(3,0);
    DU_DEF(2,1);
    DU_DEF(2,0);
    DU_DEF(1,0);
#undef DU_DEF
    
    const Fvec_t C00 = C.D00 + C.U01*DU10 + C.U02*DU20 + C.U03*DU30 + C.U04*DU40;
    const Fvec_t C10 =                DU10 + C.U12*DU20 + C.U13*DU30 + C.U14*DU40;
    const Fvec_t C11 =               C.D11 + C.U12*DU21 + C.U13*DU31 + C.U14*DU41;
    const Fvec_t C20 =                             DU20 + C.U23*DU30 + C.U24*DU40;
    const Fvec_t C21 =                             DU21 + C.U23*DU31 + C.U24*DU41;
    const Fvec_t C30 =                                          DU30 + C.U34*DU40;
    const Fvec_t C31 =                                          DU31 + C.U34*DU41;
    const Fvec_t C40 = C.D44*C.U04;
    const Fvec_t C41 = C.D44*C.U14;

    
    F0 = H0*C00 + H1*C10;
    F1 = H0*C10 + H1*C11;

    HCH = ( F0*H0 + F1*H1 );

    F2 = H0*C20 + H1*C21;
    F3 = H0*C30 + H1*C31;
    F4 = H0*C40 + H1*C41;
  }
#ifndef VC
  S = mask & 1./( V + HCH );
#else
  S = ZERO;
  S(mask) = ONE*rcp( V + HCH );
#endif // VC
  
  zetaS = zeta * S;
  track.Chi2 +=  zeta * zetaS ;
  track.NDF  += 1;
  T[0] -= F0*zetaS;
  T[1] -= F1*zetaS;
  T[2] -= F2*zetaS;
  T[3] -= F3*zetaS;
  T[4] -= F4*zetaS;

  {

      // DUH = D*U'*H' = D*(H*U)'
    const Fvec_t DUH0 = C.D00*(H0);
    const Fvec_t DUH1 = C.D11*(H0*C.U01 + H1);
    const Fvec_t DUH2 = C.D22*(H0*C.U02 + H1*C.U12);
    const Fvec_t DUH3 = C.D33*(H0*C.U03 + H1*C.U13);
    const Fvec_t DUH4 = C.D44*(H0*C.U04 + H1*C.U14);

    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;

  }
}

inline void FitUD::FilterFirst( TrackV &track, HitV &hit, Station &st ) const
{
  CovV &C = track.C;
  Fvec_t w1 = ONE-hit.w;
  Fvec_t c00 = hit.w*st.XYInfo.C00 + w1*INF;
  Fvec_t c10 = hit.w*st.XYInfo.C10 + w1*INF;
  Fvec_t c11 = hit.w*st.XYInfo.C11 + w1*INF;
  
    // initialize covariance matrix 
  C.D11 = c11;
  C.U01 = c10/C.D11;
  C.D00 = c00 - c10*C.U01;

  C.U02= ZERO;      C.U12= ZERO;      C.D22= INF2;
  C.U03= ZERO;      C.U13= ZERO;      C.U23= ZERO; C.D33= INF2;
  C.U04= ZERO;      C.U14= ZERO;      C.U24= ZERO; C.U34= ZERO; C.D44= INF;

  track.T[0] = hit.w*hit.x + w1*track.T[0];
  track.T[1] = hit.w*hit.y + w1*track.T[1];
  track.NDF = -3.0;
  track.Chi2 = ZERO;
}


inline void FitUD::AddMaterial( CovV &C, Fvec_t Q22, Fvec_t Q32, Fvec_t Q33 ) const
{
    
    // W = [ U I ]
    //     1 ? ? ? ? 1 0 0 0 0
    //     0 1 ? ? ? 0 1 0 0 0
    // W = 0 0 1 ? ? 0 0 1 0 0
    //     0 0 0 1 ? 0 0 0 1 0
    //     0 0 0 0 1 0 0 0 0 1

    // Dn = [ D 0 ]
    //      [ 0 Q ]
    //      ? 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 0
    // Dn = 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 0 ? ? 0
    //      0 0 0 0 0 0 0 ? ? 0
    //      0 0 0 0 0 0 0 0 0 0

  const Fvec_t &Dn00 = C.D00;
  const Fvec_t &Dn11 = C.D11;
  const Fvec_t &Dn22 = C.D22;
  const Fvec_t &Dn33 = C.D33;
  const Fvec_t &Dn44 = C.D44;
  const Fvec_t &Dn77 = Q22;
  const Fvec_t &Dn78 = Q32;
  // const Fvec_t &Dn87 = Q32;
  const Fvec_t &Dn88 = Q33;

  
      // Get U and v 
  CovV UD = C; // new C.U

    // v = W-(U-1)*v

  const Fvec_t v44 = 1.f; // W44
  const Fvec_t v49 = 1.f; // W49

  const Fvec_t iU4 = ONE/(v44*Dn44*v44); // Dn99 = 0
  UD.U34 = C.U34; // 

  const Fvec_t v33 = 1.f;
  const Fvec_t v34 = C.U34 - UD.U34*v44;
  const Fvec_t v38 = 1.f;
  const Fvec_t v39 = - UD.U34*v49;

  const Fvec_t iU3 = ONE/(v33*Dn33*v33 + v34*Dn44*v34 + v38*Dn88*v38 );
  UD.U23 = ( C.U23*Dn33*v33 + C.U24*Dn44*v34 + Dn78*v38 )*iU3;
  UD.U24 = (                  C.U24*Dn44*v44            )*iU4;

  const Fvec_t v22 = 1.f;
  const Fvec_t v23 = C.U23 - UD.U23*v33;
  const Fvec_t v24 = C.U24 - UD.U23*v34 - UD.U24*v44;
  const Fvec_t v27 = 1.f;
  const Fvec_t v28 = - UD.U23*v38;
  const Fvec_t v29 = - UD.U23*v39 - UD.U24*v49;

  const Fvec_t iU2 = ONE/(v22*Dn22*v22 + v23*Dn33*v23 + v24*Dn44*v24 + v27*Dn77*v27  + 2*v27*Dn78*v28 + v28*Dn88*v28 );
  UD.U12 = ( C.U12*Dn22*v22 + C.U13*Dn33*v23 + C.U14*Dn44*v24)*iU2;
  UD.U13 = (                  C.U13*Dn33*v33 + C.U14*Dn44*v34)*iU3;
  UD.U14 = (                                   C.U14*Dn44*v44)*iU4;

  const Fvec_t v11 = 1.f;
  const Fvec_t v12 = C.U12 - UD.U12*v22;
  const Fvec_t v13 = C.U13 - UD.U12*v23 - UD.U13*v33;
  const Fvec_t v14 = C.U14 - UD.U12*v24 - UD.U13*v34 - UD.U14*v44;
  const Fvec_t v16 = 1.f;
  const Fvec_t v17 = - UD.U12*v27;
  const Fvec_t v18 = - UD.U12*v28 - UD.U13*v38;
  const Fvec_t v19 = - UD.U12*v29 - UD.U13*v39 - UD.U14*v49;

  const Fvec_t iU1 = ONE/(v11*Dn11*v11 + v12*Dn22*v12 + v13*Dn33*v13 + v14*Dn44*v14 + v17*Dn77*v17 + 2*v17*Dn78*v18 + v18*Dn88*v18 );
  UD.U01 = ( C.U01*Dn11*v11 + C.U02*Dn22*v12 + C.U03*Dn33*v13 + C.U04*Dn44*v14)*iU1;
  UD.U02 = (                  C.U02*Dn22*v22 + C.U03*Dn33*v23 + C.U04*Dn44*v24)*iU2;
  UD.U03 = (                                   C.U03*Dn33*v33 + C.U04*Dn44*v34)*iU3;
  UD.U04 = (                                                    C.U04*Dn44*v44)*iU4;

  const Fvec_t v00 = 1.f;
  const Fvec_t v01 = C.U01 - UD.U01*v11;
  const Fvec_t v02 = C.U02 - UD.U01*v12 - UD.U02*v22;
  const Fvec_t v03 = C.U03 - UD.U01*v13 - UD.U02*v23 - UD.U03*v33;
  const Fvec_t v04 = C.U04 - UD.U01*v14 - UD.U02*v24 - UD.U03*v34 - UD.U04*v44;
  const Fvec_t v05 = 1.f;
  const Fvec_t v06 = - UD.U01*v16;
  const Fvec_t v07 = - UD.U01*v17- UD.U02*v27;
  const Fvec_t v08 = - UD.U01*v18- UD.U02*v28 - UD.U03*v38;
  const Fvec_t v09 = - UD.U01*v19- UD.U02*v29 - UD.U03*v39 - UD.U04*v49;


    // get D = v*D*v'
  UD.D00 =         Dn00         + v01*Dn11*v01 + v02*Dn22*v02 + v03*Dn33*v03 + v04*Dn44*v04 + v07*Dn77*v07 + 2*v07*Dn78*v08 + v08*Dn88*v08; // v00 = 1;
  UD.D11 =                            Dn11     + v12*Dn22*v12 + v13*Dn33*v13 + v14*Dn44*v14 + v17*Dn77*v17 + 2*v17*Dn78*v18 + v18*Dn88*v18;
  UD.D22 =                                           Dn22     + v23*Dn33*v23 + v24*Dn44*v24 +     Dn77     + 2*v27*Dn78*v28 + v28*Dn88*v28;
  UD.D33 =                                                          Dn33     + v34*Dn44*v34 + v38*Dn88*v38;
  UD.D44 =                                                                         Dn44;

  C = UD;
} // inline void FitUD::AddMaterial


inline void FitUD::ExtrapolateWithMaterial( TrackV &track,  const Fvec_t &z_out,  Fvec_t& qp0, FieldRegion &F, Station &st, bool isPipe, Fvec_t w) const
{
  Fvec_t *T = track.T;
  CovV   &C = track.C;

  Jacobian_t J;
  ExtrapolateJ( T, z_out, qp0, F, J, w );
  
    
 //          covariance matrix transport 

    // get W

    // W = [ JU I ]
    //     1 ? ? ? ? 1 0 0 0 0
    //     0 1 ? ? ? 0 1 0 0 0
    // W = 0 0 ? ? ? 0 0 1 0 0
    //     0 0 ? ? ? 0 0 0 1 0
    //     0 0 0 0 1 0 0 0 0 1
  
  const Fvec_t W01 = C.U01;
  const Fvec_t W02 = C.U02 + J(0,2);
  const Fvec_t W03 = C.U03 + J(0,2)*C.U23 + J(0,3);
  const Fvec_t W04 = C.U04 + J(0,2)*C.U24 + J(0,3)*C.U34 + J(0,4);
  const Fvec_t W12 = C.U12 + J(1,2);
  const Fvec_t W13 = C.U13 + J(1,2)*C.U23 + J(1,3);
  const Fvec_t W14 = C.U14 + J(1,2)*C.U24 + J(1,3)*C.U34 + J(1,4);
  const Fvec_t &W22 = J(2,2);
  const Fvec_t W23 = J(2,2)*C.U23 + J(2,3);
  const Fvec_t W24 = J(2,2)*C.U24 + J(2,3)*C.U34 + J(2,4);
  const Fvec_t &W32 = J(3,2);
  const Fvec_t W33 = J(3,2)*C.U23 + J(3,3);
  const Fvec_t W34 = J(3,2)*C.U24 + J(3,3)*C.U34 + J(3,4);

  
  Fvec_t Q22, Q32, Q33;
  if (isPipe)
    GetMSMatrix( track.T[2], track.T[3], st.RadThick + PipeRadThick, log(st.RadThick + PipeRadThick), qp0, Q22, Q32, Q33 );
  else
    GetMSMatrix( track.T[2], track.T[3], st.RadThick, st.logRadThick, qp0, Q22, Q32, Q33 );

    // Dn = [ D 0 ]
    //      [ 0 Q ]
    //      ? 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 0
    // Dn = 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 0 ? ? 0
    //      0 0 0 0 0 0 0 ? ? 0
    //      0 0 0 0 0 0 0 0 0 0

  
  const Fvec_t &Dn00 = C.D00;
  const Fvec_t &Dn11 = C.D11;
  const Fvec_t &Dn22 = C.D22;
  const Fvec_t &Dn33 = C.D33;
  const Fvec_t &Dn44 = C.D44;
  const Fvec_t &Dn77 = Q22;
  const Fvec_t &Dn78 = Q32;
  // const Fvec_t &Dn87 = Q32;
  const Fvec_t &Dn88 = Q33;


  
  
      // Get U and v 

    // v = W-(U-1)*v

  // const Fvec_t v44 = 1; // W44
  // const Fvec_t v49 = 1; // W49

  const Fvec_t iU4 = ONE/(Dn44); // Dn99 = 0 v44 = 1
  C.U34 = W34; // 

  const Fvec_t v32 = W32;
  const Fvec_t v33 = W33;
  const Fvec_t v34 = W34 - C.U34;
  // const Fvec_t v38 = 1;
  const Fvec_t v39 = - C.U34;

  const Fvec_t iU3 = ONE/(v32*Dn22*v32 + v33*Dn33*v33 + v34*Dn44*v34 + Dn88 );
  C.U23 = (  Dn22*v32 + W23*Dn33*v33 + W24*Dn44*v34 + Dn78 )*iU3;
  C.U24 = (             W24*Dn44                               )*iU4;

  const Fvec_t v22 = W22 - C.U23*v32;
  const Fvec_t v23 = W23 - C.U23*v33;
  const Fvec_t v24 = W24 - C.U23*v34 - C.U24;
  // const Fvec_t v27 = 1;
  const Fvec_t v28 = - C.U23;
  const Fvec_t v29 = - C.U23*v39 - C.U24;

  const Fvec_t iU2 = ONE/(v22*Dn22*v22 + v23*Dn33*v23 + v24*Dn44*v24 + Dn77  + 2*Dn78*v28 + v28*Dn88*v28 );
  C.U12 = ( W12*Dn22*v22 + W13*Dn33*v23 + W14*Dn44*v24)*iU2;
  C.U13 = ( W12*Dn22*v32 + W13*Dn33*v33 + W14*Dn44*v34)*iU3;
  C.U14 = (                                       W14*Dn44    )*iU4;

  // const Fvec_t v11 = 1;
  const Fvec_t v12 = W12 - C.U12*v22 - C.U13*v32;
  const Fvec_t v13 = W13 - C.U12*v23 - C.U13*v33;
  const Fvec_t v14 = W14 - C.U12*v24 - C.U13*v34 - C.U14;
  // const Fvec_t v16 = 1;
  const Fvec_t v17 = - C.U12;
  const Fvec_t v18 = - C.U12*v28 - C.U13;
  const Fvec_t v19 = - C.U12*v29 - C.U13*v39 - C.U14;

  const Fvec_t iU1 = ONE/(Dn11 + v12*Dn22*v12 + v13*Dn33*v13 + v14*Dn44*v14 + v17*Dn77*v17 + 2*v17*Dn78*v18 + v18*Dn88*v18 );
  C.U01 = ( W01*Dn11     + W02*Dn22*v12 + W03*Dn33*v13 + W04*Dn44*v14)*iU1;
  C.U02 = (                    W02*Dn22*v22 + W03*Dn33*v23 + W04*Dn44*v24)*iU2;
  C.U03 = (                    W02*Dn22*v32 + W03*Dn33*v33 + W04*Dn44*v34)*iU3;
  C.U04 = (                                                          W04*Dn44    )*iU4;

  // const Fvec_t v00 = 1;
  const Fvec_t v01 = W01 - C.U01;
  const Fvec_t v02 = W02 - C.U01*v12 - C.U02*v22 - C.U03*v32;
  const Fvec_t v03 = W03 - C.U01*v13 - C.U02*v23 - C.U03*v33;
  const Fvec_t v04 = W04 - C.U01*v14 - C.U02*v24 - C.U03*v34 - C.U04;
  // const Fvec_t v05 = 1;
  const Fvec_t v06 = - C.U01;
  const Fvec_t v07 = - C.U01*v17 - C.U02;
  const Fvec_t v08 = - C.U01*v18 - C.U02*v28 - C.U03;
  const Fvec_t v09 = - C.U01*v19 - C.U02*v29 - C.U03*v39 - C.U04;


    // get D = v*D*v'
  const Fvec_t Dn22saved = Dn22;
  C.D00 =         Dn00         + v01*Dn11*v01 + v02*Dn22*v02 + v03*Dn33*v03 + v04*Dn44*v04 + v07*Dn77*v07 + 2*v07*Dn78*v08 + v08*Dn88*v08; // v00 = 1;
  C.D11 =                            Dn11     + v12*Dn22*v12 + v13*Dn33*v13 + v14*Dn44*v14 + v17*Dn77*v17 + 2*v17*Dn78*v18 + v18*Dn88*v18;
  C.D22 =                                       v22*Dn22*v22 + v23*Dn33*v23 + v24*Dn44*v24 +     Dn77     + 2*Dn78*v28 + v28*Dn88*v28;
  C.D33 =                                  v32*Dn22saved*v32 + v33*Dn33*v33 + v34*Dn44*v34 +     Dn88;
  // C.D44 =                                                                         Dn44;

}

#endif