CbmKFParticleInterface.cxx

Go to the documentation of this file.

#include "CbmKFParticleInterface.h"
 
#define cnst static const fvec
 
// for particle finding
#include "CbmKFTrack.h"
#include "CbmL1Track.h"
#include "CbmStsTrack.h"
 
#include <map>
using std::map;
 
const float CbmKFParticleInterface::DefaultCuts[2][3] = {{3., 3., -100.}, {3., 3., -100.}};
 
CbmKFParticleInterface::CbmKFParticleInterface()
    : KFPart()
{}
 
CbmKFParticleInterface::CbmKFParticleInterface(CbmKFTrackInterface* Track[])
    : KFPart()
{
    KFPart.Create(Track);
}
 
CbmKFParticleInterface::CbmKFParticleInterface(CbmKFParticle* part[], const int nPart)
    : KFPart()
{
    KFPart.Create(part, nPart);
}
 
CbmKFParticleInterface::~CbmKFParticleInterface() {}
 
CbmKFParticleInterface::CbmKFParticleInterface(const CbmKFParticleInterface& c)
    : KFPart()
{
    KFPart = c.KFPart;
}
 
CbmKFParticleInterface CbmKFParticleInterface::operator=(const CbmKFParticleInterface& c)
{
    KFPart = c.KFPart;
    return c;
}
 
void CbmKFParticleInterface::Construct(CbmKFTrackInterface* vDaughters[][fvecLen],
                                       int NDaughters,
                                       CbmKFVertexInterface* Parent,
                                       float Mass,
                                       float CutChi2)
{
    const int ND = NDaughters;
 
    CbmKFParticle_simd* vDaughters_part = new CbmKFParticle_simd[ND];
 
    for (int tr = 0; tr < NDaughters; ++tr)
        vDaughters_part[tr].Create(vDaughters[tr]);
 
    Construct(vDaughters_part, NDaughters, Parent, Mass, CutChi2);
    delete[] vDaughters_part;
}
 
void CbmKFParticleInterface::Construct(CbmKFParticle_simd* vDaughters,
                                       int NDaughters,
                                       CbmKFVertexInterface* Parent,
                                       float Mass,
                                       float CutChi2,
                                       bool isAtVtxGuess)
{
    KFPart.CleanDaughtersId();
    KFPart.SetNDaughters(NDaughters);
    for (int tr = 0; tr < NDaughters; tr++) {
        KFPart.AddDaughterId(vDaughters[tr].Id());
    }
 
    int MaxIter = 3;
    if (!(KFPart.fIsVtxGuess)) {
        fvec Ty[2];
        fvec Y[2];
        fvec Z[2];
 
        for (int tr = 0; tr < 2; tr++) {
            Y[tr] = vDaughters[tr].GetY();
            Z[tr] = vDaughters[tr].GetZ();
            Ty[tr] = vDaughters[tr].GetPy() / vDaughters[tr].GetPz();
        }
 
        fvec r0[3];
        r0[0] = 0.;
        r0[1] = 0.;
        r0[2] = (Ty[0] * Z[0] - Ty[1] * Z[1] + Y[1] - Y[0]) / (Ty[0] - Ty[1]);
 
        CbmKFParticle_simd Daughter0 = vDaughters[0];
        Extrapolate(&Daughter0, Daughter0.r, GetDStoPoint(Daughter0, r0));
 
        KFPart.fVtxGuess[0] = Daughter0.GetX();
        KFPart.fVtxGuess[1] = Daughter0.GetY();
        KFPart.fVtxGuess[2] = Daughter0.GetZ();
 
        if (!(KFPart.fIsVtxErrGuess)) {
            KFPart.fVtxErrGuess[0] = 10. * sqrt(Daughter0.C[0]);
            KFPart.fVtxErrGuess[1] = 10. * sqrt(Daughter0.C[2]);
            KFPart.fVtxErrGuess[2] = 10. * sqrt(Daughter0.C[5]);
        }
        //   KFPart.fVtxErrGuess[0] = 0.01*KFPart.fVtxGuess[0];
        //   KFPart.fVtxErrGuess[1] = 0.01*KFPart.fVtxGuess[1];
        //   KFPart.fVtxErrGuess[2] = 0.01*KFPart.fVtxGuess[2];
    } else {
        MaxIter = 1;
    }
 
    cnst ZERO = 0.0, ONE = 1.;
    cnst TWO = 2.;
    /**if( CbmKF::Instance()->vTargets.empty() ){
      r[0] = r[1] = r[2] = 0.;
      C[0] = C[2] = C[5] = 1.;
      C[1] = C[3] = C[4] = 0;
    }else{
      CbmKFTube &t = CbmKF::Instance()->vTargets[0];
      r[0] = r[1] = 0.;
      r[2] = t.z;
      C[0] = C[2] = t.R*t.R/9.;
      C[5] = t.dz/6.;
      C[1] = C[3] = C[4] = 0;
    }**/
 
    KFPart.r[3] = ZERO;
    KFPart.r[4] = ZERO;
    KFPart.r[5] = ZERO;
    KFPart.r[6] = ZERO;
    KFPart.r[7] = ZERO;
 
    KFPart.r[0] = KFPart.fVtxGuess[0];
    KFPart.r[1] = KFPart.fVtxGuess[1];
    KFPart.r[2] = KFPart.fVtxGuess[2];
 
    KFPart.C[0] = KFPart.fVtxErrGuess[0] * KFPart.fVtxErrGuess[0];
    KFPart.C[2] = KFPart.fVtxErrGuess[1] * KFPart.fVtxErrGuess[1];
    KFPart.C[5] = KFPart.fVtxErrGuess[2] * KFPart.fVtxErrGuess[2];
    KFPart.C[1] = KFPart.C[3] = KFPart.C[4] = ZERO;
 
    for (Int_t iteration = 0; iteration < MaxIter; ++iteration) {
        fvec r0[8], C0[6];
 
        for (int i = 0; i < 8; i++)
            r0[i] = KFPart.r[i];
        for (int i = 0; i < 6; i++)
            C0[i] = KFPart.C[i];
 
        KFPart.r[3] = ZERO;
        KFPart.r[4] = ZERO;
        KFPart.r[5] = ZERO;
        KFPart.r[6] = ZERO;
 
        for (Int_t i = 0; i < 36; ++i)
            KFPart.C[i] = ZERO;
 
        KFPart.C[0] = KFPart.fVtxErrGuess[0] * KFPart.fVtxErrGuess[0];
        KFPart.C[2] = KFPart.fVtxErrGuess[1] * KFPart.fVtxErrGuess[1];
        KFPart.C[5] = KFPart.fVtxErrGuess[2] * KFPart.fVtxErrGuess[2];
 
        KFPart.C[35] = ONE;
 
        L1FieldValue B = vDaughters[0].fField.Get(r0[2]);
        B.x *= 0.000299792458;   // multiply by c
        B.y *= 0.000299792458;
        B.z *= 0.000299792458;
 
        KFPart.NDF = -3.;
        KFPart.Chi2 = ZERO;
        KFPart.Q = ZERO;
 
        for (int tr = 0; tr < NDaughters; ++tr) {
            CbmKFParticle_simd Daughter = vDaughters[tr];
            if (!isAtVtxGuess)
                Extrapolate(&Daughter, Daughter.r, GetDStoPoint(Daughter, r0));
 
            fvec* m = Daughter.r;
            fvec* Cd = Daughter.C;
 
            fvec d[3] = {r0[0] - m[0], r0[1] - m[1], r0[2] - m[2]};
            fvec SigmaS =
                .1 + 10. * sqrt((d[0] * d[0] + d[1] * d[1] + d[2] * d[2]) / (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]));
 
            fvec h[6];
 
            h[0] = m[3] * SigmaS;
            h[1] = m[4] * SigmaS;
            h[2] = m[5] * SigmaS;
            h[3] = (h[1] * B.z - h[2] * B.y) * Daughter.Q;
            h[4] = (h[2] * B.x - h[0] * B.z) * Daughter.Q;
            h[5] = (h[0] * B.y - h[1] * B.x) * Daughter.Q;
 
            //* Fit of daughter momentum (x,y,z) to r0[0,1,2] vertex
            {
                fvec zeta[3] = {r0[0] - m[0], r0[1] - m[1], r0[2] - m[2]};
 
                fvec Vv[6] = {Cd[0] + h[0] * h[0], Cd[1] + h[1] * h[0], Cd[2] + h[1] * h[1],
                              Cd[3] + h[2] * h[0], Cd[4] + h[2] * h[1], Cd[5] + h[2] * h[2]};
 
                fvec Vvp[9] = {Cd[6] + h[0] * h[3],  Cd[7] + h[1] * h[3],  Cd[8] + h[2] * h[3],
                               Cd[10] + h[0] * h[4], Cd[11] + h[1] * h[4], Cd[12] + h[2] * h[4],
                               Cd[15] + h[0] * h[5], Cd[16] + h[1] * h[5], Cd[17] + h[2] * h[5]};
 
                /**   if( CutChi2 >0. ){
 
                        fvec Si[6]={ Vv[0]+C0[0],
                                 Vv[1]+C0[1], Vv[2]+C0[2],
                                 Vv[3]+C0[3], Vv[4]+C0[4], Vv[5]+C0[5] };
                        fvec S[6] = { Si[2]*Si[5] - Si[4]*Si[4],
                                  Si[3]*Si[4] - Si[1]*Si[5],
                                  Si[0]*Si[5] - Si[3]*Si[3],
                                  Si[1]*Si[4] - Si[2]*Si[3],
                                  Si[1]*Si[3] - Si[0]*Si[4],
                                  Si[0]*Si[2] - Si[1]*Si[1] };
                        fvec det = ( Si[0]*S[0] + Si[1]*S[1] + Si[3]*S[3] );
                        fvec chi2 =
                          ( +(S[0]*zeta[0] + S[1]*zeta[1] + S[3]*zeta[2])*zeta[0]
                        +(S[1]*zeta[0] + S[2]*zeta[1] + S[4]*zeta[2])*zeta[1]
                        +(S[3]*zeta[0] + S[4]*zeta[1] + S[5]*zeta[2])*zeta[2]);
                        if( chi2 > 2*CutChi2*det ) continue;
                      }
                **/
                fvec S[6] = {Vv[2] * Vv[5] - Vv[4] * Vv[4], Vv[3] * Vv[4] - Vv[1] * Vv[5],
                             Vv[0] * Vv[5] - Vv[3] * Vv[3], Vv[1] * Vv[4] - Vv[2] * Vv[3],
                             Vv[1] * Vv[3] - Vv[0] * Vv[4], Vv[0] * Vv[2] - Vv[1] * Vv[1]};
 
                fvec s = (Vv[0] * S[0] + Vv[1] * S[1] + Vv[3] * S[3]);
 
                fvec init = fvec(s > 1.E-20);
                s = rcp(s) & init;
                //          for(int j = 0; j<fvecLen; j++)
                //      s[j] = ( s[j] > 1.E-20 )  ?1./s[j] :0;
 
                S[0] *= s;
                S[1] *= s;
                S[2] *= s;
                S[3] *= s;
                S[4] *= s;
                S[5] *= s;
 
                fvec Sz[3] = {(S[0] * zeta[0] + S[1] * zeta[1] + S[3] * zeta[2]),
                              (S[1] * zeta[0] + S[2] * zeta[1] + S[4] * zeta[2]),
                              (S[3] * zeta[0] + S[4] * zeta[1] + S[5] * zeta[2])};
 
                fvec x = m[3] + Vvp[0] * Sz[0] + Vvp[1] * Sz[1] + Vvp[2] * Sz[2];
                fvec y = m[4] + Vvp[3] * Sz[0] + Vvp[4] * Sz[1] + Vvp[5] * Sz[2];
                fvec z = m[5] + Vvp[6] * Sz[0] + Vvp[7] * Sz[1] + Vvp[8] * Sz[2];
 
                h[0] = x * SigmaS;
                h[1] = y * SigmaS;
                h[2] = z * SigmaS;
                h[3] = (h[1] * B.z - h[2] * B.y) * Daughter.Q;
                h[4] = (h[2] * B.x - h[0] * B.z) * Daughter.Q;
                h[5] = (h[0] * B.y - h[1] * B.x) * Daughter.Q;
            }
 
            fvec* V = Cd;
 
            V[0] += h[0] * h[0];
            V[1] += h[1] * h[0];
            V[2] += h[1] * h[1];
            V[3] += h[2] * h[0];
            V[4] += h[2] * h[1];
            V[5] += h[2] * h[2];
 
            V[6] += h[3] * h[0];
            V[7] += h[3] * h[1];
            V[8] += h[3] * h[2];
            V[9] += h[3] * h[3];
 
            V[10] += h[4] * h[0];
            V[11] += h[4] * h[1];
            V[12] += h[4] * h[2];
            V[13] += h[4] * h[3];
            V[14] += h[4] * h[4];
 
            V[15] += h[5] * h[0];
            V[16] += h[5] * h[1];
            V[17] += h[5] * h[2];
            V[18] += h[5] * h[3];
            V[19] += h[5] * h[4];
            V[20] += h[5] * h[5];
 
            //  V[21] = Cd[21];
            //  V[22] = Cd[22];
            //  V[23] = Cd[23];
            //  V[24] = Cd[24];
            //  V[25] = Cd[25];
            //  V[26] = Cd[26];
            //  V[27] = Cd[27];
 
            //*
            KFPart.NDF += TWO;
            KFPart.Q += Daughter.Q;
 
            //*
 
            fvec S[6];
            {
                fvec Si[6] = {KFPart.C[0] + V[0], KFPart.C[1] + V[1], KFPart.C[2] + V[2],
                              KFPart.C[3] + V[3], KFPart.C[4] + V[4], KFPart.C[5] + V[5]};
 
                S[0] = Si[2] * Si[5] - Si[4] * Si[4];
                S[1] = Si[3] * Si[4] - Si[1] * Si[5];
                S[2] = Si[0] * Si[5] - Si[3] * Si[3];
                S[3] = Si[1] * Si[4] - Si[2] * Si[3];
                S[4] = Si[1] * Si[3] - Si[0] * Si[4];
                S[5] = Si[0] * Si[2] - Si[1] * Si[1];
 
                fvec s = (Si[0] * S[0] + Si[1] * S[1] + Si[3] * S[3]);
 
                fvec init = fvec(s > 1.E-20);
                s = rcp(s) & init;
                //          for(int j=0; j<fvecLen; j++)
                //      s[j] = ( s[j] > 1.E-20 )  ?1./s[j] :0;
 
                S[0] *= s;
                S[1] *= s;
                S[2] *= s;
                S[3] *= s;
                S[4] *= s;
                S[5] *= s;
            }
 
            //* Residual (measured - estimated)
 
            fvec zeta[3] = {m[0] - KFPart.r[0], m[1] - KFPart.r[1], m[2] - KFPart.r[2]};
 
            //* Add the daughter momentum to the particle momentum
 
            KFPart.r[3] += m[3];
            KFPart.r[4] += m[4];
            KFPart.r[5] += m[5];
            KFPart.r[6] += m[6];
 
            KFPart.C[9] += V[9];
            KFPart.C[13] += V[13];
            KFPart.C[14] += V[14];
            KFPart.C[18] += V[18];
            KFPart.C[19] += V[19];
            KFPart.C[20] += V[20];
            KFPart.C[24] += V[24];
            KFPart.C[25] += V[25];
            KFPart.C[26] += V[26];
            KFPart.C[27] += V[27];
 
            //* CHt = CH' - D'
 
            fvec CHt0[7], CHt1[7], CHt2[7];
 
            CHt0[0] = KFPart.C[0];
            CHt1[0] = KFPart.C[1];
            CHt2[0] = KFPart.C[3];
            CHt0[1] = KFPart.C[1];
            CHt1[1] = KFPart.C[2];
            CHt2[1] = KFPart.C[4];
            CHt0[2] = KFPart.C[3];
            CHt1[2] = KFPart.C[4];
            CHt2[2] = KFPart.C[5];
            CHt0[3] = KFPart.C[6] - V[6];
            CHt1[3] = KFPart.C[7] - V[7];
            CHt2[3] = KFPart.C[8] - V[8];
            CHt0[4] = KFPart.C[10] - V[10];
            CHt1[4] = KFPart.C[11] - V[11];
            CHt2[4] = KFPart.C[12] - V[12];
            CHt0[5] = KFPart.C[15] - V[15];
            CHt1[5] = KFPart.C[16] - V[16];
            CHt2[5] = KFPart.C[17] - V[17];
            CHt0[6] = KFPart.C[21] - V[21];
            CHt1[6] = KFPart.C[22] - V[22];
            CHt2[6] = KFPart.C[23] - V[23];
 
            //* Kalman gain K = CH'*S
 
            fvec K0[7], K1[7], K2[7];
 
            for (Int_t i = 0; i < 7; ++i) {
                K0[i] = CHt0[i] * S[0] + CHt1[i] * S[1] + CHt2[i] * S[3];
                K1[i] = CHt0[i] * S[1] + CHt1[i] * S[2] + CHt2[i] * S[4];
                K2[i] = CHt0[i] * S[3] + CHt1[i] * S[4] + CHt2[i] * S[5];
            }
 
            //* New estimation of the vertex position r += K*zeta
 
            for (Int_t i = 0; i < 7; ++i)
                KFPart.r[i] += K0[i] * zeta[0] + K1[i] * zeta[1] + K2[i] * zeta[2];
 
            //* New covariance matrix C -= K*(CH')'
 
            for (Int_t i = 0, k = 0; i < 7; ++i) {
                for (Int_t j = 0; j <= i; ++j, ++k)
                    KFPart.C[k] -= K0[i] * CHt0[j] + K1[i] * CHt1[j] + K2[i] * CHt2[j];
            }
 
            //* Calculate Chi^2 & NDF
 
            KFPart.Chi2 += (S[0] * zeta[0] + S[1] * zeta[1] + S[3] * zeta[2]) * zeta[0]
                           + (S[1] * zeta[0] + S[2] * zeta[1] + S[4] * zeta[2]) * zeta[1]
                           + (S[3] * zeta[0] + S[4] * zeta[1] + S[5] * zeta[2]) * zeta[2];
 
        }   // add tracks
 
        if (iteration == 0) {
            r0[3] = KFPart.r[3];
            r0[4] = KFPart.r[4];
            r0[5] = KFPart.r[5];
            r0[6] = KFPart.r[6];
            if (Parent) {
                fvec dx, dy, dz;
                dx = Parent->GetRefX() - KFPart.r[0];
                dy = Parent->GetRefY() - KFPart.r[1];
                dz = Parent->GetRefZ() - KFPart.r[2];
 
                r0[7] = KFPart.r[7] =
                    sqrt((dx * dx + dy * dy + dz * dz)
                         / (KFPart.r[3] * KFPart.r[3] + KFPart.r[4] * KFPart.r[4] + KFPart.r[5] * KFPart.r[5]));
            }
        }
 
        if (Mass >= 0) {
            fvec vMass = Mass;
            MeasureMass(&KFPart, r0, vMass);
        }
        if (Parent)
            MeasureProductionVertex(&KFPart, r0, Parent);
    }   // iterations
 
    // calculate a field region for the constructed particle
    L1FieldValue field[3];
    fvec zField[3] = {0, KFPart.r[2] / 2, KFPart.r[2]};
 
    for (int iPoint = 0; iPoint < 3; iPoint++) {
        for (int iD = 0; iD < NDaughters; ++iD) {
            L1FieldValue b = vDaughters[iD].fField.Get(zField[iPoint]);
            field[iPoint].x += b.x;
            field[iPoint].y += b.y;
            field[iPoint].z += b.z;
        }
        field[iPoint].x /= NDaughters;
        field[iPoint].y /= NDaughters;
        field[iPoint].z /= NDaughters;
    }
 
    KFPart.fField.Set(field[2], zField[2], field[1], zField[1], field[0], zField[0]);
 
    KFPart.AtProductionVertex = 0;
}
 
