// from ROOT
#include "TMath.h"
#include "TRotation.h"
#include "TVector3.h"

// from hydra
#include "hphysicsconstants.h"
#include "hkalmdcmeaslayer.h"
#include "hkalrungekutta.h"
#include "hkaltrackstate.h"

#include <iostream>
#include <fstream>
using namespace std;

ClassImp(HKalRungeKutta)

//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
//
// Class: HKalRungeKutta
//
// This is the track propagator for the Kalman filter. Propagation is done
// with the Runge-Kutta method. Track state updates are done like in CBM, also see:
// S. Gorbunov and I. Kisel,
// An analytic formula for track extrapolation in an inhomogeneous magnetic field.
// CBM-SOFT-note-2005-001, 18 March 2005
//
// Before doing track propagation, a pointer to the magnetic field map must
// be set using the function
//  setFieldMap(HMdcTrackGField *fpField, Double_t fpol).
//
// The main function is propagateToPlane() that will propagate the track
// to a target plane and calculate the track state vector at that plane and
// the propagator matrix. It will return a boolean indicating whether
// problems were encountered during the Runge-Kutta stepping, like for example
// unrealistically hight track state parameters.
//
// All the points and B-field vectors may be stored in two arrays for
// debugging purposes or graphics output if that option has been enabled with
// setFillPointsArrays(kTRUE).
// The functions getPointsTrack() and getFieldTrack() will return these arrays.
// Each array element is a TVector3 object.
//
// The class also offers functions to calculate the material budget in each
// step (calcMaterialProperties()), the effects of multiple scattering
// (calcProcNoise()) and energy loss (calcEnergyLoss()).
//
//-----------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////


HKalRungeKutta::HKalRungeKutta() : TObject() {
    // Constructor that sets variables. It is still neccessary to set the field
    // map pointer with
    // setFieldMap(HMdcTrackGField *fpField, Double_t fpol)
    // before Runge-Kutta propation can be used.

    //? Eventually these parameters should be stored in a parameter container!
    initialStepSize = 50.;
    stepSizeDec     = 0.75;
    stepSizeInc     = 1.25;
    maxStepSize     = 200.;
    minStepSize     = 5.;
    minPrecision    = 2.e-04;
    maxPrecision    = 2.e-05;
    maxNumSteps     = 1000;
    maxDist         = 0.1;

    maxTan = 1.e4;
    maxPos = 5.e4;
    minMom = 20;

    bFillPointArrays = kFALSE;
    bRotBfield       = kFALSE;
    bPrintWarn       = kFALSE;
    tx2ty2Warn       = kFALSE;
}

HKalRungeKutta::~HKalRungeKutta() {
}

//  -----------------------------------
//  Implementation of public methods
//  -----------------------------------

Double_t HKalRungeKutta::calcEnergyLoss(TMatrixD &fProc, TVectorD &stateVec, Double_t beta, Double_t length, Double_t mass, Double_t qp,
        Double_t A, Double_t Z, Double_t density, Double_t radLength, Double_t exciteEner, Int_t pid) const {
    // Calculates energy loss due to bremsstrahlung and ionization.
    // Radiation loss is done for electrons/positrons only.
    // Only yields proper results if particle charge is +/- 1.
    // Return value is energy loss in MeV/particle charge.
    // fProc:      process noise matrix (will be modified)
    // stateVec:   track state vector before energy loss (q/p of state will be modified)
    // beta:       v/c of particle, must be < 1.0
    // length:     track length in mm
    // qp:         charge/momentum before energy loss in MeV/c
    // A:          mass number of passed material
    // Z:          atomic number of passed material
    // density:    density of passed material in g/mm^3
    // radLength:  radiation length of passed material in mm
    // exciteEner: mean excitation energy of passed material in MeV
    // isElectron: is the particle an electron or positron?

#ifdef kalDebug
    Int_t rows = fProc.GetNrows();
    Int_t cols = fProc.GetNcols();
    Int_t sdim = stateVec.GetNrows();
    if(rows < sdim || cols < sdim) {
        Error("calcEnergyLoss()", Form("Process noise matrix is %ix%i, but must be at least %ix%i matrix.", rows, cols, sdim, sdim));
        exit(1);
    }
    rows = stateVec.GetNrows();
    if(rows < sdim) {
        Error("calcEnergyLoss()", Form("State vector has dimension of %i, but must have at least %ix.", cols, sdim));
        exit(1);
    }
#endif

    // Energy loss due to radiation and ionization. Let
    // E' = particle energy including energy loss due to radiation and ionization.
    // E  = particle energy without energy loss.
    // delta(E) = energy loss
    //
    // E' = E - delta(E)
    //    = E * (1 - delta(E)/E)
    // E'/E = 1 - delta(E)/E

    Double_t ElossRad = 0.;
    Double_t ElossIon = 0.;

    if(pid == 2 || pid == 3) { // Particle is positron (2) or electron (3).
        // Radiation loss for electrons/positrons.
        ElossRad = calcRadLoss(fProc, length, mass, qp, radLength);
        if(direction == kIterBackward) {
            ElossRad *= -1.;
        }
        // Critical energy for gases:
        // E_c = 710 MeV / (Z + 0.92)
        // From "Passage of particles through matter", Particle Data Group, 2009
        Double_t eCritical = 710. / (Z + 0.92);
        if(1./qp < eCritical) {
            ElossIon = length * calcDEDXIonLepton(qp, A, Z, density, exciteEner, pid);
        }
    } else { // Energy loss for heavy particles.
        // Energy loss due to ionization.
        ElossIon = length * calcDEDXBetheBloch(beta, mass, A, Z, density, exciteEner);
        //Double_t ElossIonFac = 1. + ElossIon * TMath::Abs(qp);
    }
    if(direction == kIterBackward) {
        ElossIon *= -1.;
    }

    Double_t Eloss = ElossRad + ElossIon; // delta(E)

    // Correction of particle energy is:
    // E' = E + delta(E)
    //    = E * (1 + delta(E)/E)
    if(pid == 2 || pid == 3) {
        // For electrons: E ~ p
        // p' = p * (1 + delta(E)*p)
        // Track state parameter change is:
        // q/p' = q/p / (1 + delta(E)*qp)
        stateVec(kIdxQP) = qp / (1. +  Eloss*qp);
    } else {
        // E' = E + delta(E)
        // E'^2 = E^2 + 2*E*delta(E) + delta(E)^2
        // p'^2 + m^2 = p^2 + m^2 + 2*E*delta(E) + delta(E)^2
        // p'^2 = p^2 + 2*E*delta(E) + delta(E)^2
        Double_t p2 = 1./(qp*qp);
        Double_t E = TMath::Sqrt(p2 + mass*mass);
        stateVec(kIdxQP) = 1. / TMath::Sqrt(p2 + 2*E*Eloss + Eloss*Eloss);
    }

    return Eloss;
}


