#ifndef L1TrackPar_h
#define L1TrackPar_h 1

#include "../CbmL1Def.h"

class L1TrackPar{

  public:

    fvec
      x,y,tx,ty,qp,z,
      C00,
      C10, C11,
      C20, C21, C22,
      C30, C31, C32, C33,
      C40, C41, C42, C43, C44,
      chi2, NDF                ;

    L1TrackPar():
        x(0.f),y(0.f),tx(0.f),ty(0.f),qp(0.f),z(0.f),
    C00(0.f),
    C10(0.f), C11(0.f),
    C20(0.f), C21(0.f), C22(0.f),
    C30(0.f), C31(0.f), C32(0.f), C33(0.f),
    C40(0.f), C41(0.f), C42(0.f), C43(0.f), C44(0.f),
    chi2(0.f), NDF(0.f)
    {};
    L1TrackPar(double *T, double *C):
      x(T[0]),
      y(T[1]),
      tx(T[2]),
      ty(T[3]),
      qp(T[4]),
      z(T[5]),

      C00(C[0]),
      C10(C[1]),
      C11(C[2]),
      C20(C[3]),
      C21(C[4]),
      C22(C[5]),
      C30(C[6]),
      C31(C[7]),
      C32(C[8]),
      C33(C[9]),
      C40(C[10]),
      C41(C[11]),
      C42(C[12]),
      C43(C[13]),
      C44(C[14]),
      chi2(0.f), NDF(0.f)
      {};
      
    void SetOneEntry( const int i0, const L1TrackPar &T1, const int i1 );
  
    void Print(int i = -1);
// void L1Extrapolate
// ( 
// //  L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
//  fvec        z_out  , // extrapolate to this z position
//  fvec       qp0    , // use Q/p linearisation at this value
//  L1FieldRegion &F
//  );

    
} _fvecalignment;



class L1TrackParV{

  public:

    nsL1::vector <float>::TSimd
      fP[6], //x,y,tx,ty,qp,z,
      fC[15], /* C00,
      C10, C11,
      C20, C21, C22,
      C30, C31, C32, C33,
      C40, C41, C42, C43, C44,*/
      chi2, NDF                ;

    L1TrackParV() {};

    void resize(const int n)
    {
      for(int iP=0; iP<6; iP++)
        fP[iP].resize(n, 0.f);
      for(int iC=0; iC<15; iC++)
        fC[iC].resize(n, 0.f);
      chi2.resize(n, 0.f);
      NDF.resize(n, 0.f);
    }

    void GetTrackPar(L1TrackPar& t, int_v& index, float_m IsNiceDoublet)
    {

      t.x.gather(&(fP[0][0]), index, IsNiceDoublet);
      t.y.gather(&(fP[1][0]), index, IsNiceDoublet);
      t.tx.gather(&(fP[2][0]), index, IsNiceDoublet);
      t.ty.gather(&(fP[3][0]), index, IsNiceDoublet);
      t.qp.gather(&(fP[4][0]), index, IsNiceDoublet);
      t.z.gather(&(fP[5][0]), index, IsNiceDoublet);
      t.C00.gather(&(fC[0][0]), index, IsNiceDoublet);
      t.C10.gather(&(fC[1][0]), index, IsNiceDoublet);
      t.C11.gather(&(fC[2][0]), index, IsNiceDoublet);
      t.C20.gather(&(fC[3][0]), index, IsNiceDoublet);
      t.C21.gather(&(fC[4][0]), index, IsNiceDoublet);
      t.C22.gather(&(fC[5][0]), index, IsNiceDoublet);
      t.C30.gather(&(fC[6][0]), index, IsNiceDoublet);
      t.C31.gather(&(fC[7][0]), index, IsNiceDoublet);
      t.C32.gather(&(fC[8][0]), index, IsNiceDoublet);
      t.C33.gather(&(fC[9][0]), index, IsNiceDoublet);
      t.C40.gather(&(fC[10][0]), index, IsNiceDoublet);
      t.C41.gather(&(fC[11][0]), index, IsNiceDoublet);
      t.C42.gather(&(fC[12][0]), index, IsNiceDoublet);
      t.C43.gather(&(fC[13][0]), index, IsNiceDoublet);
      t.C44.gather(&(fC[14][0]), index, IsNiceDoublet);
      t.chi2.gather(&(chi2[0]), index, IsNiceDoublet);
      t.NDF.gather(&(NDF[0]), index, IsNiceDoublet);
    }