void CbmKFParticleInterface::MeasureMass(CbmKFParticle_simd* Particle, fvec r0[], fvec Mass)
{
    fvec* r = Particle->GetParameters();
    fvec* C = Particle->GetCovMatrix();
 
    fvec H[8] = {0};
    H[0] = H[1] = H[2] = 0.;
    H[3] = -2 * r0[3];
    H[4] = -2 * r0[4];
    H[5] = -2 * r0[5];
    H[6] = 2 * r0[6];
    H[7] = 0;
    fvec m2 = r0[6] * r0[6] - r0[3] * r0[3] - r0[4] * r0[4] - r0[5] * r0[5];
 
    fvec zeta = Mass * Mass - m2;
    for (Int_t i = 0; i < 8; ++i)
        zeta -= H[i] * (r[i] - r0[i]);
 
    fvec S = 0.;
    fvec CHt[8];
    for (Int_t i = 0; i < 8; ++i) {
        CHt[i] = 0.0;
        for (Int_t j = 0; j < 8; ++j)
            CHt[i] += Cij(i, j) * H[j];
        S += H[i] * CHt[i];
    }
 
    fvec init = fvec(S > 1.E-20);
    S = rcp(S) & init;
 
    Particle->Chi2 += zeta * zeta * S;
    Particle->NDF += (fvec(1.) & init);
    for (Int_t i = 0, ii = 0; i < 8; ++i) {
        fvec Ki = CHt[i] * S;
        r[i] += Ki * zeta;
        for (Int_t j = 0; j <= i; ++j)
            C[ii++] -= Ki * CHt[j];
    }
}
 
void CbmKFParticleInterface::MeasureProductionVertex(CbmKFParticle_simd* Particle,
                                                     fvec r0[],
                                                     CbmKFVertexInterface* Parent)
{
    fvec* r = Particle->GetParameters();
    fvec* C = Particle->GetCovMatrix();
 
    fvec m[3], V[6];
 
    m[0] = Parent->GetRefX();
    m[1] = Parent->GetRefY();
    m[2] = Parent->GetRefZ();
    double* V1 = Parent->GetCovMatrix();
    for (int i = 0; i < 6; i++)
        V[i] = V1[i];
 
    r[7] = r0[7];
    Extrapolate(r0, -r0[7]);
    Convert(Particle, r0, 1);
 
    fvec Ai[6];
    Ai[0] = C[4] * C[4] - C[2] * C[5];
    Ai[1] = C[1] * C[5] - C[3] * C[4];
    Ai[3] = C[2] * C[3] - C[1] * C[4];
    fvec det = 1. / (C[0] * Ai[0] + C[1] * Ai[1] + C[3] * Ai[3]);
    Ai[0] *= det;
    Ai[1] *= det;
    Ai[3] *= det;
    Ai[2] = (C[3] * C[3] - C[0] * C[5]) * det;
    Ai[4] = (C[0] * C[4] - C[1] * C[3]) * det;
    Ai[5] = (C[1] * C[1] - C[0] * C[2]) * det;
 
    fvec B[5][3];
 
    B[0][0] = C[6] * Ai[0] + C[7] * Ai[1] + C[8] * Ai[3];
    B[0][1] = C[6] * Ai[1] + C[7] * Ai[2] + C[8] * Ai[4];
    B[0][2] = C[6] * Ai[3] + C[7] * Ai[4] + C[8] * Ai[5];
 
    B[1][0] = C[10] * Ai[0] + C[11] * Ai[1] + C[12] * Ai[3];
    B[1][1] = C[10] * Ai[1] + C[11] * Ai[2] + C[12] * Ai[4];
    B[1][2] = C[10] * Ai[3] + C[11] * Ai[4] + C[12] * Ai[5];
 
    B[2][0] = C[15] * Ai[0] + C[16] * Ai[1] + C[17] * Ai[3];
    B[2][1] = C[15] * Ai[1] + C[16] * Ai[2] + C[17] * Ai[4];
    B[2][2] = C[15] * Ai[3] + C[16] * Ai[4] + C[17] * Ai[5];
 
    B[3][0] = C[21] * Ai[0] + C[22] * Ai[1] + C[23] * Ai[3];
    B[3][1] = C[21] * Ai[1] + C[22] * Ai[2] + C[23] * Ai[4];
    B[3][2] = C[21] * Ai[3] + C[22] * Ai[4] + C[23] * Ai[5];
 
    B[4][0] = C[28] * Ai[0] + C[29] * Ai[1] + C[30] * Ai[3];
    B[4][1] = C[28] * Ai[1] + C[29] * Ai[2] + C[30] * Ai[4];
    B[4][2] = C[28] * Ai[3] + C[29] * Ai[4] + C[30] * Ai[5];
 
    fvec z[3] = {m[0] - r[0], m[1] - r[1], m[2] - r[2]};
 
    {
        fvec AV[6] = {C[0] - V[0], C[1] - V[1], C[2] - V[2], C[3] - V[3], C[4] - V[4], C[5] - V[5]};
        fvec AVi[6];
        AVi[0] = AV[4] * AV[4] - AV[2] * AV[5];
        AVi[1] = AV[1] * AV[5] - AV[3] * AV[4];
        AVi[2] = AV[3] * AV[3] - AV[0] * AV[5];
        AVi[3] = AV[2] * AV[3] - AV[1] * AV[4];
        AVi[4] = AV[0] * AV[4] - AV[1] * AV[3];
        AVi[5] = AV[1] * AV[1] - AV[0] * AV[2];
 
        det = 1. / (AV[0] * AVi[0] + AV[1] * AVi[1] + AV[3] * AVi[3]);
 
        Particle->NDF += 2.;
        Particle->Chi2 += (+(AVi[0] * z[0] + AVi[1] * z[1] + AVi[3] * z[2]) * z[0]
                           + (AVi[1] * z[0] + AVi[2] * z[1] + AVi[4] * z[2]) * z[1]
                           + (AVi[3] * z[0] + AVi[4] * z[1] + AVi[5] * z[2]) * z[2])
                          * det;
    }
 
    r[0] = m[0];
    r[1] = m[1];
    r[2] = m[2];
    r[3] += B[0][0] * z[0] + B[0][1] * z[1] + B[0][2] * z[2];
    r[4] += B[1][0] * z[0] + B[1][1] * z[1] + B[1][2] * z[2];
    r[5] += B[2][0] * z[0] + B[2][1] * z[1] + B[2][2] * z[2];
    r[6] += B[3][0] * z[0] + B[3][1] * z[1] + B[3][2] * z[2];
    r[7] += B[4][0] * z[0] + B[4][1] * z[1] + B[4][2] * z[2];
 
    fvec d0, d1, d2;
 
    C[0] = V[0];
    C[1] = V[1];
    C[2] = V[2];
    C[3] = V[3];
    C[4] = V[4];
    C[5] = V[5];
 
    d0 = B[0][0] * V[0] + B[0][1] * V[1] + B[0][2] * V[3] - C[6];
    d1 = B[0][0] * V[1] + B[0][1] * V[2] + B[0][2] * V[4] - C[7];
    d2 = B[0][0] * V[3] + B[0][1] * V[4] + B[0][2] * V[5] - C[8];
 
    C[6] += d0;
    C[7] += d1;
    C[8] += d2;
    C[9] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
 
    d0 = B[1][0] * V[0] + B[1][1] * V[1] + B[1][2] * V[3] - C[10];
    d1 = B[1][0] * V[1] + B[1][1] * V[2] + B[1][2] * V[4] - C[11];
    d2 = B[1][0] * V[3] + B[1][1] * V[4] + B[1][2] * V[5] - C[12];
 
    C[10] += d0;
    C[11] += d1;
    C[12] += d2;
    C[13] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
    C[14] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
 
    d0 = B[2][0] * V[0] + B[2][1] * V[1] + B[2][2] * V[3] - C[15];
    d1 = B[2][0] * V[1] + B[2][1] * V[2] + B[2][2] * V[4] - C[16];
    d2 = B[2][0] * V[3] + B[2][1] * V[4] + B[2][2] * V[5] - C[17];
 
    C[15] += d0;
    C[16] += d1;
    C[17] += d2;
    C[18] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
    C[19] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
    C[20] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
 
    d0 = B[3][0] * V[0] + B[3][1] * V[1] + B[3][2] * V[3] - C[21];
    d1 = B[3][0] * V[1] + B[3][1] * V[2] + B[3][2] * V[4] - C[22];
    d2 = B[3][0] * V[3] + B[3][1] * V[4] + B[3][2] * V[5] - C[23];
 
    C[21] += d0;
    C[22] += d1;
    C[23] += d2;
    C[24] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
    C[25] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
    C[26] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
    C[27] += d0 * B[3][0] + d1 * B[3][1] + d2 * B[3][2];
 
    d0 = B[4][0] * V[0] + B[4][1] * V[1] + B[4][2] * V[3] - C[28];
    d1 = B[4][0] * V[1] + B[4][1] * V[2] + B[4][2] * V[4] - C[29];
    d2 = B[4][0] * V[3] + B[4][1] * V[4] + B[4][2] * V[5] - C[30];
 
    C[28] += d0;
    C[29] += d1;
    C[30] += d2;
    C[31] += d0 * B[0][0] + d1 * B[0][1] + d2 * B[0][2];
    C[32] += d0 * B[1][0] + d1 * B[1][1] + d2 * B[1][2];
    C[33] += d0 * B[2][0] + d1 * B[2][1] + d2 * B[2][2];
    C[34] += d0 * B[3][0] + d1 * B[3][1] + d2 * B[3][2];
    C[35] += d0 * B[4][0] + d1 * B[4][1] + d2 * B[4][2];
 
    Extrapolate(Particle, r0, r[7]);
    Convert(Particle, r0, 0);
}
 