Double_t HKalRungeKutta::calcDEDXBetheBloch(Double_t beta, Double_t mass, Double_t A, Double_t Z, Double_t density, Double_t exciteEner, Double_t z) const {
    // Returns the ionization energy loss for thin materials with Bethe-Bloch formula.
    // - dE/dx = K/A * Z * z^2 / beta^2 * (0.5 * ln(2*me*beta^2*gamma^2*T_max/I^2) - beta^2)
    // T_max = (2*me*beta^2*gamma^2) / (1 + 2*gamma*me/mass + me^2/mass^2)
    //
    // beta:       v/c of particle
    // A:          mass number of passed material
    // Z:          atomic number of passed material
    // density:    density of passed material in g/mm^3
    // exciteEner: mean excitation energy of passed material in MeV
    // mass:       mass of the particle in MeV/c^2 traversing through the material
    // z:          atomic number of particle

    Double_t me = HPhysicsConstants::mass("e-"); // electron mass in MeV/c^2
    Double_t K = 0.307075 * 100.; // in MeV*mm^2/g for A = 1 g/mol

    Double_t KA = K / A * density;
    Double_t beta2 = beta*beta;
    Double_t gamma = 1./TMath::Sqrt(1 - beta2);
    Double_t betagamma2 = beta2 * gamma * gamma;
    // maximum kinetic energy that can be imparted on a free electron in a single collision
    Double_t tmax = (2. * me * betagamma2) / (1 + 2*gamma*me/mass + (me*me)/(mass*mass));

    return (-((KA * z*z * Z) / beta2) * (0.5 * TMath::Log((2. * me * betagamma2 * tmax)/(exciteEner*exciteEner)) - beta2));
}

Double_t HKalRungeKutta::calcDEDXIonLepton(Double_t qp, Double_t A, Double_t Z, Double_t density, Double_t exciteEner, Int_t pid) const {
    // Calculates energy loss dE/dx in MeV/mm due to ionization for relativistic electrons/positrons.
    // Sign will be negative if propagating in forward direction, i.e. particle looses energy.
    // For formula used, see:
    // Track fitting with energy loss
    // Stampfer, Regler and Fruehwirth
    // Computer Physics Communication 79 (1994), 157-164
    //
    // qp:         particle charge divided by momentum
    // A:          atomic mass
    // Z:          atomic number
    // density:    density of material in g/mm^3
    // exciteEner: mean excitation energy in MeV
    // pid:        Geant particle id (2 = positron, 3 = electron)

    if(pid != 2 && pid != 3) {
        Warning("calcDEDXIonLepton()", Form("Can only use this function for electrons or positrons. Pid is %i.", pid));
        return 0.;
    }

    Double_t de       = 5.0989 * 1.e-23;                 // in MeV * mm^2
    Double_t avogadro = 6.022  * 1.e23;                  // Avogadro constant in 1/mol
    Double_t eDensity = Z * avogadro * density / A;      // electron density
    Double_t me       = HPhysicsConstants::mass("e-");   // electron mass in MeV
    // gamma = E/m
    Double_t E        = 1. / qp;  // Energy for relativistic electrons.
    Double_t gamma    = E / me;

    // dE/dx = 1/2 * De * ne * (2 * ln(2*me*c^2/I) + 3 * ln(gamma) - 1.95) for electrons
    // dE/dx = 1/2 * De * ne * (2 * ln(2*me*c^2/I) + 4 * ln(gamma) - 2)    for positrons
    // with:

    // Formula is slightly different for electrons and positrons.
    // Factors for positrons:
    Double_t gammaFac = 4.;
    Double_t corr     = 2.;
    if(pid == 3) { // particle is electron
        gammaFac = 3.;
        corr     = 1.95;
    }

    Double_t dedx = 0.5 * de * eDensity * (2 * TMath::Log(2*me/exciteEner) + gammaFac * TMath::Log(gamma) - corr);
    return (- dedx);
}

Double_t HKalRungeKutta::calcRadLoss(TMatrixD &fProc, Double_t length, Double_t mass, Double_t qp, Double_t radLength) const {
    // Calculates energy loss due to bremsstrahlung for electrons with Bethe-Heitler formula.
    // Sign will be negative if propagating in forward direction, i.e. particle looses energy.
    // fProc:     process noise matrix (will be modified)
    // length:    track length in mm
    // radLength: radiation length of material in mm.
    // qp:        charge/momentum before energy loss in MeV/c

    Double_t t = length / radLength; // t := l/xr
    // Bethe and Heitler formula:
    // - dE/dx = E / xr
    // Integrating yields:
    // E' = E * exp(-t)
    // E'/E = exp(-t)
    // with t := l/xr
    // l = track length
    // xr = radiation length

    // delta(E) = E' - E
    //          = E * (E'/E - 1)
    //          = (E'/E - 1) / qp since E ~ p for electrons
    Double_t ElossRad = (TMath::Exp(-t) - 1.) / TMath::Abs(qp);

    // The variance of E'/E as done in:
    // D. Stampfer, M. Regler and R. Fruehwirth,
    // Track fitting with energy loss.
    // Comp. Phys. Commun. 79 (1994) 157-164
    // http://www-linux.gsi.de/~ikisel/reco/Methods/Fruehwirth-FittingWithEnergyLoss-CPC79-1994.pdf
    Double_t varElossRadFac = TMath::Exp(- t * TMath::Log(3.) / TMath::Log(2.)) - TMath::Exp(-2. * t);

    // Update process noise.
    fProc(kIdxQP, kIdxQP) += qp * qp * varElossRadFac;

    return ElossRad;
}


TVector3 HKalRungeKutta::calcFieldIntegral() const {
    // Calculates the field integral (Tm) by numerical integration
    // along the stepping points :  | sum over i  x(i+1)-x(i) x B(i) |
    // where x and B are position and field vectors.
    // setFillPointArrays(kTRUE) has to set before call propagate
    // to fill the point and field arrays which are needed for
    // the calculation.

    TVector3 bdl;
    if(!bFillPointArrays) {
        Warning("calcFieldIntegral()", "Stepping point arrays not filled. Cannot calculate field integral.");
        return bdl;
    }
    Int_t n = pointsTrack.GetEntries() - 1;
    for(Int_t i = 0; i < n ; i++){
        TVector3& x1 = (*(TVector3*)pointsTrack.At(i    ));
        TVector3& x2 = (*(TVector3*)pointsTrack.At(i + 1));
        TVector3& B  = (*(TVector3*)fieldTrack .At(i    ));
        TVector3 l   = (x2 - x1);
        bdl += l.Cross(B);
    }
    return bdl * 0.001; // mm -> m
}

void HKalRungeKutta::calcField_mm(const TVector3& xv, TVector3& btos, Double_t fpol) const {
    // Calculate B-field in x,y,z (mm).

    static Double_t abtos[3];
    static Double_t axv  [3];
    axv[0] = xv.X() * 0.1;  // convert to cm
    axv[1] = xv.Y() * 0.1;
    axv[2] = xv.Z() * 0.1;

#ifdef kalDebug
    if(!fieldMap) {
        Error("calcField_mm()", "Pointer to field map not set.");
        exit(1);
    }
#endif

    fieldMap->calcField(axv,abtos,fpol);

    btos.SetX(abtos[0]);
    btos.SetY(abtos[1]);
    btos.SetZ(abtos[2]);
}