    void GetTrackPar(L1TrackPar& t, int_v& index)
    {

        
      t.x.gather(&(fP[0][0]), index);
      t.y.gather(&(fP[1][0]), index);
      t.tx.gather(&(fP[2][0]), index);
      t.ty.gather(&(fP[3][0]), index);
      t.qp.gather(&(fP[4][0]), index);
      t.z.gather(&(fP[5][0]), index);
      t.C00.gather(&(fC[0][0]), index);
      t.C10.gather(&(fC[1][0]), index);
      t.C11.gather(&(fC[2][0]), index);
      t.C20.gather(&(fC[3][0]), index);
      t.C21.gather(&(fC[4][0]), index);
      t.C22.gather(&(fC[5][0]), index);
      t.C30.gather(&(fC[6][0]), index);
      t.C31.gather(&(fC[7][0]), index);
      t.C32.gather(&(fC[8][0]), index);
      t.C33.gather(&(fC[9][0]), index);
      t.C40.gather(&(fC[10][0]), index);
      t.C41.gather(&(fC[11][0]), index);
      t.C42.gather(&(fC[12][0]), index);
      t.C43.gather(&(fC[13][0]), index);
      t.C44.gather(&(fC[14][0]), index);
      t.chi2.gather(&(chi2[0]), index);
      t.NDF.gather(&(NDF[0]), index);
    }

    void CopyTrackPar(L1TrackParV& t, int_v& index, int n)
    {

        
      reinterpret_cast<fvec&>(t.fP[0][n]).gather(&(fP[0][0]), index);
      reinterpret_cast<fvec&>(t.fP[1][n]).gather(&(fP[1][0]), index);
      reinterpret_cast<fvec&>(t.fP[2][n]).gather(&(fP[2][0]), index);
      reinterpret_cast<fvec&>(t.fP[3][n]).gather(&(fP[3][0]), index);
      reinterpret_cast<fvec&>(t.fP[4][n]).gather(&(fP[4][0]), index);
      reinterpret_cast<fvec&>(t.fP[5][n]).gather(&(fP[5][0]), index);
      reinterpret_cast<fvec&>(t.fC[0][n]).gather(&(fC[0][0]), index);
      reinterpret_cast<fvec&>(t.fC[1][n]).gather(&(fC[1][0]), index);
      reinterpret_cast<fvec&>(t.fC[2][n]).gather(&(fC[2][0]), index);
      reinterpret_cast<fvec&>(t.fC[3][n]).gather(&(fC[3][0]), index);
      reinterpret_cast<fvec&>(t.fC[4][n]).gather(&(fC[4][0]), index);
      reinterpret_cast<fvec&>(t.fC[5][n]).gather(&(fC[5][0]), index);
      reinterpret_cast<fvec&>(t.fC[6][n]).gather(&(fC[6][0]), index);
      reinterpret_cast<fvec&>(t.fC[7][n]).gather(&(fC[7][0]), index);
      reinterpret_cast<fvec&>(t.fC[8][n]).gather(&(fC[8][0]), index);
      reinterpret_cast<fvec&>(t.fC[9][n]).gather(&(fC[9][0]), index);
      reinterpret_cast<fvec&>(t.fC[10][n]).gather(&(fC[10][0]), index);
      reinterpret_cast<fvec&>(t.fC[11][n]).gather(&(fC[11][0]), index);
      reinterpret_cast<fvec&>(t.fC[12][n]).gather(&(fC[12][0]), index);
      reinterpret_cast<fvec&>(t.fC[13][n]).gather(&(fC[13][0]), index);
      reinterpret_cast<fvec&>(t.fC[14][n]).gather(&(fC[14][0]), index);
      reinterpret_cast<fvec&>(t.chi2[n]).gather(&(chi2[0]), index);
      reinterpret_cast<fvec&>(t.NDF[n]).gather(&(NDF[0]), index);
    }
    