void CbmKFParticleInterface::Convert(CbmKFParticle_simd* Particle, fvec r0[], bool ToProduction)
{
    fvec* C = Particle->GetCovMatrix();
 
    L1FieldValue B = KFPart.fField.Get(r0[2]);
    B.x *= 0.000299792458;   // multiply by c
    B.y *= 0.000299792458;
    B.z *= 0.000299792458;
 
    fvec h[6];
 
    h[0] = r0[3];
    h[1] = r0[4];
    h[2] = r0[5];
    if (ToProduction) {
        h[0] = -h[0];
        h[1] = -h[1];
        h[2] = -h[2];
    }
    h[3] = h[1] * B.z - h[2] * B.y;
    h[4] = h[2] * B.x - h[0] * B.z;
    h[5] = h[0] * B.y - h[1] * B.x;
 
    fvec c;
 
    c = C[28] + h[0] * C[35];
    C[0] += h[0] * (c + C[28]);
    C[28] = c;
 
    C[1] += h[1] * C[28] + h[0] * C[29];
    c = C[29] + h[1] * C[35];
    C[2] += h[1] * (c + C[29]);
    C[29] = c;
 
    C[3] += h[2] * C[28] + h[0] * C[30];
    C[4] += h[2] * C[29] + h[1] * C[30];
    c = C[30] + h[2] * C[35];
    C[5] += h[2] * (c + C[30]);
    C[30] = c;
 
    C[6] += h[3] * C[28] + h[0] * C[31];
    C[7] += h[3] * C[29] + h[1] * C[31];
    C[8] += h[3] * C[30] + h[2] * C[31];
    c = C[31] + h[3] * C[35];
    C[9] += h[3] * (c + C[31]);
    C[31] = c;
 
    C[10] += h[4] * C[28] + h[0] * C[32];
    C[11] += h[4] * C[29] + h[1] * C[32];
    C[12] += h[4] * C[30] + h[2] * C[32];
    C[13] += h[4] * C[31] + h[3] * C[32];
    c = C[32] + h[4] * C[35];
    C[14] += h[4] * (c + C[32]);
    C[32] = c;
 
    C[15] += h[5] * C[28] + h[0] * C[33];
    C[16] += h[5] * C[29] + h[1] * C[33];
    C[17] += h[5] * C[30] + h[2] * C[33];
    C[18] += h[5] * C[31] + h[3] * C[33];
    C[19] += h[5] * C[32] + h[4] * C[33];
    c = C[33] + h[5] * C[35];
    C[20] += h[5] * (c + C[33]);
    C[33] = c;
 
    C[21] += h[0] * C[34];
    C[22] += h[1] * C[34];
    C[23] += h[2] * C[34];
    C[24] += h[3] * C[34];
    C[25] += h[4] * C[34];
    C[26] += h[5] * C[34];
}
 
void CbmKFParticleInterface::ExtrapolateLine(CbmKFParticle_simd* Particle, fvec r0[], fvec dS)
{
    fvec* r = Particle->GetParameters();
    fvec* C = Particle->GetCovMatrix();
 
    if (r0 && r0 != r) {
        r0[0] += dS * r0[3];
        r0[1] += dS * r0[4];
        r0[2] += dS * r0[5];
    }
 
    r[0] += dS * r[3];
    r[1] += dS * r[4];
    r[2] += dS * r[5];
 
    fvec C6 = C[6] + dS * C[9];
    fvec C11 = C[11] + dS * C[14];
    fvec C17 = C[17] + dS * C[20];
    fvec SC13 = dS * C[13];
    fvec SC18 = dS * C[18];
    fvec SC19 = dS * C[19];
 
    C[0] += dS * (C[6] + C6);
    C[2] += dS * (C[11] + C11);
    C[5] += dS * (C[17] + C17);
 
    C[7] += SC13;
    C[8] += SC18;
    C[12] += SC19;
 
    C[1] += dS * (C[10] + C[7]);
    C[3] += dS * (C[15] + C[8]);
    C[4] += dS * (C[16] + C[12]);
 
    C[6] = C6;
    C[10] += SC13;
    C[11] = C11;
    C[15] += SC18;
    C[16] += SC19;
    C[17] = C17;
 
    C[21] += dS * C[24];
    C[22] += dS * C[25];
    C[23] += dS * C[26];
    C[28] += dS * C[31];
    C[29] += dS * C[32];
    C[30] += dS * C[33];
}
 
void CbmKFParticleInterface::Extrapolate(CbmKFParticle_simd* Particle, fvec r0[], fvec dS)
{
    if (Particle->Q[0] == 0) {
        ExtrapolateLine(Particle, r0, dS);
        return;
    }
 
    fvec* r = Particle->GetParameters();
    fvec* C = Particle->GetCovMatrix();
 
    const float c_light = 0.000299792458;
 
    fvec c = Particle->Q * c_light;
 
    // construct coefficients
 
    fvec px = r[3], py = r[4], pz = r[5];
 
    fvec sx = 0, sy = 0, sz = 0, syy = 0, syz = 0, syyy = 0, Sx = 0, Sy = 0, Sz = 0, Syy = 0, Syz = 0, Syyy = 0;
 
    {   // get field integrals
 
        L1FieldValue B[3];
        fvec p0[3], p1[3], p2[3];
 
        // line track approximation
 
        p0[0] = r[0];
        p0[1] = r[1];
        p0[2] = r[2];
 
        p2[0] = r[0] + px * dS;
        p2[1] = r[1] + py * dS;
        p2[2] = r[2] + pz * dS;
 
        p1[0] = 0.5 * (p0[0] + p2[0]);
        p1[1] = 0.5 * (p0[1] + p2[1]);
        p1[2] = 0.5 * (p0[2] + p2[2]);
 
        B[0] = Particle->fField.Get(p0[2]);
        B[1] = Particle->fField.Get(p1[2]);
        B[2] = Particle->fField.Get(p2[2]);
 
        // first order track approximation
 
        fvec Sy1 = (7 * B[0].y + 6 * B[1].y - B[2].y) * c * dS * dS / 96.;
        fvec Sy2 = (B[0].y + 2 * B[1].y) * c * dS * dS / 6.;
 
        p1[0] -= Sy1 * pz;
        p1[2] += Sy1 * px;
        p2[0] -= Sy2 * pz;
        p2[2] += Sy2 * px;
 
        B[0] = Particle->fField.Get(p0[2]);
        B[1] = Particle->fField.Get(p1[2]);
        B[2] = Particle->fField.Get(p2[2]);
 
        sx = c * (B[0].x + 4 * B[1].x + B[2].x) * dS / 6.;
        sy = c * (B[0].y + 4 * B[1].y + B[2].y) * dS / 6.;
        sz = c * (B[0].z + 4 * B[1].z + B[2].z) * dS / 6.;
 
        Sx = c * (B[0].x + 2 * B[1].x) * dS * dS / 6.;
        Sy = c * (B[0].y + 2 * B[1].y) * dS * dS / 6.;
        Sz = c * (B[0].z + 2 * B[1].z) * dS * dS / 6.;
 
        fvec c2[3][3] = {{5, -4, -1}, {44, 80, -4}, {11, 44, 5}};     // /=360.
        fvec C2[3][3] = {{38, 8, -4}, {148, 208, -20}, {3, 36, 3}};   // /=2520.
        for (Int_t n = 0; n < 3; n++)
            for (Int_t m = 0; m < 3; m++) {
                syz += c2[n][m] * B[n].y * B[m].z;
                Syz += C2[n][m] * B[n].y * B[m].z;
            }
 
        syz *= c * c * dS * dS / 360.;
        Syz *= c * c * dS * dS * dS / 2520.;
 
        syy = c * (B[0].y + 4 * B[1].y + B[2].y) * dS;
        syyy = syy * syy * syy / 1296;
        syy = syy * syy / 72;
 
        Syy = (B[0].y * (38 * B[0].y + 156 * B[1].y - B[2].y) + B[1].y * (208 * B[1].y + 16 * B[2].y)
               + B[2].y * (3 * B[2].y))
              * dS * dS * dS * c * c / 2520.;
        Syyy = (B[0].y
                    * (B[0].y * (85 * B[0].y + 526 * B[1].y - 7 * B[2].y) + B[1].y * (1376 * B[1].y + 84 * B[2].y)
                       + B[2].y * (19 * B[2].y))
                + B[1].y * (B[1].y * (1376 * B[1].y + 256 * B[2].y) + B[2].y * (62 * B[2].y))
                + B[2].y * B[2].y * (3 * B[2].y))
               * dS * dS * dS * dS * c * c * c / 90720.;
    }
 
    fvec J[11];
    //   for( int i=0; i<8; i++ ) for( int j=0; j<8; j++) J[i][j]=0;
    //
    //   J[0][0]=1; J[0][1]=0; J[0][2]=0; J[0][3]=dS-Syy;  J[0][4]=Sx;  J[0][5]=Syyy-Sy;
    //   J[1][0]=0; J[1][1]=1; J[1][2]=0; J[1][3]=-Sz;     J[1][4]=dS;  J[1][5]=Sx+Syz;
    //   J[2][0]=0; J[2][1]=0; J[2][2]=1; J[2][3]=Sy-Syyy; J[2][4]=-Sx; J[2][5]=dS-Syy;
    //
    //   J[3][0]=0; J[3][1]=0; J[3][2]=0; J[3][3]=1-syy;   J[3][4]=sx;  J[3][5]=syyy-sy;
    //   J[4][0]=0; J[4][1]=0; J[4][2]=0; J[4][3]=-sz;     J[4][4]=1;   J[4][5]=sx+syz;
    //   J[5][0]=0; J[5][1]=0; J[5][2]=0; J[5][3]=sy-syyy; J[5][4]=-sx; J[5][5]=1-syy;
    //   J[6][6] = J[7][7] = 1;
 
    J[0] = dS - Syy;
    J[1] = Sx;
    J[2] = Syyy - Sy;
    J[3] = -Sz;
    J[4] = dS;
    J[5] = Sx + Syz;
 
    J[6] = 1 - syy;
    J[7] = sx;
    J[8] = syyy - sy;
    J[9] = -sz;
    J[10] = sx + syz;
 
    r[0] += J[0] * px + J[1] * py + J[2] * pz;
    r[1] += J[3] * px + J[4] * py + J[5] * pz;
    r[2] += -J[2] * px - J[1] * py + J[0] * pz;
    r[3] = J[6] * px + J[7] * py + J[8] * pz;
    r[4] = J[9] * px + py + J[10] * pz;
    r[5] = -J[8] * px - J[7] * py + J[6] * pz;
 
    if (r0 && r0 != r) {
        px = r0[3];
        py = r0[4];
        pz = r0[5];
 
        r0[0] += J[0] * px + J[1] * py + J[2] * pz;
        r0[1] += J[3] * px + J[4] * py + J[5] * pz;
        r0[2] += -J[2] * px - J[1] * py + J[0] * pz;
        r0[3] = J[6] * px + J[7] * py + J[8] * pz;
        r0[4] = J[9] * px + py + J[10] * pz;
        r0[5] = -J[8] * px - J[7] * py + J[6] * pz;
    }
 
    multQSQt1(J, C);
}
 