void HKalRungeKutta::calcField_mm(Double_t* xv, Double_t* btos, Double_t fpol) const {
    // Calculate B-field in x,y,z (mm).

    static Double_t axv  [3];
    axv[0] = xv[0] * 0.1;  // convert to cm
    axv[1] = xv[1] * 0.1;
    axv[2] = xv[2] * 0.1;

#ifdef kalDebug
    if(!fieldMap) {
        Error("calcField_mm()", "Pointer to field map not set.");
        exit(1);
    }
#endif

    fieldMap->calcField(axv,btos,fpol);
}

void HKalRungeKutta::calcMaterialProperties(
        Double_t &A, Double_t &Z, Double_t &density, Double_t &radLength, Double_t &exciteEner,
        const TVector3 &posPreStep, const TVector3 &posPostStep,
        const HKalMdcMeasLayer &planeFrom, const HKalMdcMeasLayer &planeTo) const {
    // Calculates material budget for a Runge-Kutta step between two planes.
    // A:           mass number of mixture of passed material
    // Z:           atomic number of mixture of passed material
    // density:     density of mixture of passed material in g/mm^3
    // radLength:   radiation length of mixture of passed material in mm
    // exciteEner:  mean excitation energy of mixture of passed material in MeV
    // posPreStep:  position of track before the Runge-Kutta step
    // posPostStep: position of track after the Runge-Kutta step
    // planeFrom:   propagate track from this plane
    // planeTo:     propagate track to this plane

    // Length of straight line between position before and after stepping.
    Double_t length      = distance(posPostStep, posPreStep);

    // Direction of straight line to target plane.
    TVector3 dir(posPostStep - posPreStep);

    // Direction of straight line to source plane.
    TVector3 dirFrom(posPreStep - posPostStep);

    // Cosines of intersection angles of direction with the planes.
    //?
    Double_t angleFromPre = dir.Angle(planeFrom.getNormal());
    if(angleFromPre > TMath::Pi()/2.) {
        angleFromPre -= TMath::Pi()/2.;
    }
    angleFromPre = TMath::Cos(angleFromPre);

    Double_t angleToPost  = dir.Angle(planeTo.  getNormal());
    if(angleToPost > TMath::Pi()/2.) {
        angleToPost -= TMath::Pi()/2.;
    }
    angleToPost = TMath::Cos(angleToPost);

    angleFromPre = 1.;
    angleToPost  = 1.;

    // Distance of track position before the RK step to the source plane.
    Double_t distFromPre  = planeFrom.distanceToPlane(posPreStep)  / angleFromPre;
    // Distance of track position after the RK step to target plane.
    Double_t distToPost   = planeTo.  distanceToPlane(posPostStep) / angleToPost;

    // Maximum length the track can spend inside a layer.
    Double_t distFrom     = planeFrom.getThickness() / (2. * angleFromPre);
    Double_t distTo       = planeTo.  getThickness() / (2. * angleToPost);

    // Calculate fractions of the track where the particle has been inside
    // and outside the layers.
    Double_t fracInsideFrom = 0.;
    Double_t fracInsideTo   = 0.;
    Double_t fracOutside    = 1.;

    if(planeFrom.isInside(posPreStep)) {
        // Track stayed inside an mdc.
        if(planeFrom.isInside(posPostStep)) {
            fracInsideFrom = 1.;
        } else {
            // Track started inside an mdc and ended inside the next mdc.
            if(planeTo.isInside(posPostStep)) {
                fracInsideFrom = (distFrom - distFromPre) / length;
                fracInsideTo   = (distTo   - distToPost)  / length;
                // Track started inside of mdc and ended outside of mdc.
            } else {
                fracInsideFrom = (distFrom - distFromPre) / length;
            }
        }
    } else {
        if(planeTo.isInside(posPostStep)) {
            // Track stayed in mdc.
            if(planeTo.isInside(posPreStep)) {
                fracInsideTo = 1.;
                // Track started outside of mdc, but ended inside of mdc.
            } else {
                fracInsideTo = (distTo - distToPost) / length;
            }
        }
    }
    fracOutside = 1. - fracInsideFrom - fracInsideTo;

    if(TMath::Abs(fracInsideFrom + fracInsideTo + fracOutside - 1.F) > 1.e-5
            || (fracInsideFrom + fracInsideTo + fracOutside < 0.F)
            || fracInsideFrom > 1.F || fracInsideTo > 1.F || fracOutside > 1.F
            || fracInsideFrom < 0.F || fracInsideTo < 0.F || fracOutside < 0.F) {
        Warning("calcMaterialProperties()", "Track length fractions do not sum up to 1.");
        Warning("", Form("Fraction inside source plane: %f, inside target plane: %f, outside planes: %f", fracInsideFrom, fracInsideTo, fracOutside));
        fracOutside    = 1.;
        fracInsideFrom = 0.;
        fracInsideTo   = 0.;
    }

    /*
    Warning("", Form("Fraction inside source plane: %f, inside target plane: %f, outside planes: %f", fracInsideFrom, fracInsideTo, fracOutside));
    cout<<"length:          "<<length<<endl;
    cout<<"distFrom:        "<<distFrom<<endl;
    cout<<"distTo:          "<<distTo<<endl;
    cout<<"distFromPre:     "<<distFromPre<<endl;
    cout<<"distFromPost:    "<<planeFrom.distanceToPlane(posPostStep)/angleFromPre<<endl;
    cout<<"distToPost:      "<<distToPost<<endl;
    cout<<"distToPre:       "<<planeTo.distanceToPlane(posPreStep)/angleToPost<<endl;
    cout<<"pre inside from: "<<planeFrom.isInside(posPreStep)<<endl;
    cout<<"post inside from:"<<planeFrom.isInside(posPostStep)<<endl;
    cout<<"pre inside to:   "<<planeTo.  isInside(posPreStep)<<endl;
    cout<<"post inside to:  "<<planeTo.  isInside(posPostStep)<<endl;
    cout<<"distance between layers: "<<planeTo.distanceToPlane(planeFrom.getCenter())<<endl;

    cout<<"posPreStep: "<<endl;
    posPreStep.Print();
    cout<<"posPostStep:"<<endl;
    posPostStep.Print();
    cout<<"planeFrom:"<<endl;
    planeFrom.print("lay");
    cout<<"planeTo:"<<endl;
    planeTo.print("lay");
     */

    // Make a mixture of passed material.
    HKalMixture mat("Passed material for an RK step.", "rkstep", 3);
    Float_t w[3];    // mass fractions to be calculated
    Float_t vol[3]; // volume fractions
    vol[0] = fracInsideFrom;
    vol[1] = fracInsideTo;
    vol[2] = fracOutside;
    Float_t rho[3]; // densities of passed materials
    rho[0] = planeFrom.getDensity(1);
    rho[1] = planeTo.getDensity(1);
    rho[2] = planeTo.getDensity(0);
    mat.calcMassFracFromVolFrac(w, 3, rho, vol);

    // Calculate properties of material mixture.
    mat.defineElement(0, planeFrom.getA(1), planeFrom.getZ(1), planeFrom.getDensity(1), planeFrom.getExcitationEnergy(1), planeFrom.getRadLength(1), w[0]);
    mat.defineElement(1, planeTo.  getA(1), planeTo.  getZ(1), planeTo.  getDensity(1), planeTo.  getExcitationEnergy(1), planeTo.  getRadLength(1), w[1]);
    mat.defineElement(2, planeTo.  getA(0), planeTo.  getZ(0), planeTo.  getDensity(0), planeTo.  getExcitationEnergy(0), planeTo.  getRadLength(0), w[2]);
    mat.calcMixture();

    A          = mat.GetA();
    Z          = mat.GetZ();
    density    = mat.GetDensity();
    exciteEner = mat.GetExciteEner();
    radLength  = mat.GetRadLength();
}