    void SetTrackParV(L1TrackPar& t, uint_v& index)
    {
        
        
        t.x.scatter(&(fP[0][0]), index);
        t.y.scatter(&(fP[1][0]), index);
        t.tx.scatter(&(fP[2][0]), index);
        t.ty.scatter(&(fP[3][0]), index);
        t.qp.scatter(&(fP[4][0]), index);
        t.z.scatter(&(fP[5][0]), index);
        t.C00.scatter(&(fC[0][0]), index);
        t.C10.scatter(&(fC[1][0]), index);
        t.C11.scatter(&(fC[2][0]), index);
        t.C20.scatter(&(fC[3][0]), index);
        t.C21.scatter(&(fC[4][0]), index);
        t.C22.scatter(&(fC[5][0]), index);
        t.C30.scatter(&(fC[6][0]), index);
        t.C31.scatter(&(fC[7][0]), index);
        t.C32.scatter(&(fC[8][0]), index);
        t.C33.scatter(&(fC[9][0]), index);
        t.C40.scatter(&(fC[10][0]), index);
        t.C41.scatter(&(fC[11][0]), index);
        t.C42.scatter(&(fC[12][0]), index);
        t.C43.scatter(&(fC[13][0]), index);
        t.C44.scatter(&(fC[14][0]), index);
        t.chi2.scatter(&(chi2[0]), index);
        t.NDF.scatter(&(NDF[0]), index);
    }

    void Set(int n, L1TrackPar& t)
    {
       reinterpret_cast<fvec&>(fP[0][n])=t.x;
       reinterpret_cast<fvec&>(fP[1][n])=t.y;
       reinterpret_cast<fvec&>(fP[2][n])=t.tx;
       reinterpret_cast<fvec&>(fP[3][n])=t.ty;
       reinterpret_cast<fvec&>(fP[4][n])=t.qp;
       reinterpret_cast<fvec&>(fP[5][n])=t.z;

       reinterpret_cast<fvec&>(fC[0][n])=t.C00;
       reinterpret_cast<fvec&>(fC[1][n])=t.C10;
       reinterpret_cast<fvec&>(fC[2][n])=t.C11;
       reinterpret_cast<fvec&>(fC[3][n])=t.C20;
       reinterpret_cast<fvec&>(fC[4][n])=t.C21;
       reinterpret_cast<fvec&>(fC[5][n])=t.C22;
       reinterpret_cast<fvec&>(fC[6][n])=t.C30;
       reinterpret_cast<fvec&>(fC[7][n])=t.C31;
       reinterpret_cast<fvec&>(fC[8][n])=t.C32;
       reinterpret_cast<fvec&>(fC[9][n])=t.C33;
       reinterpret_cast<fvec&>(fC[10][n])=t.C40;
       reinterpret_cast<fvec&>(fC[11][n])=t.C41;
       reinterpret_cast<fvec&>(fC[12][n])=t.C42;
       reinterpret_cast<fvec&>(fC[13][n])=t.C43;
       reinterpret_cast<fvec&>(fC[14][n])=t.C44;
       reinterpret_cast<fvec&>(chi2[n])=t.chi2;
       reinterpret_cast<fvec&>(NDF[n])=t.NDF;
    }
    
    void Set(int n, L1TrackPar& t, int i)
    {
        fP[0].push_back(t.x[i]);
        fP[1].push_back(t.y[i]);
        fP[2].push_back(t.tx[i]);
        fP[3].push_back(t.ty[i]);
        fP[4].push_back(t.qp[i]);
        fP[5].push_back(t.z[i]);
        
        fC[0].push_back(t.C00[i]);
        fC[1].push_back(t.C10[i]);
        fC[2].push_back(t.C11[i]);
        fC[3].push_back(t.C20[i]);
        fC[4].push_back(t.C21[i]);
        fC[5].push_back(t.C22[i]);
        fC[6].push_back(t.C30[i]);
        fC[7].push_back(t.C31[i]);
        fC[8].push_back(t.C32[i]);
        fC[9].push_back(t.C33[i]);
        fC[10].push_back(t.C40[i]);
        fC[11].push_back(t.C41[i]);
        fC[12].push_back(t.C42[i]);
        fC[13].push_back(t.C43[i]);
        fC[14].push_back(t.C44[i]);
        chi2.push_back(t.chi2[i]);
        NDF.push_back(t.NDF[i]);
    }
    