void CbmKFParticleInterface::multQSQt(const fvec Q[], fvec S[])
{
    fvec A[64] = {0};
 
    for (Int_t i = 0, n = 0; i < 8; i++)
        for (Int_t j = 0; j < 8; j++, ++n)
            for (Int_t k = 0; k < 8; ++k)
                A[n] += S[IJ(i, k)] * Q[j * 8 + k];
 
    for (Int_t i = 0; i < 8; i++) {
        for (Int_t j = 0; j <= i; j++) {
            Int_t n = IJ(i, j);
            S[n] = 0;
            for (Int_t k = 0; k < 8; k++)
                S[n] += Q[i * 8 + k] * A[k * 8 + j];
        }
    }
}
 
void CbmKFParticleInterface::multQSQt1(const fvec J[11], fvec S[])
{
    const fvec A00 = S[0] + S[6] * J[0] + S[10] * J[1] + S[15] * J[2];
    const fvec A10 = S[1] + S[7] * J[0] + S[11] * J[1] + S[16] * J[2];
    const fvec A20 = S[3] + S[8] * J[0] + S[12] * J[1] + S[17] * J[2];
    const fvec A30 = S[6] + S[9] * J[0] + S[13] * J[1] + S[18] * J[2];
    const fvec A40 = S[10] + S[13] * J[0] + S[14] * J[1] + S[19] * J[2];
    const fvec A50 = S[15] + S[18] * J[0] + S[19] * J[1] + S[20] * J[2];
    const fvec A60 = S[21] + S[24] * J[0] + S[25] * J[1] + S[26] * J[2];
    const fvec A70 = S[28] + S[31] * J[0] + S[32] * J[1] + S[33] * J[2];
 
    S[0] = A00 + J[0] * A30 + J[1] * A40 + J[2] * A50;
    S[1] = A10 + J[3] * A30 + J[4] * A40 + J[5] * A50;
    S[3] = A20 - J[2] * A30 - J[1] * A40 + J[0] * A50;
    S[6] = J[6] * A30 + J[7] * A40 + J[8] * A50;
    S[10] = J[9] * A30 + A40 + J[10] * A50;
    S[15] = -J[8] * A30 - J[7] * A40 + J[6] * A50;
    S[21] = A60;
    S[28] = A70;
 
    //  const fvec A01 = S[1 ]+S[6 ]*J[3]+S[10]*J[4]+S[15]*J[5];
    const fvec A11 = S[2] + S[7] * J[3] + S[11] * J[4] + S[16] * J[5];
    const fvec A21 = S[4] + S[8] * J[3] + S[12] * J[4] + S[17] * J[5];
    const fvec A31 = S[7] + S[9] * J[3] + S[13] * J[4] + S[18] * J[5];
    const fvec A41 = S[11] + S[13] * J[3] + S[14] * J[4] + S[19] * J[5];
    const fvec A51 = S[16] + S[18] * J[3] + S[19] * J[4] + S[20] * J[5];
    const fvec A61 = S[22] + S[24] * J[3] + S[25] * J[4] + S[26] * J[5];
    const fvec A71 = S[29] + S[31] * J[3] + S[32] * J[4] + S[33] * J[5];
 
    S[2] = A11 + J[3] * A31 + J[4] * A41 + J[5] * A51;
    S[4] = A21 - J[2] * A31 - J[1] * A41 + J[0] * A51;
    S[7] = J[6] * A31 + J[7] * A41 + J[8] * A51;
    S[11] = J[9] * A31 + A41 + J[10] * A51;
    S[16] = -J[8] * A31 - J[7] * A41 + J[6] * A51;
    S[22] = A61;
    S[29] = A71;
 
    //  const fvec A02 = S[3 ]+S[6 ]*J[2][3]+S[10]*J[2][4]+S[15]*J[2][5];
    //  const fvec A12 = S[4 ]+S[7 ]*J[2][3]+S[11]*J[2][4]+S[16]*J[2][5];
    const fvec A22 = S[5] - S[8] * J[2] - S[12] * J[1] + S[17] * J[0];
    const fvec A32 = S[8] - S[9] * J[2] - S[13] * J[1] + S[18] * J[0];
    const fvec A42 = S[12] - S[13] * J[2] - S[14] * J[1] + S[19] * J[0];
    const fvec A52 = S[17] - S[18] * J[2] - S[19] * J[1] + S[20] * J[0];
    const fvec A62 = S[23] - S[24] * J[2] - S[25] * J[1] + S[26] * J[0];
    const fvec A72 = S[30] - S[31] * J[2] - S[32] * J[1] + S[33] * J[0];
 
    S[5] = A22 - J[2] * A32 - J[1] * A42 + J[0] * A52;
    S[8] = J[6] * A32 + J[7] * A42 + J[8] * A52;
    S[12] = J[9] * A32 + A42 + J[10] * A52;
    S[17] = -J[8] * A32 - J[7] * A42 + J[6] * A52;
    S[23] = A62;
    S[30] = A72;
 
    //  const fvec A03 = S[6] *J[6]+S[10]*J[7]+S[15]*J[8];
    //  const fvec A13 = S[7] *J[6]+S[11]*J[7]+S[16]*J[8];
    //  const fvec A23 = S[8] *J[6]+S[12]*J[7]+S[17]*J[8];
    const fvec A33 = S[9] * J[6] + S[13] * J[7] + S[18] * J[8];
    const fvec A43 = S[13] * J[6] + S[14] * J[7] + S[19] * J[8];
    const fvec A53 = S[18] * J[6] + S[19] * J[7] + S[20] * J[8];
    const fvec A63 = S[24] * J[6] + S[25] * J[7] + S[26] * J[8];
    const fvec A73 = S[31] * J[6] + S[32] * J[7] + S[33] * J[8];
 
    //  const fvec A04 = S[6] *J[9]+S[10]*J[4][4]+S[15]*J[10];
    //  const fvec A14 = S[7] *J[9]+S[11]*J[4][4]+S[16]*J[10];
    //  const fvec A24 = S[8] *J[9]+S[12]*J[4][4]+S[17]*J[10];
    const fvec A34 = S[9] * J[9] + S[13] + S[18] * J[10];
    const fvec A44 = S[13] * J[9] + S[14] + S[19] * J[10];
    const fvec A54 = S[18] * J[9] + S[19] + S[20] * J[10];
    const fvec A64 = S[24] * J[9] + S[25] + S[26] * J[10];
    const fvec A74 = S[31] * J[9] + S[32] + S[33] * J[10];
 
    //  const fvec A05 = S[6] *J[5][3]+S[10]*J[5][4]+S[15]*J[6];
    //  const fvec A15 = S[7] *J[5][3]+S[11]*J[5][4]+S[16]*J[6];
    //  const fvec A25 = S[8] *J[5][3]+S[12]*J[5][4]+S[17]*J[6];
    const fvec A35 = -S[9] * J[8] - S[13] * J[7] + S[18] * J[6];
    const fvec A45 = -S[13] * J[8] - S[14] * J[7] + S[19] * J[6];
    const fvec A55 = -S[18] * J[8] - S[19] * J[7] + S[20] * J[6];
    const fvec A65 = -S[24] * J[8] - S[25] * J[7] + S[26] * J[6];
    const fvec A75 = -S[31] * J[8] - S[32] * J[7] + S[33] * J[6];
 
    S[9] = J[6] * A33 + J[7] * A43 + J[8] * A53;
    S[13] = J[9] * A33 + A43 + J[10] * A53;
    S[18] = -J[8] * A33 - J[7] * A43 + J[6] * A53;
    S[24] = A63;
    S[31] = A73;
 
    S[14] = J[9] * A34 + A44 + J[10] * A54;
    S[19] = -J[8] * A34 - J[7] * A44 + J[6] * A54;
    S[25] = A64;
    S[32] = A74;
 
    S[20] = -J[8] * A35 - J[7] * A45 + J[6] * A55;
    S[26] = A65;
    S[33] = A75;
 
    // S[27] = S27;
    // S[34] = S34;
    // S[35] = S35;
}
 
void CbmKFParticleInterface::Extrapolate(fvec r0[], fvec dS)
{
    Extrapolate(&KFPart, r0, dS);
}
 
void CbmKFParticleInterface::TransportToProductionVertex()
{
    if (KFPart.AtProductionVertex)
        return;
    fvec r0[8];
    for (int i = 0; i < 8; i++)
        r0[i] = KFPart.r[i];
    Extrapolate(r0, -(KFPart.r[7]));
    Convert(&KFPart, r0, 1);
    KFPart.AtProductionVertex = 1;
}
 
void CbmKFParticleInterface::TransportToDecayVertex()
{
    if (!KFPart.AtProductionVertex)
        return;
    fvec r0[8];
    for (int i = 0; i < 8; i++)
        r0[i] = KFPart.r[i];
    Extrapolate(r0, KFPart.r[7]);
    Convert(&KFPart, r0, 0);
    KFPart.AtProductionVertex = 0;
}
 
fvec CbmKFParticleInterface::GetDStoPoint(const CbmKFParticle_simd& Particle, const fvec xyz[]) const
{
    //   const fvec *r = Particle.GetParameters();
    //
    //   const fvec dx = xyz[0] - r[0];
    //   const fvec dy = xyz[1] - r[1];
    //   const fvec dz = xyz[2] - r[2];
    //   const fvec p2 = r[3]*r[3] + r[4]*r[4]+r[5]*r[5];
    //
    //   L1FieldValue B = const_cast<L1FieldRegion*>(&Particle.fField)->Get(r[2]);
    //   B.x *= 0.000299792458; //multiply by c
    //   B.y *= 0.000299792458;
    //   B.z *= 0.000299792458;
    //
    //   fvec bq1 = B.x*Particle.Q;
    //   fvec bq2 = B.y*Particle.Q;
    //   fvec bq3 = B.z*Particle.Q;
    //   fvec a = dx*r[3]+dy*r[4]+dz*r[5];
    //   fvec B2 = B.x*B.x+B.y*B.y+B.z*B.z;
    //   fvec bq = Particle.Q*sqrt(B2);
    //   fvec pB = r[3]*B.x+r[4]*B.y+r[5]*B.z;
    //   fvec rB = dx*B.x+dy*B.y+dz*B.z;
    //
    //   fvec small = 0.0001;
    //   fvec init = fvec(B2 > 0.0001);
    //   B2 = (init & B2) + (fvec(!init) & small);
    //   fvec pt2 = p2 - pB*pB/B2;
    //   a = a - pB*rB/B2;
    //
    //   fvec dS = atan2(bq*a, pt2 + bq1*(dz*r[4]-dy*r[5]) - bq2*(dz*r[3]-dx*r[5])+bq3*(dy*r[3]-dx*r[4]))/bq;
    //   fvec dS2 = a/pt2;
    //   fvec smallBq = 1.e-8;
    //   fvec initBq = fvec(fabs(bq) > smallBq);
    //   dS = (initBq & dS) + (fvec(!initBq) & dS2);
    //   fvec initPt2 = fvec(pt2 > small);
    //   dS = (initPt2 & dS) + (fvec(!initPt2) & fvec(0));
    //
    //   //fvec dSm = rB/pB;
    //  return dS;
 
    const fvec* r = Particle.GetParameters();
    fvec p2 = r[3] * r[3] + r[4] * r[4] + r[5] * r[5];
    //  if( p2<1.e-4 ) p2 = 1;
    return (r[3] * (xyz[0] - r[0]) + r[4] * (xyz[1] - r[1]) + r[5] * (xyz[2] - r[2])) / p2;
}
 
fvec CbmKFParticleInterface::GetDStoPoint(const fvec xyz[]) const
{
    return GetDStoPoint(KFPart, xyz);
}
 