void HKalRungeKutta::calcProcNoise(TMatrixD &fProc, const TVectorD &stateVec, Double_t length, Double_t radLength, Double_t beta) const {
    Double_t tx = stateVec(kIdxTanPhi);
    Double_t ty = stateVec(kIdxTanTheta);
    Double_t t  = 1. + tx*tx + ty*ty;

    // 1/beta^2
    Double_t beta2Inv = 1. / (beta*beta);

    // Squared scatter angle cms2.
    // cms = 13.6 MeV / (beta * c * p) * sqrt(l/X0) * (1 + 0.038 * ln(l/X0))
    // with l/X0 = length of particle track in units of radiation length.
    Double_t lx0 = length / radLength;
    Double_t cms2 = (13.6 * 13.6 * beta2Inv * stateVec(kIdxQP) * stateVec(kIdxQP) * lx0 * TMath::Power((1 + .038 * TMath::Log(lx0)),2));

    // Update process noise.
    fProc(kIdxTanPhi  , kIdxTanPhi)   += (1 + tx*tx) * t * cms2;
    fProc(kIdxTanTheta, kIdxTanTheta) += (1 + ty*ty) * t * cms2;
    fProc(kIdxTanPhi  , kIdxTanTheta) += tx * ty     * t * cms2;
    fProc(kIdxTanTheta, kIdxTanPhi)   += fProc(kIdxTanPhi  , kIdxTanTheta); // matrix is symmetric
}

void HKalRungeKutta::clear() {
    // Clears magnetic field definition and vertex position and direction.
    fieldMap      = 0;
    fieldScaleFact = 0.;
}

Bool_t HKalRungeKutta::checkTrack(TVectorD &statevec) const {
    // Reduces values in the state vector that have become too large to avoid
    // overflows or underflows and print a warning.
    // Returns whether track parameters have been reduced or not as a boolean.

    Bool_t noerr = kTRUE;

    if(statevec(kIdxTanPhi) > maxTan || TMath::IsNaN((statevec(kIdxTanPhi)))) {
        statevec(kIdxTanPhi) = maxTan;
        noerr = kFALSE;
    }
    if(statevec(kIdxTanTheta) > maxTan || TMath::IsNaN((statevec(kIdxTanTheta)))) {
        statevec(kIdxTanTheta) = maxTan;
        noerr = kFALSE;
    }
    if(statevec(kIdxTanPhi) < -maxTan) {
        statevec(kIdxTanPhi) = -maxTan;
        noerr = kFALSE;
    }
    if(statevec(kIdxTanTheta) < -maxTan) {
        statevec(kIdxTanTheta) = -maxTan;
        noerr = kFALSE;
    }
    if(statevec(kIdxX0) > maxPos || TMath::IsNaN((statevec(kIdxX0)))) {
        statevec(kIdxX0) = maxPos;
        noerr = kFALSE;
    }
    if(statevec(kIdxX0) < -maxPos) {
        statevec(kIdxX0) = -maxPos;
        noerr = kFALSE;
    }
    if(statevec(kIdxY0) > maxPos || TMath::IsNaN((statevec(kIdxY0)))) {
        statevec(kIdxY0) = maxPos;
        noerr = kFALSE;
    }
    if(statevec(kIdxY0) < -maxPos) {
        statevec(kIdxY0) = -maxPos;
        noerr = kFALSE;
    }
    if(!noerr) {
        if(bPrintWarn) {
            Warning("checkTrack()", "Track parameters too large/small.");
        }
    }
    if(TMath::Abs(1/statevec(kIdxQP)) < minMom) {
        if(statevec(kIdxQP) > 0.) {
            statevec(kIdxQP) =   (Double_t)(1./minMom);
        } else {
            statevec(kIdxQP) = - (Double_t)(1./minMom);
        }
        noerr = kFALSE;
        if(bPrintWarn) {
            Warning("checkTrack()", Form("Momentum smaller than %i MeV", minMom));
        }
    }
    return noerr;
}

Double_t HKalRungeKutta::distance(const TVector3 &vec1, const TVector3 &vec2) const {
    // Calculates the distance between two points.
    TVector3 distV = vec1 - vec2;
    return distV.Mag();
}