    void Set(L1TrackPar& t)
    {
        
        
        for(int iP=0; iP<6; iP++)
            fP[iP].resize(fP[iP].size()+fvecLen);
        for(int iC=0; iC<15; iC++)
            fC[iC].resize(fC[iC].size()+fvecLen);
        chi2.resize(chi2.size()+fvecLen);
        NDF.resize(NDF.size()+fvecLen);
        
        reinterpret_cast<fvec&>(fP[0][chi2.size()-fvecLen])=t.x;
        reinterpret_cast<fvec&>(fP[1][chi2.size()-fvecLen])=t.y;
        reinterpret_cast<fvec&>(fP[2][chi2.size()-fvecLen])=t.tx;
        reinterpret_cast<fvec&>(fP[3][chi2.size()-fvecLen])=t.ty;
        reinterpret_cast<fvec&>(fP[4][chi2.size()-fvecLen])=t.qp;
        reinterpret_cast<fvec&>(fP[5][chi2.size()-fvecLen])=t.z;
        
        reinterpret_cast<fvec&>(fC[0][chi2.size()-fvecLen])=t.C00;
        reinterpret_cast<fvec&>(fC[1][chi2.size()-fvecLen])=t.C10;
        reinterpret_cast<fvec&>(fC[2][chi2.size()-fvecLen])=t.C11;
        reinterpret_cast<fvec&>(fC[3][chi2.size()-fvecLen])=t.C20;
        reinterpret_cast<fvec&>(fC[4][chi2.size()-fvecLen])=t.C21;
        reinterpret_cast<fvec&>(fC[5][chi2.size()-fvecLen])=t.C22;
        reinterpret_cast<fvec&>(fC[6][chi2.size()-fvecLen])=t.C30;
        reinterpret_cast<fvec&>(fC[7][chi2.size()-fvecLen])=t.C31;
        reinterpret_cast<fvec&>(fC[8][chi2.size()-fvecLen])=t.C32;
        reinterpret_cast<fvec&>(fC[9][chi2.size()-fvecLen])=t.C33;
        reinterpret_cast<fvec&>(fC[10][chi2.size()-fvecLen])=t.C40;
        reinterpret_cast<fvec&>(fC[11][chi2.size()-fvecLen])=t.C41;
        reinterpret_cast<fvec&>(fC[12][chi2.size()-fvecLen])=t.C42;
        reinterpret_cast<fvec&>(fC[13][chi2.size()-fvecLen])=t.C43;
        reinterpret_cast<fvec&>(fC[14][chi2.size()-fvecLen])=t.C44;
        reinterpret_cast<fvec&>(chi2[chi2.size()-fvecLen])=t.chi2;
        reinterpret_cast<fvec&>(NDF[chi2.size()-fvecLen])=t.NDF;
    }
    
    

} _fvecalignment;

class L1TrackParR{

  public:

    fvec&
      x,y,tx,ty,qp,z,
      C00,
      C10, C11,
      C20, C21, C22,
      C30, C31, C32, C33,
      C40, C41, C42, C43, C44,
      chi2, NDF                ;

    L1TrackParR(L1TrackParV *TV, int n):
      x(reinterpret_cast<fvec&>((*TV).fP[0][n])),
      y(reinterpret_cast<fvec&>((*TV).fP[1][n])),
      tx(reinterpret_cast<fvec&>((*TV).fP[2][n])),
      ty(reinterpret_cast<fvec&>((*TV).fP[3][n])),
      qp(reinterpret_cast<fvec&>((*TV).fP[4][n])),
      z(reinterpret_cast<fvec&>((*TV).fP[5][n])),