void CbmKFParticleInterface::GetKFVertex(CbmKFVertex vtx[])
{
    float r0[fvecLen], r1[fvecLen], r2[fvecLen];
    float C_temp[6][fvecLen];
    float Chi2_temp[fvecLen];
    int NDF_temp[fvecLen];
 
    for (int j = 0; j < fvecLen; j++) {
        r0[j] = KFPart.r[0][j];
        r1[j] = KFPart.r[1][j];
        r2[j] = KFPart.r[2][j];
        for (int i = 0; i < 6; i++)
            C_temp[i][j] = KFPart.C[i][j];
 
        Chi2_temp[j] = KFPart.Chi2[j];
        NDF_temp[j] = (int)KFPart.NDF[j];
    }
 
    for (int j = 0; j < fvecLen; j++)
        vtx[j] = GetKFVertexJ(j);
}
 
CbmKFVertex CbmKFParticleInterface::GetKFVertexJ(int j)
{
    CbmKFVertex vtx;
    vtx.GetRefX() = KFPart.r[0][j];
    vtx.GetRefY() = KFPart.r[1][j];
    vtx.GetRefZ() = KFPart.r[2][j];
    for (int i = 0; i < 6; i++)
        vtx.GetCovMatrix()[i] = KFPart.C[i][j];
    vtx.GetRefChi2() = KFPart.Chi2[j];
    vtx.GetRefNDF() = (int)KFPart.NDF[j];
 
    return vtx;
}
 
void CbmKFParticleInterface::GetKFVertexJ(int j, CbmKFVertex* vtx)
{
    vtx->GetRefX() = KFPart.r[0][j];
    vtx->GetRefY() = KFPart.r[1][j];
    vtx->GetRefZ() = KFPart.r[2][j];
    for (int i = 0; i < 6; i++)
        vtx->GetCovMatrix()[i] = KFPart.C[i][j];
    vtx->GetRefChi2() = KFPart.Chi2[j];
    vtx->GetRefNDF() = (int)KFPart.NDF[j];
}
 
void CbmKFParticleInterface::GetKFParticle(CbmKFParticle& Part, int iPart)
{
    Part.SetId(static_cast<int>(KFPart.Id()[iPart]));
 
    Part.CleanDaughtersId();
    Part.SetNDaughters(KFPart.DaughterIds().size());
    for (unsigned int i = 0; i < KFPart.DaughterIds().size(); i++)
        Part.AddDaughter(static_cast<int>(KFPart.DaughterIds()[i][iPart]));
 
    Part.SetPDG(static_cast<int>(KFPart.GetPDG()[iPart]));
 
    for (int iP = 0; iP < 8; iP++)
        Part.GetParameters()[iP] = KFPart.GetParameters()[iP][iPart];
    for (int iC = 0; iC < 36; iC++)
        Part.GetCovMatrix()[iC] = KFPart.GetCovMatrix()[iC][iPart];
 
    Part.NDF = static_cast<int>(KFPart.GetNDF()[iPart]);
    Part.Chi2 = KFPart.GetChi2()[iPart];
    Part.Q = KFPart.GetQ()[iPart];
    Part.AtProductionVertex = KFPart.AtProductionVertex;
 
    Part.fieldRegion[0] = KFPart.fField.cx0[iPart];
    Part.fieldRegion[1] = KFPart.fField.cx1[iPart];
    Part.fieldRegion[2] = KFPart.fField.cx2[iPart];
    Part.fieldRegion[3] = KFPart.fField.cy0[iPart];
    Part.fieldRegion[4] = KFPart.fField.cy1[iPart];
    Part.fieldRegion[5] = KFPart.fField.cy2[iPart];
    Part.fieldRegion[6] = KFPart.fField.cz0[iPart];
    Part.fieldRegion[7] = KFPart.fField.cz1[iPart];
    Part.fieldRegion[8] = KFPart.fField.cz2[iPart];
    Part.fieldRegion[9] = KFPart.fField.z0[iPart];
}
 
void CbmKFParticleInterface::GetKFParticle(CbmKFParticle* Part, int nPart)
{
    for (int i = 0; i < nPart; i++)
        GetKFParticle(Part[i], i);
}
 
void CbmKFParticleInterface::SetVtxGuess(fvec& x, fvec& y, fvec& z)
{
    KFPart.SetVtxGuess(x, y, z);
}
 
void CbmKFParticleInterface::SetVtxErrGuess(fvec& dx, fvec& dy, fvec& dz)
{
    KFPart.SetVtxErrGuess(dx, dy, dz);
}
 
void CbmKFParticleInterface::GetMomentum(fvec& P, fvec& Error)
{
    KFPart.GetMomentum(P, Error);
}
 
void CbmKFParticleInterface::GetMass(fvec& M, fvec& Error)
{
    KFPart.GetMass(M, Error);
}
 