Bool_t HKalRungeKutta::propagateToPlane(TMatrixD &fProp, TMatrixD &fProc, TVectorD &stateVecTo, const TVectorD &stateVecFrom, const HKalMdcMeasLayer &planeFrom, const HKalMdcMeasLayer &planeTo, Int_t pid, Bool_t propDir) {
    // Propagate a particle trajectory to a plane.
    // fProp:        propagator matrix (return value)
    // fProc:        process noise matrix with contributions of multiple scattering and energy loss (return value)
    // stateVecTo:   track state vector (q/p,x,y,tx,ty) at next plane (return value)
    // stateVecFrom: track state vector (q/p,x,y,tx,ty) (will be propagated to next plane)
    // planeFrom:    plane to propagate from
    // planeTo:      plane to propagate to
    // propDir:      propagation direction kIterForward or kIterBackward
    // return true if no errors were encountered during track propagation.

    if(!fieldMap) {
        Error("propagateToPlane()", "Cannot propagate track without field map.");
        exit(1);
    }

    pointsTrack.Clear();
    fieldTrack.Clear();

    tx2ty2Warn     = kFALSE;
    Bool_t trackOk = kTRUE;
    direction      = propDir; // forward/backward
    trackLength    = 0.;
    stepLength     = 0.;
    energyLoss     = 0.;

    Double_t mass = HPhysicsConstants::mass(pid);

    stateVecTo     = stateVecFrom;

    fProp.UnitMatrix();
    fProc.Zero();

    //cosines(dir);     //? do some work on the direction

    TVectorD stateVecPreStep(stateVecTo.GetNrows());
    TVector3 posPreStep;
    TVector3 posAt;
    HKalTrackState::calcPosAtPlane(posAt, planeFrom, stateVecFrom);

    trackPosAtZ       = posAt.z();
    jstep             = 0;
    Double_t d        = 0.;
    Double_t step     = initialStepSize;
    Double_t nextStep = step;
    Double_t stepz;
    Double_t stepFac  = 1.;
    Bool_t doNextStep = kFALSE;

    TMatrixD DFstep(fProp.GetNrows(), fProp.GetNcols());

    //cout<<"***************** START RUNGE-KUTTA ***********************"<<endl;
    do {
        //cout<<"**** Do single step "<<jstep<<" ****"<<endl;

        // Store some properties before stepping.
        posPreStep.SetXYZ(stateVecTo(kIdxX0), stateVecTo(kIdxY0), trackPosAtZ);
        stateVecPreStep = stateVecTo;

        // calculate step size in z
        TVector3 dirAt;
        HKalTrackState::calcDir(dirAt, stateVecTo);
        stepz = step * dirAt.z();

        stepFac = rkSingleStep(stateVecTo, DFstep, stepz); // do one step
        fProp = DFstep * fProp; // update propagator

        // Decide if more steps must be done.
        TVector3 pointIntersect;
        posAt.SetXYZ(stateVecTo(kIdxX0), stateVecTo(kIdxY0), trackPosAtZ);

        //--------------------------------------------------------------------
        //  Process Noise and Energy Loss


        if(bDoMS || bDoDEDX) {
            // track inclination = sqrt(1 + tx^2 + ty^2)
            Double_t trackIncl = TMath::Sqrt(1. + stateVecPreStep(kIdxTanPhi)*stateVecPreStep(kIdxTanPhi) + stateVecPreStep(kIdxTanTheta)*stateVecPreStep(kIdxTanTheta));
            // Total step length = step length in z direction * track inclination.
            stepLength = stepz * trackIncl;
            Double_t beta = 1. / TMath::Sqrt(1. + mass*mass * stateVecPreStep(kIdxQP)*stateVecPreStep(kIdxQP));

            // Properties of passed material.
            Double_t A, Z, density, radLength, exciteEner;
            calcMaterialProperties(A, Z, density, radLength, exciteEner, posPreStep, posAt, planeFrom, planeTo);
            if(bDoMS) {
                calcProcNoise(fProc, stateVecPreStep, stepLength, radLength, beta);
            }

            if(bDoDEDX) {
                energyLoss += calcEnergyLoss(fProc, stateVecTo, beta, stepLength, mass, stateVecPreStep(kIdxQP), A, Z, density, radLength, exciteEner, pid);
            }
        }

        //  End Process Noise and Energy Loss
        //--------------------------------------------------------------------

        HKalTrackState::calcDir(dirAt, stateVecTo);
        trackOk &= planeTo.findIntersection(pointIntersect, posAt, dirAt);
        d = distance(posAt, pointIntersect);
        if (step < nextStep) {  // last step was rest step to chamber
            if (stepFac < 1.) nextStep = step * stepFac;
        } else {
            nextStep *= stepFac;
        }
        if (d > nextStep || d < maxDist) step = nextStep;
        else step = d;
        if(direction == kIterForward) {
            if(posAt.z() < pointIntersect.z()) { doNextStep = kTRUE; }
            else { doNextStep = kFALSE; }
        } else {
            if(posAt.z() > pointIntersect.z()) { doNextStep = kTRUE; }
            else { doNextStep = kFALSE; }
        }
        //cout<<"**** End single step. step: "<<jstep<<", z: "<<trackPosAtZ<<", nextStep: "<<nextStep<<" ****"<<endl;
    } while (d >= maxDist && doNextStep && jstep < maxNumSteps);

    trackOk &= checkTrack(stateVecTo); // avoid too large/too small values
    // Make sure the track position is on the target layer.
    stateVecPreStep = stateVecTo;
    propagateStraightLine(stateVecTo, DFstep, planeTo);
    fProp = DFstep * fProp;


    // Energy loss for straight line propagating.
    if(bDoDEDX) {
        posPreStep.SetXYZ(stateVecTo(kIdxX0), stateVecTo(kIdxY0), trackPosAtZ);

        Double_t A, Z, density, radLength, exciteEner;
        TVector3 posStart;
        HKalTrackState::calcPosAtPlane(posStart, planeFrom, stateVecFrom);
        TVector3 posAt;
        HKalTrackState::calcPosAtPlane(posAt, planeTo, stateVecTo);
        Double_t beta = 1. / TMath::Sqrt(1. + mass*mass * stateVecPreStep(kIdxQP)*stateVecPreStep(kIdxQP));

        calcMaterialProperties(A, Z, density, radLength, exciteEner, posPreStep, posAt, planeFrom, planeTo);
        energyLoss += calcEnergyLoss(fProc, stateVecTo, beta, stepLength, mass, stateVecTo(kIdxQP), A, Z, density, radLength, exciteEner, pid);
    }


    /*
    // Energy loss total track length.
    if(bDoDEDX) {
        Double_t A, Z, density, radLength, exciteEner;
        TVector3 posStart;
        HKalTrackState::calcPosAtPlane(posStart, planeFrom, stateVecFrom);
        TVector3 posAt;
        HKalTrackState::calcPosAtPlane(posAt, planeTo, stateVecTo);
		Double_t beta = 1. / TMath::Sqrt(1. + mass*mass * stateVecPreStep(kIdxQP)*stateVecPreStep(kIdxQP));

    	calcMaterialProperties(A, Z, density, radLength, exciteEner, posStart, posAt, planeFrom, planeTo);
    	energyLoss = calcEnergyLoss(fProc, stateVecTo, beta, getTrackLength(), mass, stateVecTo(kIdxQP), A, Z, density, radLength, exciteEner, pid);
    }
     */
    //cout<<"**** step: "<<jstep<<", z: "<<trackPosAtZ<<endl;
    //cout<<"***************** END RUNGE-KUTTA ***********************"<<endl;

    return (trackOk & !tx2ty2Warn);
}

void HKalRungeKutta::propagateStraightLine(TVectorD &stateVec, TMatrixD &fPropChange, const HKalPlane &planeTo) {
    // From the position and direction stored in the track state vector, propagate the track
    // to a target plane using a straight line. The track state and reference layer are updated
    // and the propagator matrix is calculated. The function returns the length of the straight line.
    // stateVec:    track state vector (will be modified)
    // fPropChange: change in propagator matrix (will be modified)
    // planeTo:     plane to move to


    // Calculate propagator matrix.
    TVector3 layToNorm   = planeTo.getNormal();
    Double_t nx  = layToNorm.X();
    Double_t ny  = layToNorm.Y();
    Double_t nz  = layToNorm.Z();
    Double_t tx  = stateVec(kIdxTanPhi);
    Double_t ty  = stateVec(kIdxTanTheta);

    Double_t s2    = nx*tx + ny*ty + nz;
    Double_t dsdx0 = -nx/s2;
    Double_t dsdy0 = -ny/s2;

    // For a straight line, only the position on the target plane will change.
    fPropChange.UnitMatrix();
    // dx0/da
    fPropChange(kIdxX0, kIdxX0)   = 1 + dsdx0*tx;
    fPropChange(kIdxX0, kIdxY0)   = dsdy0;

    // dy0/da
    fPropChange(kIdxY0, kIdxX0)   = dsdx0*ty;
    fPropChange(kIdxY0, kIdxY0)   = 1 + dsdy0*ty;

    // Calculate new track state vector.
    TVector3 posOld(stateVec(kIdxX0), stateVec(kIdxY0), trackPosAtZ);
    TVector3 dir;
    HKalTrackState::calcDir(dir, stateVec);
    // straight line to target plane
    TVector3 pointIntersect;
    planeTo.findIntersection(pointIntersect, posOld, dir);
    // update track state
    stateVec(kIdxX0) = pointIntersect.X();
    stateVec(kIdxY0) = pointIntersect.Y();

    stepLength = distance(posOld, pointIntersect);
    trackLength += stepLength;
}