      C00(reinterpret_cast<fvec&>((*TV).fC[0][n])),
      C10(reinterpret_cast<fvec&>((*TV).fC[1][n])),
      C11(reinterpret_cast<fvec&>((*TV).fC[2][n])),
      C20(reinterpret_cast<fvec&>((*TV).fC[3][n])),
      C21(reinterpret_cast<fvec&>((*TV).fC[4][n])),
      C22(reinterpret_cast<fvec&>((*TV).fC[5][n])),
      C30(reinterpret_cast<fvec&>(( *TV).fC[6][n])),
      C31(reinterpret_cast<fvec&>((*TV).fC[7][n])),
      C32(reinterpret_cast<fvec&>((*TV).fC[8][n])),
      C33(reinterpret_cast<fvec&>((*TV).fC[9][n])),
      C40(reinterpret_cast<fvec&>((*TV).fC[10][n])),
      C41(reinterpret_cast<fvec&>((*TV).fC[11][n])),
      C42(reinterpret_cast<fvec&>((*TV).fC[12][n])),
      C43(reinterpret_cast<fvec&>((*TV).fC[13][n])),
      C44(reinterpret_cast<fvec&>((*TV).fC[14][n])),
      chi2(reinterpret_cast<fvec&>((*TV).chi2[n])), NDF(reinterpret_cast<fvec&>((*TV).NDF[n]))
      {};
      
    //void SetOneEntry( const int i0, const L1TrackPar &T1, const int i1 );
  
    //void Print(int i = -1);
// void L1Extrapolate
// ( 
// //  L1TrackPar &T, // input track parameters (x,y,tx,ty,Q/p) and cov.matrix
//  fvec        z_out  , // extrapolate to this z position
//  fvec       qp0    , // use Q/p linearisation at this value
//  L1FieldRegion &F
//  );

    
} _fvecalignment;

 // =============================================================================================

inline void L1TrackPar::Print(int i)
{
  std::cout.setf(std::ios::scientific,std::ios::floatfield);
  if (i == -1){
    std::cout << "T = " << std::endl;
    std::cout << x  << std::endl;
    std::cout << y  << std::endl;
    std::cout << tx << std::endl;
    std::cout << ty << std::endl;
    std::cout << qp << std::endl;
    std::cout << z  << std::endl;
  }
  else{
    std::cout << "T = ";
    std::cout << x[i]  << " ";
    std::cout << y[i]  << " ";
    std::cout << tx[i] << " ";
    std::cout << ty[i] << " ";
    std::cout << qp[i] << " ";
    std::cout << z[i]  << std::endl;
    std::cout << "C = ";
    std::cout << C00[i]  << " ";
    std::cout << C11[i]  << " ";
    std::cout << C22[i]  << " ";
    std::cout << C33[i]  << " ";
    std::cout << C44[i]  << std::endl;
  }
}

inline void L1TrackPar::SetOneEntry( const int i0, const L1TrackPar &T1, const int i1 )
{
  x[i0] = T1.x[i1];
  y[i0] = T1.y[i1];
  tx[i0] = T1.tx[i1];
  ty[i0] = T1.ty[i1];
  qp[i0] = T1.qp[i1];
  z[i0] = T1.z[i1];
  C00[i0] = T1.C00[i1];
  C10[i0] = T1.C10[i1];
  C11[i0] = T1.C11[i1];
  C20[i0] = T1.C20[i1];
  C21[i0] = T1.C21[i1];
  C22[i0] = T1.C22[i1];
  C30[i0] = T1.C30[i1];
  C31[i0] = T1.C31[i1];
  C32[i0] = T1.C32[i1];
  C33[i0] = T1.C33[i1];
  C40[i0] = T1.C40[i1];
  C41[i0] = T1.C41[i1];
  C42[i0] = T1.C42[i1];
  C43[i0] = T1.C43[i1];
  C44[i0] = T1.C44[i1];
   
  chi2[i0] = T1.chi2[i1];
  NDF[i0] = T1.NDF[i1];
} // SetOneEntry

#endif