// template<class T>
void CbmKFParticleInterface::FindParticles(vector<CbmKFTrack>& vRTracks,
                                           vector<float>& ChiToPrimVtx,
                                           vector<L1FieldRegion>& vField,
                                           vector<CbmKFParticle>& Particles,
                                           CbmKFVertex& PrimVtx,
                                           const vector<int>& vTrackPDG,
                                           const float cuts[2][3])
{
    //* Finds particles (K0s and Lambda) from a given set of tracks
 
    static const int NTrackTypes = 8;
 
    int pdgPos[NTrackTypes] = {-11, -13, 211, 321, 2212, 211, 321, 2212};
    int pdgNeg[NTrackTypes] = {11, 13, -211, -321, -2212, -211, -321, -2212};
 
    vector<short> idPosSec[NTrackTypes];
    vector<short> idNegSec[NTrackTypes];
 
    vector<short> idPosPrim[NTrackTypes];
    vector<short> idNegPrim[NTrackTypes];
 
    for (unsigned short iTr = 0; iTr < vRTracks.size(); iTr++) {
        CbmKFTrack& kfTrack = vRTracks[iTr];
        bool ok = 1;
        for (unsigned short iT = 0; iT < 5; iT++)
            ok = ok && finite(kfTrack.GetTrack()[iT]);
        for (unsigned short iC = 0; iC < 15; iC++)
            ok = ok && finite(kfTrack.GetCovMatrix()[iC]);
        ok = ok && (kfTrack.GetCovMatrix()[0] < 1. && kfTrack.GetCovMatrix()[0] > 0.)
             && (kfTrack.GetCovMatrix()[2] < 1. && kfTrack.GetCovMatrix()[2] > 0.)
             && (kfTrack.GetCovMatrix()[5] < 1. && kfTrack.GetCovMatrix()[5] > 0.)
             && (kfTrack.GetCovMatrix()[9] < 1. && kfTrack.GetCovMatrix()[9] > 0.)
             && (kfTrack.GetCovMatrix()[14] < 1. && kfTrack.GetCovMatrix()[14] > 0.);
        ok = ok && kfTrack.GetRefChi2() < 10 * kfTrack.GetRefNDF();
        if (!ok)
            continue;
 
        const int pdg = abs(vTrackPDG[iTr]);
 
        short pdgIndex = -1;
        switch (pdg) {
            case 11:
                pdgIndex = 0;
                break;
            case 13:
                pdgIndex = 1;
                break;
            case 211:
                pdgIndex = 2;
                break;
            case 321:
                pdgIndex = 3;
                break;
            case 2212:
                pdgIndex = 4;
                break;
        }
 
        short incr = 3;
        short pdgIndexMax = pdgIndex + incr;
 
        if (pdgIndex < 2) {
            incr = 1;
            pdgIndexMax = pdgIndex;
        }
 
        if (pdgIndex < 0) {
            pdgIndex = 5;
            pdgIndexMax = 7;
            incr = 1;
        }
 
        if (ChiToPrimVtx[iTr] < cuts[0][0]) {
            if (kfTrack.GetTrack()[4] >= 0.) {
                for (int ipdg = pdgIndex; ipdg <= pdgIndexMax; ipdg += incr)
                    idPosPrim[ipdg].push_back(iTr);
            }
            if (kfTrack.GetTrack()[4] < 0.)
                for (int ipdg = pdgIndex; ipdg <= pdgIndexMax; ipdg += incr)
                    idNegPrim[ipdg].push_back(iTr);
        } else {
            if (kfTrack.GetTrack()[4] >= 0.)
                for (int ipdg = pdgIndex; ipdg <= pdgIndexMax; ipdg += incr)
                    idPosSec[ipdg].push_back(iTr);
            if (kfTrack.GetTrack()[4] < 0.)
                for (int ipdg = pdgIndex; ipdg <= pdgIndexMax; ipdg += incr)
                    idNegSec[ipdg].push_back(iTr);
        }
    }
 
    const int nPart = idPosSec[5].size() * idNegSec[5].size() + idPosSec[5].size() * idNegSec[7].size()
                      + idPosSec[7].size() * idNegSec[5].size() + idPosPrim[2].size() * idNegPrim[3].size()
                      + idPosPrim[3].size() * idNegPrim[2].size() + idPosPrim[3].size() * idNegPrim[3].size()
                      + idPosPrim[4].size() * idNegPrim[3].size() + idPosPrim[3].size() * idNegPrim[4].size()
                      + idPosPrim[0].size() * idNegPrim[0].size() + idPosPrim[1].size() * idNegPrim[1].size();
 
    // std::cout << "NPart estim " << nPart << std::endl;
    Particles.reserve(vRTracks.size() + nPart);
 
    const float massLambdaPDG = 1.115683;
    const float massK0sPDG = 0.497614;
    const float massKsiPDG = 1.32171;
 
    for (unsigned short iTr = 0; iTr < vRTracks.size(); iTr++) {
        CbmKFTrack& kfTrack = vRTracks[iTr];
        kfTrack.SetId(iTr);
        CbmKFParticle tmp(&kfTrack);
        tmp.SetPDG(211);   // vTrackPDG[iTr]);
        tmp.SetId(Particles.size());
        Particles.push_back(tmp);
    }
 
    vector<float> vLambdaTopoChi2Ndf;
    vector<CbmKFParticle> vLambdaSec;
    vector<CbmKFParticle> vLambdaPrim;
 
    vector<float> vLambdaBarTopoChi2Ndf;
    vector<CbmKFParticle> vLambdaBarSec;
    vector<CbmKFParticle> vLambdaBarPrim;
 
    vector<float> vK0sTopoChi2Ndf;
    vector<CbmKFParticle> vK0sPrim;
 
    vector<CbmKFParticle> vXiPrim;
    vector<CbmKFParticle> vXiSec;
    vector<CbmKFParticle> vXiBarPrim;
 
    vector<CbmKFParticle> vXiStarPrim;
    vector<CbmKFParticle> vXiStarBarPrim;
 
    const float SecCuts[3] = {3.f, 5.f, 4.f};
    // K0s -> pi+ pi-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[5], pdgPos[5], 310, idNegSec[5], idPosSec[5], PrimVtx,
                       cuts[0], 0, &vK0sTopoChi2Ndf, SecCuts, massK0sPDG, 0.0022, &vK0sPrim, 0);
    // Lambda -> p pi-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[5], pdgPos[7], 3122, idNegSec[5], idPosSec[7], PrimVtx,
                       cuts[1], 0, &vLambdaTopoChi2Ndf, SecCuts, massLambdaPDG, 0.0012, &vLambdaPrim, &vLambdaSec);
    // Lambda_bar -> pi+ p-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgPos[5], pdgNeg[4], -3122, idPosSec[5], idNegSec[4], PrimVtx,
                       cuts[1], 0, &vLambdaBarTopoChi2Ndf, SecCuts, massLambdaPDG, 0.0012, &vLambdaBarPrim,
                       &vLambdaBarSec);
    // K*0 -> K+ pi-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgPos[3], pdgNeg[2], 313, idPosPrim[3], idNegPrim[2], PrimVtx,
                       cuts[1], 1);
    // K*0_bar -> pi+ K-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[3], pdgPos[2], -313, idNegPrim[3], idPosPrim[2], PrimVtx,
                       cuts[1], 1);
    // Lambda* -> p K-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[3], pdgPos[4], 3124, idNegPrim[3], idPosPrim[4], PrimVtx,
                       cuts[1], 1);
    // Lambda*_bar -> K+ p-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[4], pdgPos[3], -3124, idNegPrim[4], idPosPrim[3], PrimVtx,
                       cuts[1], 1);
    // phi -> K+ K-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[3], pdgPos[3], 333, idNegPrim[3], idPosPrim[3], PrimVtx,
                       cuts[1], 1);
    //   //rho -> pi+ pi-
    //   Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[2], pdgPos[2], 113,
    //                      idNegPrim[2], idPosPrim[2],
    //                      PrimVtx, cuts[1]);
    // gamma -> e+ e-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 22, idNegPrim[0], idPosPrim[0], PrimVtx,
                       cuts[1], 1);
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 22, idNegSec[0], idPosSec[0], PrimVtx,
                       cuts[1], 0);
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 22, idNegSec[0], idPosPrim[0], PrimVtx,
                       cuts[1], 0);
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 22, idNegPrim[0], idPosSec[0], PrimVtx,
                       cuts[1], 0);
    // JPsi-> e+ e-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 443, idNegPrim[0], idPosPrim[0], PrimVtx,
                       cuts[1], 1, 1.f);
    // JPsi-> mu+ mu-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[1], pdgPos[1], 100443, idNegPrim[1], idPosPrim[1], PrimVtx,
                       cuts[1], 1, 1.f);
    // rho, omega, phi -> e+ e-
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[0], pdgPos[0], 100113, idNegPrim[0], idPosPrim[0], PrimVtx,
                       cuts[1], 1, 0.2f);
    // rho, omega, phi -> mu+ mu-
    const float PCut = 1.f;
    Find2DaughterDecay(vRTracks, vField, Particles, pdgNeg[1], pdgPos[1], 200113, idNegPrim[1], idPosPrim[1], PrimVtx,
                       cuts[1], 1, 0.2f, -100, 0, &PCut);
 
    ExtrapolateToPV(vLambdaPrim, PrimVtx);
    ExtrapolateToPV(vLambdaBarPrim, PrimVtx);
    ExtrapolateToPV(vK0sPrim, PrimVtx);
 
    // Find Xi-
    float cutXi[3] = {3., 5., 6.};
    FindTrackV0Decay(3312, Particles, vLambdaSec, vRTracks, vField, pdgNeg[5], idNegSec[5], PrimVtx, cutXi, 0, 0,
                     &vXiPrim, massKsiPDG, 0.002);
 
    float cutLL[3] = {6., 10000000., 3.};
    float cutLL2[3] = {6., 3., 3.};
    vector<CbmKFParticle> vLL;
    FindTrackV0Decay(3002, vLL, vLambdaSec, vRTracks, vField, pdgNeg[5], idNegSec[5], PrimVtx, cutLL, 0, &ChiToPrimVtx);
    // Find H0->Lambda p pi-
    // Find Omega*-
    FindTrackV0Decay(3001, Particles, vLL, vRTracks, vField, pdgPos[4], idPosSec[4], PrimVtx, cutLL2, 0, &ChiToPrimVtx);
    // Find Xi+
    float cutXiPlus[3] = {3., 5., 6.};
    FindTrackV0Decay(-3312, Particles, vLambdaBarSec, vRTracks, vField, pdgPos[5], idPosSec[5], PrimVtx, cutXiPlus, 0,
                     0, &vXiBarPrim, massKsiPDG, 0.002);
    // Find Omega-
    float cutOmega[3] = {3., 3., 3.};
    FindTrackV0Decay(3334, Particles, vLambdaSec, vRTracks, vField, pdgNeg[6], idNegSec[6], PrimVtx, cutOmega, 0,
                     &ChiToPrimVtx);
    // Find Omega+
    float cutOmegaPlus[3] = {3., 3., 3.};
    FindTrackV0Decay(-3334, Particles, vLambdaBarSec, vRTracks, vField, pdgPos[6], idPosSec[6], PrimVtx, cutOmegaPlus,
                     0, &ChiToPrimVtx);
    // Find Xi*-
    float cutXiStarMinus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(1003314, Particles, vLambdaPrim, vRTracks, vField, pdgNeg[3], idNegPrim[3], PrimVtx,
                     cutXiStarMinus, 1);
    // Find Xi*+
    float cutXiStarPlus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(-1003314, Particles, vLambdaBarPrim, vRTracks, vField, pdgPos[3], idPosPrim[3], PrimVtx,
                     cutXiStarPlus, 1);
 
    ExtrapolateToPV(vXiPrim, PrimVtx);
    ExtrapolateToPV(vXiBarPrim, PrimVtx);
 
    // Find Xi*0
    float cutXiStar0[3] = {-100., 10000., 3.};
    FindTrackV0Decay(3324, Particles, vXiPrim, vRTracks, vField, pdgPos[5], idPosPrim[5], PrimVtx, cutXiStar0, 1, 0,
                     &vXiStarPrim);
    // Find Xi*0 bar
    float cutXiBarStar0[3] = {-100., 10000., 3.};
    FindTrackV0Decay(-3324, Particles, vXiBarPrim, vRTracks, vField, pdgNeg[5], idNegPrim[5], PrimVtx, cutXiBarStar0, 1,
                     0, &vXiStarBarPrim);
    // Find Omega*-
    const float cutOmegaStar[2] = {-100., 3.};
    for (unsigned int iPart = 0; iPart < vXiStarPrim.size(); iPart++)
        CombineTrackPart(vRTracks, vField, Particles, vXiStarPrim[iPart], pdgNeg[3], 1003334, idNegPrim[3],
                         cutOmegaStar, 0);
    // Find Omega*+
    for (unsigned int iPart = 0; iPart < vXiStarBarPrim.size(); iPart++)
        CombineTrackPart(vRTracks, vField, Particles, vXiStarBarPrim[iPart], pdgPos[3], -1003334, idPosPrim[3],
                         cutOmegaStar, 0);
    // Find K*+
    float cutKStarPlus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(323, Particles, vK0sPrim, vRTracks, vField, pdgPos[5], idPosPrim[5], PrimVtx, cutKStarPlus, 1, 0);
    // Find K*-
    float cutKStarMinus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(-323, Particles, vK0sPrim, vRTracks, vField, pdgNeg[5], idNegPrim[5], PrimVtx, cutKStarMinus, 1,
                     0);
    // Find Sigma*+
    float cutSigmaStarPlus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(3224, Particles, vLambdaPrim, vRTracks, vField, pdgPos[5], idPosPrim[5], PrimVtx, cutSigmaStarPlus,
                     1, 0);
    // Find Sigma*+ bar
    float cutSigmaStarPlusBar[3] = {-100., 10000., 3.};
    FindTrackV0Decay(-3114, Particles, vLambdaBarPrim, vRTracks, vField, pdgPos[5], idPosPrim[5], PrimVtx,
                     cutSigmaStarPlusBar, 1, 0);
    // Find Sigma*-
    float cutSigmaStarMinus[3] = {-100., 10000., 3.};
    FindTrackV0Decay(3114, Particles, vLambdaPrim, vRTracks, vField, pdgNeg[5], idNegPrim[5], PrimVtx,
                     cutSigmaStarMinus, 1, 0);
    // Find Sigma*- bar
    float cutSigmaStarMinusBar[3] = {-100., 10000., 3.};
    FindTrackV0Decay(-3224, Particles, vLambdaBarPrim, vRTracks, vField, pdgNeg[5], idNegPrim[5], PrimVtx,
                     cutSigmaStarMinusBar, 1, 0);
    // Find H-dibarion
    vector<CbmKFParticle> vHdibarion;
    float cutHdb[3] = {3., 3., 3.};
    for (unsigned short iL = 0; iL < vLambdaSec.size(); iL++) {
        CbmKFParticle_simd vDaughters[2] = {CbmKFParticle_simd(), CbmKFParticle_simd(vLambdaSec[iL])};
 
        vector<int> daughterIds;
        for (unsigned int iD = 0; iD < vLambdaSec[iL].DaughterIds().size(); iD++)
            daughterIds.push_back(vLambdaSec[iL].DaughterIds()[iD]);
        FindHyperons(3000, vDaughters, daughterIds, vLambdaSec, vHdibarion, PrimVtx, cutHdb, iL + 1);
    }
 
    for (unsigned int iH = 0; iH < vHdibarion.size(); iH++) {
        vHdibarion[iH].SetId(Particles.size());
        Particles.push_back(vHdibarion[iH]);
    }
    //                          chi2_prim  chi2_geo    z     pt  chi2_topo
    const float cutsD[8][8] = {{6., 3., 0.04, 0.3, 3.},        // D0 -> pi+ K-
                               {6., 3., 0.04, 0.3, 3.},        // D+ -> K- pi+ pi+
                               {6., 3., 0.04, 0.3, 3.},        // D0 -> pi+ pi+ pi- K-
                               {6., 3., 0.04, 0.3, 3.},        // Ds+ -> K- K+ pi+
                               {6., 3., 0.04, 0.3, 3.},        // Lambdac -> pi+ K- p
                               {6., 3., -100., 0.3, -100.},    // D*0  -> D+ pi-
                               {6., 3., -100., 0.3, -100.},    // D*+  -> D0 pi+
                               {6., 3., -100., 0.3, -100.}};   // D*+4 -> D04 pi+
 
    const int DMesLambdcDaughterPDG[5] = {211, -321, -211, 321, 2212};
    const int DMesLambdcMotherPDG[8] = {421, 411, 100421, 431, 4122, 10421, 10411, 20411};
    vector<short>* DMesLambdcIdTrack[5] = {&idPosSec[5], &idNegSec[3], &idNegSec[5], &idPosSec[3], &idPosSec[4]};
    FindDMesLambdac(vRTracks, vField, Particles, DMesLambdcDaughterPDG, DMesLambdcMotherPDG, DMesLambdcIdTrack, PrimVtx,
                    cutsD, ChiToPrimVtx);
 
    const int DMesLambdcBarDaughterPDG[5] = {-211, 321, 211, -321, -2212};
    const int DMesLambdcBarMotherPDG[8] = {-421, -411, -100421, -431, -4122, -10421, -10411, -20411};
    vector<short>* DMesLambdcBarIdTrack[5] = {&idNegSec[5], &idPosSec[3], &idPosSec[5], &idNegSec[3], &idNegSec[4]};
    FindDMesLambdac(vRTracks, vField, Particles, DMesLambdcBarDaughterPDG, DMesLambdcBarMotherPDG, DMesLambdcBarIdTrack,
                    PrimVtx, cutsD, ChiToPrimVtx);
    //  std::cout << "NPart  " << Particles.size() << std::endl;
}
 
// void CbmKFParticleInterface::FindParticles(vector<CbmL1Track>& vRTracks, vector<CbmKFParticle>& Particles,
//                                            CbmKFVertex& PrimVtx, const vector<int>& vTrackPDG, const float
//                                            cuts[2][3])
// {
//   FindParticlesT(vRTracks, Particles, PrimVtx, vTrackPDG, cuts);
// }
//
// void CbmKFParticleInterface::FindParticles(vector<CbmStsTrack>& vRTracks, vector<CbmKFParticle>& Particles,
//                                            CbmKFVertex& PrimVtx, const vector<int>& vTrackPDG, const float
//                                            cuts[2][3])
// {
//   FindParticlesT(vRTracks, Particles, PrimVtx, vTrackPDG, cuts);
// }
 
void CbmKFParticleInterface::ExtrapolateToPV(vector<CbmKFParticle>& vParticles, CbmKFVertex& PrimVtx)
{
    fvec pVtx[3] = {PrimVtx.GetRefX(), PrimVtx.GetRefY(), PrimVtx.GetRefZ()};
    for (unsigned int iL = 0; iL < vParticles.size(); iL += fvecLen) {
        CbmKFParticle* parts[fvecLen];
        unsigned int nPart = vParticles.size();
 
        unsigned int nEntries = (iL + fvecLen < nPart) ? fvecLen : (nPart - iL);
 
        for (unsigned short iv = 0; iv < nEntries; iv++)
            parts[iv] = &vParticles[iL + iv];
 
        CbmKFParticleInterface tmp(parts, nEntries);
        tmp.Extrapolate(tmp.KFPart.r, tmp.GetDStoPoint(pVtx));
 
        for (unsigned int iv = 0; iv < nEntries; iv++) {
            tmp.GetKFParticle(vParticles[iL + iv], iv);
        }
    }
}
 
fvec CbmKFParticleInterface::GetChi2BetweenParticles(CbmKFParticle_simd& p1, CbmKFParticle_simd& p2)
{
    const fvec& x1 = p1.GetX();
    const fvec& y1 = p1.GetY();
    const fvec& z1 = p1.GetZ();
 
    const fvec& x2 = p2.GetX();
    const fvec& y2 = p2.GetY();
    const fvec& z2 = p2.GetZ();
 
    const fvec dx = x1 - x2;
    const fvec dy = y1 - y2;
    const fvec dz = z1 - z2;
 
    const fvec& c0 = p1.GetCovariance(0) + p2.GetCovariance(0);
    const fvec& c1 = p1.GetCovariance(1) + p2.GetCovariance(1);
    const fvec& c2 = p1.GetCovariance(2) + p2.GetCovariance(2);
    const fvec& c3 = p1.GetCovariance(3) + p2.GetCovariance(3);
    const fvec& c4 = p1.GetCovariance(4) + p2.GetCovariance(4);
    const fvec& c5 = p1.GetCovariance(5) + p2.GetCovariance(5);
 
    const fvec r2 = dx * dx + dy * dy + dz * dz;
    const fvec err2 = c0 * dx * dx + c2 * dy * dy + c5 * dz * dz + 2 * (c1 * dx * dy + c3 * dx * dz + c4 * dy * dz);
 
    return (r2 * r2 / err2);
}
 