Double_t HKalRungeKutta::rkSingleStep(TVectorD &stateVec, TMatrixD &fPropStep, Double_t stepSize) {
    // one step of track tracing from track state.
    // stateVec:  starting track parameters (will be changed)
    // fPropStep: change in propagator matrix for this step. (will be changed)
    // totStep:   step size


    //  Changes of state vector a by stepping in z direction:
    //  da/dz, a = (x, y, tx, ty, qp), z = stepping dir
    //  x'  = tx
    //  y'  = ty
    // tx'  = h * (tx * ty   * B[0] - (1 + tx2) * B[1] + ty * B[2])
    // ty'  = h * ((1 + ty2) * B[0] - txty      * B[1] - tx * B[2])
    //  h   = c * (q/p) * sqrt(1 + tx2 + ty2)

    // f    = f(z, a(z))
    // k1   = f(zn        , an)
    // k2   = f(zn + 1/2*h, an + 1/2*k1)
    // k3   = f(zn + 1/2*h, an + 1/2*k2)
    // k4   = f(zn + h    , an + k3)
    // fn+1 = fn + 1/6*k1 + 1/3*k2 + 1/3*k3 + 1/6*k4 + O(h^5)


    const Double_t c_light = 0.299792458;  // in MeV/(c * T * mm)

    static const Int_t numPars = 4;

    // Constants for RK stepping.
    static Double_t a[numPars] = { 0.0    , 0.5    , 0.5    , 1.0     };
    static Double_t b[numPars] = { 0.0    , 0.5    , 0.5    , 1.0     };
    static Double_t c[numPars] = { 1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0 };

    // for rk stepping
    //Int_t step4;
    Int_t rksteps = 4; // The 4 points used in Runge-Kutta stepping: start, 2 mid points and end
    Int_t rkStart = 0;
    Int_t rkMid1  = 1;
    Int_t rkMid2  = 2;
    Int_t rkEnd   = 3;

    Double_t k[rksteps][numPars]; // first index: step point (0 = start; 1,2 = mid, 3 = end), second index: state parameter
    //Double_t k[numPars*numPars];

    // for propagator
    Double_t F_tx[numPars], F_ty[numPars], F_tx_tx[numPars], F_ty_tx[numPars], F_tx_ty[numPars], F_ty_ty[numPars];

    //----------------------------------------------------------------
    Double_t est     = 0.; // error estimation
    Double_t stepFac = 1.;

    TVector3 B;                  // B-field
    Double_t h = stepSize;       // step size
    if(direction == kIterBackward) {
        h *= -1;                 // stepping in negative z-direction
    }
    Double_t half    = h * 0.5;  // half step interval for fourth order RK

    Double_t qp_in   = stateVec(kIdxQP);
    TVector3 posFrom = TVector3(stateVec(kIdxX0), stateVec(kIdxY0), trackPosAtZ);
    Double_t z_in    = posFrom.z();
    TVector3 posAt   = posFrom;

    Double_t hC      = h * c_light;

    // Input track state vector and state vector during stepping.
    Double_t sv_in[numPars], sv_step[numPars];
    sv_in[0] = stateVec(kIdxX0);
    sv_in[1] = stateVec(kIdxY0);
    sv_in[2] = stateVec(kIdxTanPhi);
    sv_in[3] = stateVec(kIdxTanTheta);

    fPropStep.UnitMatrix();

    //------------------------------------------------------------------------
    //   Runge-Kutta step
    //
    Int_t istep;
    Int_t ipar;

    do {
        half  = h * 0.5;
        for (istep = 0; istep < rksteps; ++istep) {// k1,k2,k3,k4  (k1=start, k2,k3 = half, k4=end of interval)
            for(ipar = 0; ipar < numPars; ++ipar) {         // 4 track parameters
                if(istep == 0) {
                    sv_step[ipar] = sv_in[ipar];            // in first step copy input track parameters (x,y,tx,ty)
                } else {
                    sv_step[ipar] = sv_in[ipar] + b[istep] * k[istep-1][ipar];     // do step
                    //sv_step[ipar] = sv_in[ipar] + b[istep] * k[istep*4-4+i];    // do step
                }
            }

            trackPosAtZ = z_in + a[istep] * h; // move z along with track
            posAt.SetXYZ(sv_step[kIdxX0], sv_step[kIdxY0], trackPosAtZ ); // update z value for current position

            // rotate B-field vector
            if(bRotBfield) {
                TVector3 posAtSector = posAt;
                posAtSector.Transform(pRotMat->Inverse());
                calcField_mm(posAtSector, B, fieldScaleFact);
                B.Transform(*pRotMat);
            } else {
                calcField_mm(posAt, B, fieldScaleFact);
            }

            Double_t tx        = sv_step[kIdxTanPhi];
            Double_t ty        = sv_step[kIdxTanTheta];
            Double_t tx2       = tx * tx;
            Double_t ty2       = ty * ty;
            Double_t txty      = tx * ty;
            Double_t tx2ty21   = 1.0 + tx2 + ty2;

            if(tx2ty21 > maxTan ) {
                if(bPrintWarn && !tx2ty2Warn) {
                    Warning("rkSingleStep()", "tx^2 + ty^2 too large. Track nearly perpendicular to beam. tx=%f, ty=%f.",
                            TMath::ATan(tx)*TMath::RadToDeg(),
                            TMath::ATan(ty)*TMath::RadToDeg()
                    );
                }
                tx2ty2Warn = kTRUE; // print a warning only once during propagation
                tx2ty21 = maxTan;   // reduce value to avoid overflows
            }

            Double_t I_tx2ty21 = 1.0 / tx2ty21 * qp_in;
            Double_t tx2ty2    = sqrt(tx2ty21);
            tx2ty2 *= hC;
            Double_t tx2ty2qp = tx2ty2 * qp_in;

            // for state propagation
            F_tx[istep] = ( txty        *B.x() - ( 1.0 + tx2 )*B.y() + ty*B.z()) * tx2ty2;     // h * tx' / qp
            F_ty[istep] = (( 1.0 + ty2 )*B.x() - txty         *B.y() - tx*B.z()) * tx2ty2;     // h * ty' / qp

            //------------------------------------------------------------------------
            // for transport matrix
            F_tx_tx[istep] = F_tx[istep]*tx*I_tx2ty21 + ( ty*B.x()-2.0*tx*B.y() ) * tx2ty2qp; // h * @tx'/@tx
            F_tx_ty[istep] = F_tx[istep]*ty*I_tx2ty21 + ( tx*B.x()+B.z()        ) * tx2ty2qp; // h * @tx'/@ty
            F_ty_tx[istep] = F_ty[istep]*tx*I_tx2ty21 + (-ty*B.y()-B.z()        ) * tx2ty2qp; // h * @ty'/@tx
            F_ty_ty[istep] = F_ty[istep]*ty*I_tx2ty21 + ( 2.0*ty*B.x()-tx*B.y() ) * tx2ty2qp; // h * @ty'/@ty
            //------------------------------------------------------------------------

            // for state propagation
            k[istep][kIdxX0]       = tx * h;
            k[istep][kIdxY0]       = ty * h;
            k[istep][kIdxTanPhi]   = F_tx[istep] * qp_in; // tx'
            k[istep][kIdxTanTheta] = F_ty[istep] * qp_in; // ty'

            //step4 = istep * 4;
            //k[step4  ] = tx * h;
            //k[step4+1] = ty * h;
            //k[step4+2] = F_tx[istep] * qp_in;
            //k[step4+3] = F_ty[istep] * qp_in;

        }  // end of Runge-Kutta steps
        //------------------------------------------------------------------------

        //------------------------------------------------------------------------
        // error estimation ala GEANT
        est = 0.;
        //est += fabs(k1 + k4 - k2 - k3) * half;  for all compoments

        //est += fabs(k[0] + k[12] - k[4] - k[ 8]) * half;
        //est += fabs(k[1] + k[13] - k[5] - k[ 9]) * half;
        //est += fabs(k[2] + k[14] - k[6] - k[10]) * half;
        //est += fabs(k[3] + k[15] - k[7] - k[11]) * half;

        est += fabs(k[rkStart][kIdxX0]       + k[rkEnd][kIdxX0]       - k[rkMid1][kIdxX0]       - k[rkMid2][kIdxX0])       * half;
        est += fabs(k[rkStart][kIdxY0]       + k[rkEnd][kIdxY0]       - k[rkMid1][kIdxY0]       - k[rkMid2][kIdxY0])       * half;
        est += fabs(k[rkStart][kIdxTanPhi]   + k[rkEnd][kIdxTanPhi]   - k[rkMid1][kIdxTanPhi]   - k[rkMid2][kIdxTanPhi])   * half;
        est += fabs(k[rkStart][kIdxTanTheta] + k[rkEnd][kIdxTanTheta] - k[rkMid1][kIdxTanTheta] - k[rkMid2][kIdxTanTheta]) * half;
        //------------------------------------------------------------------------

        if (est < minPrecision || h <= minStepSize || stepFac <= minStepSize) {
            // we found a step size with good precision
            jstep ++;
            break;
        } else {
            // precision not good enough. make smaller step
            stepFac *= stepSizeDec;
            h       *= stepSizeDec;
            hC       = h * c_light;
        }
    } while (jstep < maxNumSteps);

    //------------------------------------------------------------------------
    // set output track parameters
    for(ipar = 0; ipar < numPars; ++ ipar) {
        // yn+1        = yn         + 1/6*k1                    + 1/3*k2                  + 1/3*k3                  + 1/6*k4
        stateVec(ipar) = sv_in[ipar]+c[rkStart]*k[rkStart][ipar]+c[rkMid1]*k[rkMid1][ipar]+c[rkMid2]*k[rkMid2][ipar]+c[rkEnd]*k[rkEnd][ipar];
        //stateVec(ipar) = sv_in[ipar]+c[0]*k[ipar]+c[1]*k[4+ipar]+c[2]*k[8+ipar]+c[3]*k[12+ipar];
    }

    //------------------------------------------------------------------------
    //
    //     Derivatives

    //     x  y tx ty qp
    //  x  0  5 10 15 20    J[ 0 ...] = da/dx       = 0,   1 = 1
    //  y  1  6 11 16 21    J[ 5 ...] = da/dy       = 0,   6 = 1
    // tx  2  7 12 17 22    J[10 ...] = da/dtx            12 = 1, 14 = 0
    // ty  3  8 13 18 23    J[15 ...] = da/dty            18 = 1, 19 = 0
    // qp  4  9 14 19 24    J[20 ...] = da/dqp            24 = 1
    //


    //------------------------------------------------------------------------
    //
    //     Derivatives    dx/dqp
    //

    Double_t x0[numPars], x[numPars];
    Double_t k1[rksteps][numPars];     // Sum over a row of transport matrix from the previous step.
    //Double_t J[25];  // May be used later instead of a matrix to improve efficiency.
    //Double_t k1[16];

    x0[kIdxX0] = 0.0; x0[kIdxY0] = 0.0; x0[kIdxTanPhi] = 0.0; x0[kIdxTanTheta] = 0.0;

    //
    //   Runge-Kutta step for derivatives dx/dqp

    for (istep = 0; istep < rksteps; ++ istep) {
        for(ipar = 0; ipar < numPars; ++ ipar) {
            if(istep == 0) {
                x[ipar] = x0[ipar];
            } else {
                x[ipar] = x0[ipar] + b[istep] * k1[istep-1][ipar];
                //x[ipar] = x0[ipar] + b[istep] * k1[istep*4-4+ipar];
            }
        }

        k1[istep][kIdxX0]       = x[kIdxTanPhi]   * h;
        k1[istep][kIdxY0]       = x[kIdxTanTheta] * h;
        // sum (F_i-1(tx, *))   =(@tx'/@qp    + @tx'/@tx                       + @tx'/@ty) * z_i - z_0
        k1[istep][kIdxTanPhi]   = F_tx[istep] + F_tx_tx[istep] * x[kIdxTanPhi] + F_tx_ty[istep] * x[kIdxTanTheta];
        // sum (F_i-1(ty, *))   =(@ty'/@qp    + @ty'/@tx                       + @ty'/@ty) * z_i - z_0
        k1[istep][kIdxTanTheta] = F_ty[istep] + F_ty_tx[istep] * x[kIdxTanPhi] + F_ty_ty[istep] * x[kIdxTanTheta];


        //step4 = istep * 4;
        //k1[step4  ] = x[2] * h;
        //k1[step4+1] = x[3] * h;
        //k1[step4+2] = F_tx[istep] + F_tx_tx[istep] * x[2] + F_tx_ty[istep] * x[3];
        //k1[step4+3] = F_ty[istep] + F_ty_tx[istep] * x[2] + F_ty_ty[istep] * x[3];


    }  // end of Runge-Kutta steps for derivatives dx/dqp


    for (ipar = 0; ipar < numPars; ++ipar ) { // ? resort
        //J[20+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
        fPropStep(ipar, kIdxQP) = x0[ipar] + c[rkStart]*k1[rkStart][ipar] + c[rkMid1]*k1[rkMid1][ipar]
                                                                                                 + c[rkMid2] *k1[rkMid2][ipar]  + c[rkEnd] *k1[rkEnd][ipar];
        //fPropStep(i,kIdxQP) = x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
    }
    //J[24] = 1.;
    fPropStep(kIdxQP,kIdxQP) = 1;
    //------------------------------------------------------------------------



    //------------------------------------------------------------------------
    //     Derivatives    dx/tx
    //

    x0[kIdxX0] = 0.0; x0[kIdxY0] = 0.0; x0[kIdxTanPhi] = 1.0; x0[kIdxTanTheta] = 0.0;

    //
    //   Runge-Kutta step for derivatives dx/dtx
    //

    for (istep = 0; istep < 4; ++ istep) {
        for(ipar = 0; ipar < numPars; ++ipar) {
            if(istep == 0) {
                x[ipar] = x0[ipar];
            } else if ( ipar != 2 ){
                x[ipar] = x0[ipar] + b[istep] * k1[istep-1][ipar];
                //x[ipar] = x0[ipar] + b[istep] * k1[istep*4-4+ipar];
            }
        }

        k1[istep][kIdxX0]       = x[kIdxTanPhi]   * h;
        k1[istep][kIdxY0]       = x[kIdxTanTheta] * h;
        //k1[istep][kIdxTanPhi] = F_tx_tx[istep] * x[kIdxTanPhi] + F_tx_ty[istep] * x[kIdxTanTheta]; // not needed
        k1[istep][kIdxTanTheta] = F_ty_tx[istep] * x[kIdxTanPhi] + F_ty_ty[istep] * x[kIdxTanTheta];


        //step4 = istep * 4;
        //k1[step4  ] = x[2] * h;
        //k1[step4+1] = x[3] * h;
        //    k1[step4+2] = F_tx_tx[istep] * x[2] + F_tx_ty[istep] * x[3]; // not needed
        //k1[step4+3] = F_ty_tx[istep] * x[2] + F_ty_ty[istep] * x[3];


    }  // end of Runge-Kutta steps for derivatives dx/dtx

    for (ipar = 0; ipar < numPars; ++ipar ) { // ? resort
        if(ipar != kIdxTanPhi) {
            //J[10+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
            fPropStep(ipar, kIdxTanPhi) = x0[ipar] + c[rkStart]*k1[rkStart][ipar] + c[rkMid1]*k1[rkMid1][ipar]
                                                                                                         + c[rkMid2] *k1[rkMid2][ipar]  + c[rkEnd] *k1[rkEnd][ipar];
            //fPropStep(ipar, kIdxTanPhi) = x0[ipar]+c[0]*k1[ipar]+c[1]*k1[4+ipar]+c[2]*k1[8+ipar]+c[3]*k1[12+ipar];
        }
    }
    //      end of derivatives dx/dtx
    //J[12] = 1.0;
    //J[14] = 0.0;
    fPropStep(kIdxTanPhi, kIdxTanTheta) = 1.;
    fPropStep(kIdxQP,     kIdxTanPhi)   = 0.;
    //------------------------------------------------------------------------

    //------------------------------------------------------------------------
    //     Derivatives    dx/ty
    //

    x0[0] = 0.0; x0[1] = 0.0; x0[2] = 0.0; x0[3] = 1.0;

    //
    //   Runge-Kutta step for derivatives dx/dty
    //

    for (istep = 0; istep < 4; ++ istep) {
        for(ipar = 0; ipar < numPars; ++ipar) {
            if(istep == 0) {
                x[ipar] = x0[ipar];           // ty fixed
            } else if(ipar != 3) {
                x[ipar] = x0[ipar] + b[istep] * k1[istep-1][ipar];
                //x[ipar] = x0[ipar] + b[istep] * k1[istep*4-4+ipar];
            }
        }

        k1[istep][kIdxX0]         = x[kIdxTanPhi]   * h;
        k1[istep][kIdxY0]         = x[kIdxTanTheta] * h;
        k1[istep][kIdxTanPhi]     = F_tx_tx[istep] * x[kIdxTanPhi] + F_tx_ty[istep] * x[kIdxTanTheta];
        //k1[istep][kIdxTanTheta] = F_ty_tx[istep] * x[kIdxTanPhi] + F_ty_ty[istep] * x[kIdxTanTheta]; // not needed


        //step4 = istep * 4;
        //k1[step4  ] = x[2] * h;
        //k1[step4+1] = x[3] * h;
        //k1[step4+2] = F_tx_tx[istep] * x[2] + F_tx_ty[istep] * x[3];
        //    k1[step4+3] = F_ty_tx[istep] * x[2] + F_ty_ty[istep] * x[3]; // not needed

    }  // end of Runge-Kutta steps for derivatives dx/dty

    for (ipar = 0; ipar < 3; ++ipar ) { // ? resort
        //J[15+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i];
        fPropStep(ipar, kIdxTanTheta) = x0[ipar] + c[rkStart]*k1[rkStart][ipar] + c[rkMid1]*k1[rkMid1][ipar]
                                                                                                       + c[rkMid2] *k1[rkMid2][ipar]  + c[rkEnd] *k1[rkEnd][ipar];
        //fPropStep(ipar,kIdxTanTheta) = x0[ipar]+c[0]*k1[ipar]+c[1]*k1[4+ipar]+c[2]*k1[8+ipar]+c[3]*k1[12+ipar];
    }
    //      end of derivatives dx/dty
    //J[18] = 1.;
    //J[19] = 0.;
    fPropStep(kIdxTanTheta,kIdxTanTheta) = 1.;
    fPropStep(kIdxQP      ,kIdxTanTheta) = 0.;
    //------------------------------------------------------------------------

    //------------------------------------------------------------------------
    //
    //    derivatives dx/dx and dx/dy

    //for(i = 0; i < 10; ++i){
    //J[i] = 0.;
    //}
    for(ipar = 0; ipar < numPars + 1; ipar++) {
        fPropStep(ipar, kIdxX0) = 0.;
        fPropStep(ipar, kIdxY0) = 0.;
    }
    //J[0] = 1.; J[6] = 1.;
    fPropStep(kIdxX0, kIdxX0) = 1.;
    fPropStep(kIdxY0, kIdxY0) = 1.;



    //------------------------------------------------------------------------
    // for debugging + graphics purpose
    if(bFillPointArrays) {
        // Transform positions back to sector coordinates.
        if(bRotBfield) {
            posAt.Transform(pRotMat->Inverse());
            posFrom.Transform(pRotMat->Inverse());
        }

        if(pointsTrack.GetEntries() == 0) {
            // only for the first time add
            // start point
            pointsTrack.SetOwner();
            fieldTrack .SetOwner();

            calcField_mm(posFrom, B, fieldScaleFact);

            pointsTrack.Add(new TVector3(posFrom));
            fieldTrack .Add(new TVector3(B));
        }

        calcField_mm(posAt, B, fieldScaleFact);

        pointsTrack.Add(new TVector3(posAt));
        fieldTrack .Add(new TVector3(B));
    }
    //------------------------------------------------------------------------

    trackLength += fabs(distance(posFrom, posAt));  // calculate track length

    if (est < maxPrecision && h < maxStepSize) {
        stepFac *= stepSizeInc;
    }
    return stepFac;

}