// template<class T>
void CbmKFParticleInterface::Find2DaughterDecay(vector<CbmKFTrack>& vTracks,
                                                const vector<L1FieldRegion>& vField,
                                                vector<CbmKFParticle>& Particles,
                                                const int DaughterNegPDG,
                                                const int DaughterPosPDG,
                                                const int MotherPDG,
                                                vector<short>& idNeg,
                                                vector<short>& idPos,
                                                CbmKFVertex& PrimVtx,
                                                const float* cuts,
                                                bool isPrimary,
                                                vector<float>* vMotherTopoChi2Ndf,
                                                const float* secCuts,
                                                const float massMotherPDG,
                                                const float massMotherPDGSigma,
                                                vector<CbmKFParticle>* vMotherPrim,
                                                vector<CbmKFParticle>* vMotherSec)
{
    const int NPositive = idPos.size();
    CbmKFParticle mother_temp;
 
    CbmKFParticleInterface mother;
    mother.SetPDG(MotherPDG);
    CbmKFParticleInterface motherTopo;
 
    const short NPosVect = NPositive % fvecLen == 0 ? NPositive / fvecLen : NPositive / fvecLen + 1;
    vector<CbmKFParticle_simd> posPart(NPosVect);
 
    // create particles from tracks
    CbmKFTrackInterface* vvPos[fvecLen];
 
    int iPosVect = 0;
    for (unsigned short iTrP = 0; iTrP < NPositive; iTrP += fvecLen) {
        const unsigned short NTracks = (iTrP + fvecLen < NPositive) ? fvecLen : (NPositive - iTrP);
 
        L1FieldRegion posField;
        for (unsigned short iv = 0; iv < NTracks; iv++) {
            vvPos[iv] = &vTracks[idPos[iTrP + iv]];
 
            int entrSIMDPos = idPos[iTrP + iv] % fvecLen;
            int entrVecPos = idPos[iTrP + iv] / fvecLen;
            posField.SetOneEntry(iv, vField[entrVecPos], entrSIMDPos);
        }
 
        posPart[iPosVect].Create(vvPos, NTracks, 0, &DaughterPosPDG);
        posPart[iPosVect].SetField(posField);
        fvec posId;
        for (int iv = 0; iv < NTracks; iv++)
            posId[iv] = idPos[iTrP + iv];
        posPart[iPosVect].SetId(posId);
        iPosVect++;
    }
 
    for (unsigned short iTrN = 0; iTrN < idNeg.size(); iTrN++) {
        CbmKFTrack& kfTrackNeg = vTracks[idNeg[iTrN]];
        CbmKFParticle_simd vDaughters[2] = {CbmKFParticle_simd(kfTrackNeg, 0, &DaughterNegPDG), CbmKFParticle_simd()};
        int entrSIMD = idNeg[iTrN] % fvecLen;
        int entrVec = idNeg[iTrN] / fvecLen;
        vDaughters[0].SetField(vField[entrVec], 1, entrSIMD);
        vDaughters[0].SetId(idNeg[iTrN]);
 
        for (unsigned short iTrP = 0; iTrP < NPositive; iTrP += fvecLen) {
            const unsigned short NTracks = (iTrP + fvecLen < NPositive) ? fvecLen : (NPositive - iTrP);
 
            vDaughters[1] = posPart[iTrP / fvecLen];
 
            if (isPrimary) {
                fvec vtxGuess[3] = {PrimVtx.GetRefX(), PrimVtx.GetRefY(), PrimVtx.GetRefZ()};
                fvec errGuess[3] = {100 * sqrt(PrimVtx.GetCovMatrix()[0]), 100 * sqrt(PrimVtx.GetCovMatrix()[2]),
                                    100 * sqrt(PrimVtx.GetCovMatrix()[5])};
                mother.SetVtxGuess(vtxGuess[0], vtxGuess[1], vtxGuess[2]);
                mother.SetVtxErrGuess(errGuess[0], errGuess[1], errGuess[2]);
                mother.Construct(vDaughters, 2, 0, -1, -1, 1);
            } else
                mother.Construct(vDaughters, 2, 0);
 
            if (vMotherTopoChi2Ndf) {
                motherTopo = mother;
                motherTopo.MeasureProductionVertex(&motherTopo.KFPart, motherTopo.GetParameters(), &PrimVtx);
            }
 
            // if( (fabs(DaughterNegPDG) == 211) && (fabs(DaughterPosPDG) == 211) )
            // {
            // fvec pl0[3] = {vDaughters[0].GetPx(),vDaughters[0].GetPy(),vDaughters[0].GetPz()};
            // fvec pl1[3] = {vDaughters[1].GetPx(),vDaughters[1].GetPy(),vDaughters[1].GetPz()};
            //
            // fvec pcm[3] = {pl0[0]+pl1[0], pl0[1]+pl1[1], pl0[2]+pl1[2]};
            // fvec ecm = vDaughters[0].GetE()+vDaughters[1].GetE();
            //
            // fvec gamma = sqrt(fvec(1.) - (pcm[0]*pcm[0]+pcm[1]*pcm[1]+pcm[2]*pcm[2])/(ecm*ecm));
            //
            // fvec e0 = gamma*(vDaughters[0].GetE() - (pcm[0]*pl0[0]+pcm[1]*pl0[1]+pcm[2]*pl0[2])/ecm);
            // fvec e1 = gamma*(vDaughters[1].GetE() - (pcm[0]*pl1[0]+pcm[1]*pl1[1]+pcm[2]*pl1[2])/ecm);
            // if( fabs(e0[0]-e1[0]) > 1.e-7 )
            // std::cout << e0 << "  " << e1 << std::endl;
            // }
 
            // CbmKFParticle mother_temp[fvecLen];
            // mother.GetKFParticle(mother_temp, NTracks);
 
            for (int iv = 0; iv < NTracks; iv++) {
                if (!finite(mother.GetChi2()[iv]))
                    continue;
                if (!(mother.GetChi2()[iv] > 0.0f))
                    continue;
                if (!(mother.GetChi2()[iv] == mother.GetChi2()[iv]))
                    continue;
                if (mother.GetZ()[iv] < cuts[2])
                    continue;
 
                if (mother.GetChi2()[iv] / mother.GetNDF()[iv] < cuts[1]) {
                    mother.GetKFParticle(mother_temp, iv);
                    mother_temp.SetId(Particles.size());
                    Particles.push_back(mother_temp);
 
                    float motherTopoChi2Ndf(0);
                    if (vMotherTopoChi2Ndf)
                        motherTopoChi2Ndf = motherTopo.GetChi2()[iv] / motherTopo.GetNDF()[iv];
                    //            vMotherTopoChi2Ndf->push_back(motherTopo.GetChi2()[iv]/motherTopo.GetNDF()[iv]);
 
                    if (vMotherPrim) {
                        Double_t mass, errMass;
 
                        mother_temp.GetMass(mass, errMass);
                        if ((fabs(mass - massMotherPDG) / massMotherPDGSigma) > secCuts[0])
                            continue;
 
                        if (motherTopoChi2Ndf < secCuts[1]) {
                            vMotherPrim->push_back(mother_temp);
                        } else if (vMotherSec) {
                            if (mother_temp.GetZ() < secCuts[2])
                                continue;
                            vMotherSec->push_back(mother_temp);
                        }
                    }
                }
            }
        }
    }
}
 
// template<class T>
void CbmKFParticleInterface::Find2DaughterDecay(vector<CbmKFTrack>& vTracks,
                                                const vector<L1FieldRegion>& vField,
                                                vector<CbmKFParticle>& Particles,
                                                const int DaughterNegPDG,
                                                const int DaughterPosPDG,
                                                const int MotherPDG,
                                                vector<short>& idNeg,
                                                vector<short>& idPos,
                                                CbmKFVertex& PrimVtx,
                                                const float* cuts,
                                                bool isPrimary,
                                                const float PtCut,
                                                const float Chi2PrimCut,
                                                vector<float>* ChiToPrimVtx,
                                                const float* PCut)
{
    vector<short> idPosPt;
    vector<short> idNegPt;
 
    for (unsigned int iEl = 0; iEl < idPos.size(); iEl++) {
        CbmKFTrack& kfTrack = vTracks[idPos[iEl]];
        const float tx = kfTrack.GetTrack()[2];
        const float ty = kfTrack.GetTrack()[3];
        const float p = fabs(1 / kfTrack.GetTrack()[4]);
        const float t2 = tx * tx + ty * ty;
        float pt = p * (sqrt(t2 / (1 + t2)));
        if (PCut)
            if (p < *PCut)
                continue;
        if (pt < PtCut)
            continue;
        if (Chi2PrimCut > -1)
            if (ChiToPrimVtx->at(idPos[iEl]) < Chi2PrimCut)
                continue;
        idPosPt.push_back(idPos[iEl]);
    }
 
    for (unsigned int iEl = 0; iEl < idNeg.size(); iEl++) {
        CbmKFTrack& kfTrack = vTracks[idNeg[iEl]];
        const float tx = kfTrack.GetTrack()[2];
        const float ty = kfTrack.GetTrack()[3];
        const float p = fabs(1 / kfTrack.GetTrack()[4]);
        const float t2 = tx * tx + ty * ty;
        float pt = p * (sqrt(t2 / (1 + t2)));
        if (pt < PtCut)
            continue;
        if (Chi2PrimCut > -1)
            if (ChiToPrimVtx->at(idNeg[iEl]) < Chi2PrimCut)
                continue;
        idNegPt.push_back(idNeg[iEl]);
    }
 
    Find2DaughterDecay(vTracks, vField, Particles, DaughterNegPDG, DaughterPosPDG, MotherPDG, idNegPt, idPosPt, PrimVtx,
                       cuts, isPrimary);
}
 
// template<class T>
void CbmKFParticleInterface::FindTrackV0Decay(const int MotherPDG,
                                              vector<CbmKFParticle>& Particles,
                                              vector<CbmKFParticle>& vV0,
                                              vector<CbmKFTrack>& vTracks,
                                              const vector<L1FieldRegion>& vField,
                                              const int DaughterPDG,
                                              vector<short>& idTrack,
                                              CbmKFVertex& PrimVtx,
                                              const float* cuts,
                                              bool isPrimary,
                                              vector<float>* ChiToPrimVtx,
                                              vector<CbmKFParticle>* vHyperonPrim,
                                              float hyperonPrimMass,
                                              float hyperonPrimMassErr,
                                              vector<CbmKFParticle>* vHyperonSec)
{
    CbmKFParticle hyperon_temp;
    CbmKFParticleInterface hyperon;
    hyperon.SetPDG(MotherPDG);
 
    for (unsigned short iV0 = 0; iV0 < vV0.size(); iV0++) {
        unsigned short nElements = 0;
        CbmKFParticle_simd vDaughters[2] = {CbmKFParticle_simd(vV0[iV0]), CbmKFParticle_simd()};
 
        CbmKFTrackInterface* vvTr[fvecLen];
        L1FieldRegion field;
        fvec trId;
 
        for (unsigned short iTr = 0; iTr < idTrack.size(); iTr++) {
            bool ok = 1;
            if (ChiToPrimVtx)
                if ((ChiToPrimVtx->at(idTrack[iTr]) < 7))
                    ok = 0;   // TODO 7 for Omega
 
            if (ok) {
                trId[nElements] = idTrack[iTr];
                vvTr[nElements] = &vTracks[idTrack[iTr]];
 
                int entrSIMD = idTrack[iTr] % fvecLen;
                int entrVec = idTrack[iTr] / fvecLen;
                field.SetOneEntry(nElements, vField[entrVec], entrSIMD);
 
                nElements++;
            } else if ((iTr != idTrack.size() - 1))
                continue;
 
            if ((nElements == fvecLen) || ((iTr == idTrack.size() - 1) && (nElements > 0))) {
                vDaughters[1].Create(vvTr, nElements, 0, &DaughterPDG);
                vDaughters[1].SetField(field);
                vDaughters[1].SetId(trId);
 
                if (isPrimary) {
                    fvec vtxGuess[3] = {PrimVtx.GetRefX(), PrimVtx.GetRefY(), PrimVtx.GetRefZ()};
                    fvec errGuess[3] = {100 * sqrt(PrimVtx.GetCovMatrix()[0]), 100 * sqrt(PrimVtx.GetCovMatrix()[2]),
                                        100 * sqrt(PrimVtx.GetCovMatrix()[5])};
                    hyperon.SetVtxGuess(vtxGuess[0], vtxGuess[1], vtxGuess[2]);
                    hyperon.SetVtxErrGuess(errGuess[0], errGuess[1], errGuess[2]);
                    hyperon.Construct(vDaughters, 2, 0, -1, -1, 1);
                } else
                    hyperon.Construct(vDaughters, 2, 0);
 
                CbmKFParticleInterface hyperonTopo(hyperon);
                hyperonTopo.MeasureProductionVertex(&hyperonTopo.KFPart, hyperonTopo.GetParameters(), &PrimVtx);
 
                for (unsigned int iv = 0; iv < nElements; iv++) {
                    bool isSameTrack = 0;
                    for (unsigned short iD = 0; iD < vV0[iV0].DaughterIds().size(); iD++)
                        if (vV0[iV0].DaughterIds()[iD] == trId[iv])
                            isSameTrack = 1;
 
                    if (isSameTrack)
                        continue;
                    if (!finite(hyperon.GetChi2()[iv]))
                        continue;
                    if (!(hyperon.GetChi2()[iv] > 0.0f))
                        continue;
                    if (!(hyperon.GetChi2()[iv] == hyperon.GetChi2()[iv]))
                        continue;
                    if ((hyperon.GetZ()[iv] < cuts[0]) || ((vV0[iV0].GetZ() - hyperon.GetZ()[iv]) < 0))
                        continue;
 
                    if (hyperonTopo.GetChi2()[iv] / hyperonTopo.GetNDF()[iv] > cuts[1])
                        continue;
 
                    if (hyperon.GetChi2()[iv] / hyperon.GetNDF()[iv] > cuts[2])
                        continue;
                    hyperon.GetKFParticle(hyperon_temp, iv);
 
                    hyperon_temp.SetId(Particles.size());
                    Particles.push_back(hyperon_temp);
 
                    if (vHyperonPrim) {
                        Double_t mass, errMass;
 
                        hyperon_temp.GetMass(mass, errMass);
                        if ((fabs(mass - hyperonPrimMass) / hyperonPrimMassErr) <= 3)
                            vHyperonPrim->push_back(hyperon_temp);
                        else if (vHyperonSec)
                            vHyperonSec->push_back(hyperon_temp);
                    }
                }
                nElements = 0;
            }
        }
    }
}
 
void CbmKFParticleInterface::FindHyperons(int PDG,
                                          CbmKFParticle_simd vDaughters[2],
                                          vector<int>& daughterIds,
                                          vector<CbmKFParticle>& vLambdaSec,
                                          vector<CbmKFParticle>& vHyperon,
                                          CbmKFVertex& PrimVtx,
                                          const float* cuts,
                                          int startIndex)
{
    CbmKFParticle* lambdas[fvecLen];
    int nLambdasSec = vLambdaSec.size();
 
    for (unsigned short iL = startIndex; iL < vLambdaSec.size(); iL += fvecLen) {
        unsigned int nEntries = (iL + fvecLen < nLambdasSec) ? fvecLen : (nLambdasSec - iL);
 
        for (unsigned short iv = 0; iv < nEntries; iv++)
            lambdas[iv] = &vLambdaSec[iL + iv];
 
        vDaughters[0].Create(lambdas, nEntries);
 
        CbmKFParticleInterface hyperon;
 
        hyperon.SetPDG(PDG);
        hyperon.Construct(vDaughters, 2, 0);
 
        CbmKFParticleInterface hyperonTopo(hyperon);
        hyperonTopo.MeasureProductionVertex(&hyperonTopo.KFPart, hyperonTopo.GetParameters(), &PrimVtx);
 
        for (unsigned int iv = 0; iv < nEntries; iv++) {
            bool isSameTrack = 0;
            for (unsigned short iD = 0; iD < lambdas[iv]->DaughterIds().size(); iD++)
                for (unsigned short iD0 = 0; iD0 < daughterIds.size(); iD0++)
                    if (lambdas[iv]->DaughterIds()[iD] == daughterIds[iD0])
                        isSameTrack = 1;
 
            if (isSameTrack)
                continue;
            if (!finite(hyperon.GetChi2()[iv]))
                continue;
            if (!(hyperon.GetChi2()[iv] > 0.0f))
                continue;
            if (!(hyperon.GetChi2()[iv] == hyperon.GetChi2()[iv]))
                continue;
            if ((hyperon.GetZ()[iv] < cuts[0]) || ((lambdas[iv]->GetZ() - hyperon.GetZ()[iv]) < 0))
                continue;
 
            if (hyperonTopo.GetChi2()[iv] / hyperonTopo.GetNDF()[iv] > cuts[1])
                continue;
 
            if (hyperon.GetChi2()[iv] / hyperon.GetNDF()[iv] > cuts[2])
                continue;
            CbmKFParticle hyperon_temp;
            hyperon.GetKFParticle(hyperon_temp, iv);
            vHyperon.push_back(hyperon_temp);
        }
    }
}
 
// template<class T>
void CbmKFParticleInterface::FindDMesLambdac(vector<CbmKFTrack>& vTracks,
                                             const vector<L1FieldRegion>& vField,
                                             vector<CbmKFParticle>& Particles,
                                             const int DaughterPDG[5],    // pi, K_b, pi_b, K, p
                                             const int MotherPDG[8],      // D0, D+, D0_4, Ds+, Lc
                                             vector<short>* idTrack[5],   // pi, K_b, pi_b, K, p
                                             CbmKFVertex& PrimVtx,
                                             const float cuts[8][8],
                                             vector<float> ChiToPrimVtx)
{
    vector<short> id[5];   // pi, K_b, pi_b, K, p
    for (int iId = 0; iId < 5; iId++) {
        for (unsigned int iTr = 0; iTr < idTrack[iId]->size(); iTr++) {
            CbmKFTrack& kfTrack = vTracks[idTrack[iId]->at(iTr)];
            const float tx = kfTrack.GetTrack()[2];
            const float ty = kfTrack.GetTrack()[3];
            const float p = fabs(1 / kfTrack.GetTrack()[4]);
            const float t2 = tx * tx + ty * ty;
            float pt = p * (sqrt(t2 / (1 + t2)));
            if (pt < cuts[0][3])
                continue;
            if (ChiToPrimVtx[idTrack[iId]->at(iTr)] < cuts[0][0])
                continue;
            id[iId].push_back(idTrack[iId]->at(iTr));
        }
    }
 
    vector<CbmKFParticle> kpi;
    vector<CbmKFParticle> kpipi;
    vector<CbmKFParticle> kpipipi;
    vector<CbmKFParticle> kpik;
    vector<CbmKFParticle> kpip;
 
    const float cutskpi[3] = {3., 3., -100.};
    Find2DaughterDecay(vTracks, vField, kpi, DaughterPDG[0], DaughterPDG[1], MotherPDG[0], id[0], id[1], PrimVtx,
                       cutskpi, 0);
 
    for (unsigned int iKPi = 0; iKPi < kpi.size(); iKPi++) {
        unsigned short startPiIndex = kpi[iKPi].DaughterIds()[0] + 1;
 
        CombineTrackPart(vTracks, vField, kpipi, kpi[iKPi], DaughterPDG[0], MotherPDG[1], id[0], cuts[1], startPiIndex,
                         1);
        CombineTrackPart(vTracks, vField, kpik, kpi[iKPi], DaughterPDG[3], MotherPDG[3], id[3], cuts[3], 0, 1);
        CombineTrackPart(vTracks, vField, kpip, kpi[iKPi], DaughterPDG[4], MotherPDG[4], id[4], cuts[4], 0, 1);
    }
    for (unsigned int iKPiPi = 0; iKPiPi < kpipi.size(); iKPiPi++)
        CombineTrackPart(vTracks, vField, kpipipi, kpipi[iKPiPi], DaughterPDG[2], MotherPDG[2], id[2], cuts[2], 0, 1);
 
    SelectParticleCandidates(Particles, kpi, PrimVtx, cuts[0]);
    SelectParticleCandidates(Particles, kpipi, PrimVtx, cuts[1]);
    SelectParticleCandidates(Particles, kpipipi, PrimVtx, cuts[2]);
    SelectParticleCandidates(Particles, kpik, PrimVtx, cuts[3]);
    SelectParticleCandidates(Particles, kpip, PrimVtx, cuts[4]);
 
    vector<CbmKFParticle> d0pi;
    vector<CbmKFParticle> d2pi;
    vector<CbmKFParticle> d4pi;
 
    for (unsigned int iKPiPi = 0; iKPiPi < kpipi.size(); iKPiPi++)
        CombineTrackPart(vTracks, vField, d0pi, kpipi[iKPiPi], DaughterPDG[2], MotherPDG[5], id[2], cuts[5], 0, 0);
 
    for (unsigned int iKPi = 0; iKPi < kpi.size(); iKPi++)
        CombineTrackPart(vTracks, vField, d2pi, kpi[iKPi], DaughterPDG[0], MotherPDG[6], id[0], cuts[6], 0, 0);
 
    for (unsigned int iKPiPiPi = 0; iKPiPiPi < kpipipi.size(); iKPiPiPi++)
        CombineTrackPart(vTracks, vField, d4pi, kpipipi[iKPiPiPi], DaughterPDG[0], MotherPDG[7], id[0], cuts[7], 0, 0);
 
    SelectParticleCandidates(Particles, d0pi, PrimVtx, cuts[5]);
    SelectParticleCandidates(Particles, d2pi, PrimVtx, cuts[6]);
    SelectParticleCandidates(Particles, d4pi, PrimVtx, cuts[7]);
}
 
// template<class T>
void CbmKFParticleInterface::CombineTrackPart(vector<CbmKFTrack>& vTracks,
                                              const vector<L1FieldRegion>& vField,
                                              vector<CbmKFParticle>& Particles,
                                              CbmKFParticle& part,
                                              const int DaughterPDG,
                                              const int MotherPDG,
                                              vector<short>& id,
                                              const float* cuts,
                                              const unsigned short startIndex,
                                              const bool IsSamePart)
{
    CbmKFParticle_simd vDaughters[2] = {CbmKFParticle_simd(part), CbmKFParticle_simd()};
 
    CbmKFTrackInterface* vTr[fvecLen];
 
    fvec vtxGuess[3] = {part.GetX(), part.GetY(), part.GetZ()};
    fvec errGuess[3] = {fvec(10.f * sqrt(part.GetCovMatrix()[0])), fvec(10.f * sqrt(part.GetCovMatrix()[2])),
                        fvec(10.f * sqrt(part.GetCovMatrix()[5]))};
 
    const unsigned short NTr = id.size();
    for (unsigned short iTr = startIndex; iTr < NTr; iTr += fvecLen) {
        const unsigned short NTracks = (iTr + fvecLen < NTr) ? fvecLen : (NTr - iTr);
 
        L1FieldRegion trField;
        for (unsigned short iv = 0; iv < NTracks; iv++) {
            vTr[iv] = &vTracks[id[iTr + iv]];
 
            int entrSIMDPos = id[iTr + iv] % fvecLen;
            int entrVecPos = id[iTr + iv] / fvecLen;
            trField.SetOneEntry(iv, vField[entrVecPos], entrSIMDPos);
        }
 
        vDaughters[1].Create(vTr, NTracks, 0, &DaughterPDG);
        vDaughters[1].SetField(trField);
        fvec trId;
        for (int iv = 0; iv < NTracks; iv++)
            trId[iv] = id[iTr + iv];
        vDaughters[1].SetId(trId);
 
        CbmKFParticleInterface mother;
        mother.SetPDG(MotherPDG);
        if (IsSamePart) {
            mother.SetVtxGuess(vtxGuess[0], vtxGuess[1], vtxGuess[2]);
            mother.SetVtxErrGuess(errGuess[0], errGuess[1], errGuess[2]);
        }
        mother.Construct(vDaughters, 2, 0);
 
        for (int iv = 0; iv < NTracks; iv++) {
            if (!finite(mother.GetChi2()[iv]))
                continue;
            if (!(mother.GetChi2()[iv] > 0.0f))
                continue;
            if (!(mother.GetChi2()[iv] == mother.GetChi2()[iv]))
                continue;
 
            if (mother.GetChi2()[iv] / mother.GetNDF()[iv] > cuts[1])
                continue;
 
            bool isSameTrack = 0;
            for (unsigned short iD = 0; iD < part.DaughterIds().size(); iD++)
                if (part.DaughterIds()[iD] == id[iTr + iv])
                    isSameTrack = 1;
            if (isSameTrack)
                continue;
 
            CbmKFParticle mother_temp;
            mother.GetKFParticle(mother_temp, iv);
            mother_temp.CleanDaughtersId();
            mother_temp.AddDaughter(id[iTr + iv]);
            for (unsigned short iD = 0; iD < part.DaughterIds().size(); iD++)
                mother_temp.AddDaughter(part.DaughterIds()[iD]);
            Particles.push_back(mother_temp);
        }
    }
}
 
void CbmKFParticleInterface::SelectParticleCandidates(vector<CbmKFParticle>& Particles,
                                                      vector<CbmKFParticle>& vCandidates,
                                                      CbmKFVertex& PrimVtx,
                                                      const float cuts[5])
{
    CbmKFParticle* cand[fvecLen];
    int nCand = vCandidates.size();
 
    for (unsigned short iC = 0; iC < nCand; iC += fvecLen) {
        unsigned int nEntries = (iC + fvecLen < nCand) ? fvecLen : (nCand - iC);
 
        for (unsigned short iv = 0; iv < nEntries; iv++)
            cand[iv] = &vCandidates[iC + iv];
 
        CbmKFParticleInterface candTopo(cand, nEntries);
 
        candTopo.MeasureProductionVertex(&candTopo.KFPart, candTopo.GetParameters(), &PrimVtx);
 
        for (unsigned int iv = 0; iv < nEntries; iv++) {
            if (!finite(candTopo.GetChi2()[iv]))
                continue;
            if (!(candTopo.GetChi2()[iv] > 0.0f))
                continue;
            if (!(candTopo.GetChi2()[iv] == candTopo.GetChi2()[iv]))
                continue;
            if (candTopo.GetZ()[iv] < cuts[2])
                continue;
 
            if (candTopo.GetChi2()[iv] / candTopo.GetNDF()[iv] > cuts[4])
                continue;
 
            Particles.push_back(vCandidates[iC + iv]);
        }
    }
}
 
// template<class T>
void CbmKFParticleInterface::ConstructPVT(vector<CbmKFTrack>& vRTracks) {}
 
// void CbmKFParticleInterface::ConstructPV(vector<CbmL1Track>& vRTracks)
// {
//   ConstructPVT(vRTracks);
// }
//
// void CbmKFParticleInterface::ConstructPV(vector<CbmStsTrack>& vRTracks)
// {
//   ConstructPVT(vRTracks);
// }
 
#undef cnst