//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
// 
// Data container holding all output information from the Kalman filter
// for dst production. The lower left triangle of the symmetric Covariance Matrix
// and the track state is stored. The track state is size 6
// (x[mm],y[mm],z[mm],tanx,tany,qp[1/MeV/c], sec coordinate sys) not 5 as the one from the internal
// kalman calculations since the z value is stored in addition. The track state
// vector is defined on the innermost MDC plane. Track state and covariance matrix
// can be obtained in lab system and different formats compatible to HParticleCand
// (z[mm],r[mm],theta [deg],phi[deg , 0-360],qp[1/MeV/c])(Lab system) or
// KFParticle (x[cm],y[cm],z[cm],px[MeV/c],py[MeV/c],pz[MeVc])(Lab system)
//
///////////////////////////////////////////////////////////////////////////////

#include "hkaloutput.h"
#include "hphysicsconstants.h"

#include <iostream>
#include <iomanip>


#include "TMath.h"
#include "TBuffer.h"

using namespace std;

ClassImp(HKalOutput)

HKalOutput::HKalOutput() {

    reset();
}

void HKalOutput::reset()
{
    // reset all members to default values
    fx  = 0;
    fy  = 0;
    fz  = 0;
    ftx = 0;
    fty = 0;
    fqp = 0;

    for(Int_t i = 0 ; i< 15; i++) fcovariance[i] = 0;

}

void HKalOutput::getStateVec(TVectorD &sv)
{
    // fill the the state vector of size 6 (x,y,z,tanx,tany,qp)
    // sec coordinate system [mm, 1/MeV/c]

    if(sv.GetNoElements() != NDIMSTATE){
	sv.ResizeTo(NDIMSTATE);
        Error("getStateVec()","Wrong dimension of StateVector!");
    }
    sv[0] = fx ;
    sv[1] = fy ;
    sv[2] = fz ;
    sv[3] = ftx;
    sv[4] = fty;
    sv[5] = fqp;
}

void HKalOutput::getStateVecLab(TVectorD &sv,Int_t s,Bool_t convert)
{
    // fill the the state vector of size 6 (x,y,z,tanx,tany,qp)
    // in lab system making use of sector for the
    // rotation [mm, 1/MeV/c] . convert=kTRUE -> [cm, 1/GeV/c]

    if(sv.GetNoElements() != NDIMSTATE){
	sv.ResizeTo(NDIMSTATE);
        Error("getStateVec()","Wrong dimension of StateVector!");
    }

    // get the rotation angle to Lab from sector
    Double_t  ph = 0;
    if(s < 5)  ph =  60.0f * s;
    else       ph = -60.0f;
    ph*=TMath::DegToRad();

    TVector3 pos,dir;

    pos.SetXYZ(fx,fy,fz);
    dir.SetZ( 1./(TMath::Abs(fqp) * TMath::Sqrt(ftx*ftx + fty*fty + 1)) );
    dir.SetX(ftx * dir.Z());
    dir.SetY(fty * dir.Z());

    pos.RotateZ(ph);
    dir.RotateZ(ph);

    sv[0] = pos.X();
    sv[1] = pos.Y();
    sv[2] = pos.Z();
    sv[3] = dir.X()/dir.Z();
    sv[4] = dir.Y()/dir.Z();
    sv[5] = fqp;

    if(convert){  // cm,GeV
	convertStateLab(sv,0.1,0.001);
    }
}
void HKalOutput::convertStateLab(TVectorD& sv,Float_t scalePos,Float_t scaleP)
{
    // returns 5 param track state ala HADES
    // (tx,ty,z[cm],q/p[1/GeV/c] (Lab system)
    // converted to HADES units [mm][MeV/c] (default: scalePos=0.1,scaleP=0.001)

    if(sv.GetNoElements() != 6){
	::Error("getStateVec()","Wrong dimension of StateVector!");
        return;
    }

    sv[0] *= scalePos;  // x
    sv[1] *= scalePos;  // y
    sv[2] *= scalePos;  // z
    sv[5] *= 1./scaleP; // q/p
}

void HKalOutput::setStateVec(const TVectorD &sv)
{
    // set the the state vector of size 6 (x,y,z,tanx,tany,qp)
    // sec coordinate system [mm, 1/MeV/c]
    if(sv.GetNoElements() != NDIMSTATE) {
	Error("setStateVec()","Wrong dimension of StateVector!");
        return;
    }
    fx  = sv[0];
    fy  = sv[1];
    fz  = sv[2];
    ftx = sv[3];
    fty = sv[4];
    fqp = sv[5];

}

void HKalOutput::getPosAndDir(TVector3& pos,TVector3& dir,Bool_t unit)
{
    // return pos and dir (unit vec if unit==kTRUE(default)) in sec coordinate sys [mm]
    pos.SetXYZ(fx,fy,fz);
    dir.SetZ( 1./(TMath::Abs(fqp) * TMath::Sqrt(ftx*ftx + fty*fty + 1)) );
    dir.SetX(ftx * dir.Z());
    dir.SetY(fty * dir.Z());
    if(unit) dir = dir.Unit();
}

void HKalOutput::getPosAndDirLab(TVector3& pos,TVector3& dir,Int_t s,Bool_t unit)
{
    // return pos and dir (unit vec if unit==kTRUE(default)) in Lab coordinate sys
    // using the sector s for rotation [mm]
    TVectorD sv(6);
    getStateVecLab(sv,s);

    pos.SetXYZ(sv[0],sv[1],sv[2]);
    dir.SetZ( 1./(TMath::Abs(fqp) * TMath::Sqrt(sv[3]*sv[3] + sv[4]*sv[4] + 1)) );
    dir.SetX(sv[3] * dir.Z());
    dir.SetY(sv[4] * dir.Z());
    if(unit) dir = dir.Unit();
}

void  HKalOutput::copyFrom(HKalOutput& fP)
{
    // copy data from fP
    fP.getHalfCovMatrix(fcovariance);
    fx  = fP.getX();
    fy  = fP.getY();
    fz  = fP.getZ();
    ftx = fP.getTx();
    fty = fP.getTy();
    fqp = fP.getQp();
}

void  HKalOutput::getHalfCovMatrix(Double_t cov[])
{
    // copy just the half covariance Matrix as stored from HKalOutput
    // cov array has to be size 15! No range check!
    for(Int_t i = 0; i < 15; i ++) cov[i] = fcovariance[i];
}

void  HKalOutput::getHalfCovMatrixLab(Double_t cov[],Int_t s)
{
    // transforms the half matrix to Lab frame
    // cov array has to be size 15! No range check!
    TMatrixD m(5,5);
    getCovMatrixLab(m,s);

    Int_t ct = 0;
    for(Int_t row = 0; row < NDIM; row++){
	for(Int_t col = 0; col <= row; col++){
	    cov[ct] = m(row,col);
	    ct++;
	}
    }
}

void  HKalOutput::setHalfCovMatrix(Double_t cov[])
{
    // copy just the half covariance Matrix to HKalOutput
    // cov array has to be size 15! No range check!
    for(Int_t i = 0; i < 15; i ++) fcovariance[i] = cov[i];
}

void HKalOutput::getCovMatrix(TMatrixD& m)
{
    // fill 5x5 symmetric Matrix from the
    // stored half matrix
    if(m.GetNrows()!= NDIM || m.GetNcols() != NDIM) {
	m.ResizeTo(NDIM,NDIM);
        Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
    }
    Int_t ct = 0;
    for(Int_t row = 0; row < NDIM; row++){
	for(Int_t col = 0; col <= row; col++){
	    m(row,col) = fcovariance[ct];
	    m(col,row) = fcovariance[ct];
            ct++;
	}
    }
}

void HKalOutput::getCovMatrixLab(TMatrixD& m,Int_t s,Bool_t convert)
{
    // returns the covariance matrix in Lab system
    // the sector is used for definition of the rotation
    // between sector and Lab system [mm,1/MeV/c], convert=kTRUE ==> [cm,1/GeV/c]

    if(m.GetNrows()!= NDIM || m.GetNcols() != NDIM) {
	m.ResizeTo(NDIM,NDIM);
        Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
    }


    //-----------------------------------------------
    // get the rotation angle to Lab from sector
    Double_t  ph = 0;
    if(s < 5)  ph =  60.0f * s;
    else       ph = -60.0f;
    ph*=TMath::DegToRad();
    //-----------------------------------------------


    //-----------------------------------------------
    // construct axis and normal vector of sector plane
    // sector sys is rotated arround z axis from Lab system
    TVector3 u(1,0,0); // -> x
    TVector3 v(0,1,0); // -> y
    TVector3 w(0,0,1); // -> z

    u.RotateZ(ph);  // rotate according sector counter clock wise, z stays untouched
    v.RotateZ(ph);  // rotate according sector counter clock wise, z stays untouched
    //-----------------------------------------------

    //-----------------------------------------------
    //  Calc jacobian matrix  sec -> Lab
    //
    // State vector in lab coordinate system: (x,y,tx,ty,qp,z).
    // State vector in sec coordinate system: (u,v,tu,tv,qp).
    // Sector coordinate system is defined by two orthogonal
    // Axis U and V on the layer and a normal vector W.
    // All axis are given in lab coordinate system.

    // Transformation from sector to Lab:
    // Calculate position in Lab coordinates (x,y,z) from sector coordinates:
    // P = u*U + v*V
    // direction in Lab coordinates:
    // A = 1/norm(W + tu*U + tv*V) * (W + tu*U + tv*V)
    // tx = A.x() / A.z()
    // ty = A.y() / A.z()

    // jac = d(x,y,tx,ty,qp,z) / d(u,v,tu,tv,qp)
    // row index:    state vector parameter in Lab coordinates
    // column index: state vector parameter in sector coordinates

    Double_t tu      = ftx;
    Double_t tv      = fty;
    TVector3 dir     = tu*u + tv*v + w;      // non normalized direction from sector state vector.
    Double_t dirz2   = dir.Z() * dir.Z();

    TMatrixD jacSecLab(5,5);
    jacSecLab.Zero();

    jacSecLab(0,0) = u.X(); // dx/du
    jacSecLab(1,0) = u.Y(); // dy/du

    jacSecLab(0,1) = v.X(); // dx/dv
    jacSecLab(1,1) = v.Y(); // dy/dv

    jacSecLab(2,2) = (tv*u.X()*v.Z() + u.X()*w.Z() - tv*u.Z()*v.X() - u.Z()*w.X()) / dirz2; // dtx/dtu
    jacSecLab(2,3) = (tu*u.Z()*v.X() + v.X()*w.Z() - tu*u.X()*v.Z() - v.Z()*w.X()) / dirz2; // dtx/dtv
    jacSecLab(3,2) = (tv*u.y()*v.Z() + u.Y()*w.Z() - tv*u.Z()*v.Y() - u.Z()*w.Y()) / dirz2; // dty/dtu
    jacSecLab(3,3) = (tu*u.Z()*v.Y() + v.Y()*w.Z() - tu*u.Y()*v.Z() - v.Z()*w.Y()) / dirz2; // dty/dtv

    jacSecLab(4,4) = 1.;  // qp does not change
    //-----------------------------------------------

    TMatrixD covSec(5,5);
    getCovMatrix(covSec);

    TMatrixD jacSecLabTrans = TMatrixD(TMatrixD::kTransposed, jacSecLab);

    m = jacSecLab * covSec * jacSecLabTrans;

    if(convert){
        convertCovMatrixLab(m,0.1,0.001);  // [mm][MeV] -> [cm][GeV/c]
    }
}

void HKalOutput::convertCovMatrixLab(TMatrixD& m, Float_t scalePos,Float_t scaleP)
{
    // returns 5 param track state covariance ala HADES (x,y,tx,ty,q/p) (Lab system)
    // converting [mm][MeV/c] -> [cm][GeV/c] (default scalePos=0.1,scaleP=0.001

    if(m.GetNrows()!= 5 || m.GetNcols() != 5) {
	::Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
        return;
    }
    TMatrixD scale(5,5);
    scale.Zero();

    scale(0,0) = scalePos;    // x  cm -> mm
    scale(1,1) = scalePos;    // y  cm -> mm
    scale(2,2) = 1.;          // tx
    scale(3,3) = 1.;          // ty
    scale(4,4) = 1./scaleP;   // q/p MeV/c -> GeV/c


    TMatrixD scaleTrans = TMatrixD(TMatrixD::kTransposed, scale);
    m = scale * m * scaleTrans;
}

void HKalOutput::setCovMatrixFromTrack(const TMatrixD& covTrack,const TRotation* rot)
{
    // set the covariance matrix in sec system
    // using TRotation to rotate back from track coord.
    // (when Kalman::kVarRot is used)
    // to sector coord. system  [mm,1/MeV/c]

    if(covTrack.GetNrows()!= NDIM || covTrack.GetNcols() != NDIM) {
        Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
    }


    //-----------------------------------------------
    // construct axis and normal vector of track system
    //
    TVector3 u(1,0,0); // -> x  track system
    TVector3 v(0,1,0); // -> y
    TVector3 w(0,0,1); // -> z

    u.Transform(*rot);
    v.Transform(*rot);
    w.Transform(*rot);
    //-----------------------------------------------

    //-----------------------------------------------
    //  Calc jacobian matrix  track -> sector
    //
    // State vector in sec coordinate system: (x,y,tx,ty,qp,z).
    // State vector in track coordinate system: (u,v,tu,tv,qp).
    // Track coordinate system is defined by two orthogonal
    // Axis U and V on the layer and a normal vector W.
    // All axis are given in sector coordinate system.

    // Transformation from track to sector:
    // Calculate position in sec coordinates (x,y,z) from track coordinates:
    // P = u*U + v*V
    // direction in sec coordinates:
    // A = 1/norm(W + tu*U + tv*V) * (W + tu*U + tv*V)
    // tx = A.x() / A.z()
    // ty = A.y() / A.z()

    // jac = d(x,y,tx,ty,qp,z) / d(u,v,tu,tv,qp)
    // row index:    state vector parameter in sector coordinates
    // column index: state vector parameter in track coordinates

    Double_t tu      = ftx;
    Double_t tv      = fty;
    TVector3 dir     = tu*u + tv*v + w;      // non normalized direction from track state vector.
    Double_t dirz2   = dir.Z() * dir.Z();

    TMatrixD jacTrackSec(5,5);
    jacTrackSec.Zero();

    jacTrackSec(0,0) = u.X(); // dx/du
    jacTrackSec(1,0) = u.Y(); // dy/du

    jacTrackSec(0,1) = v.X(); // dx/dv
    jacTrackSec(1,1) = v.Y(); // dy/dv

    jacTrackSec(2,2) = (tv*u.X()*v.Z() + u.X()*w.Z() - tv*u.Z()*v.X() - u.Z()*w.X()) / dirz2; // dtx/dtu
    jacTrackSec(2,3) = (tu*u.Z()*v.X() + v.X()*w.Z() - tu*u.X()*v.Z() - v.Z()*w.X()) / dirz2; // dtx/dtv
    jacTrackSec(3,2) = (tv*u.y()*v.Z() + u.Y()*w.Z() - tv*u.Z()*v.Y() - u.Z()*w.Y()) / dirz2; // dty/dtu
    jacTrackSec(3,3) = (tu*u.Z()*v.Y() + v.Y()*w.Z() - tu*u.Y()*v.Z() - v.Z()*w.Y()) / dirz2; // dty/dtv

    jacTrackSec(4,4) = 1.;  // qp does not change
    //-----------------------------------------------

    TMatrixD jacTrackSecTrans = TMatrixD(TMatrixD::kTransposed, jacTrackSec);
    TMatrixD m(5,5);
    m = jacTrackSec * covTrack * jacTrackSecTrans;

    setCovMatrix(m);
}

void HKalOutput::getMdcSeg(Double_t &zm, Double_t &r0, Double_t &theta, Double_t &phi,Int_t s)
{
    // return  track state (x,y,tx,ty,qp) direction in r[mm],z[mm],theta[rad],phi[rad] (like MdcSeg)
    // Input and output are in sector coor.sys. if s == 0 (default) otherwise in
    // Lab system (phi 0 - 2pi).
    // theta=atan(TMath::Sqrt(dX*dX+dY*dY)/dZ);
    // phi=atan(dY/dX)
    // zm= z1 - cos(theta)/sin(theta) * (x1*cos(phi)+y1*sin(phi))
    // r0=y1*cos(phi)-x1*sin(phi)
    //
    // If (x1,y1,z1)=(x2,y2,z2) dZ will eq.1.!
    TVector3 p1,p2;
    getPosAndDir(p1,p2);

    Double_t dX    = p2.X();
    Double_t dY    = p2.Y();
    Double_t dZ    = p2.Z();

    p2 = p1 + p2;   // create second point (dir is delta of p2-p1)


    Double_t lenXY = TMath::Sqrt(dX*dX+dY*dY);

    if(lenXY<2.E-5) {
	dX    = p2.X() * 1.E-5/TMath::Sqrt(p2.X()*p2.X()+p2.Y()*p2.Y());
	dY    = p2.Y() * 1.E-5/TMath::Sqrt(p2.X()*p2.X()+p2.Y()*p2.Y());
	lenXY = 1.E-5;            //dX*dX+dY*dY;
	dZ    = 1.;
    }
    dX   /= lenXY;
    dY   /= lenXY;
    dZ   /= lenXY;

    phi   = TMath::ATan2(dY,dX);
    theta = TMath::ATan2(1.,dZ);
    zm    = p1.Z()-dZ*(p1.X()*dX+p1.Y()*dY);
    r0    = p1.Y()*dX-p1.X()*dY;

    if(s != 0)
    {
	//-----------------------------------------------
	// get the rotation angle to Lab from sector
	Double_t  ph = 0;
	if(s < 5)  ph =  60.0f * s;
	else       ph = -60.0f;
	ph *=TMath::DegToRad();
	phi+=ph;
	if(phi < 0) phi += TMath::TwoPi();   // range 0 2pi (0-360 deg)
	//-----------------------------------------------
    }
}

void HKalOutput::getCandidateState(Double_t &zm, Double_t &r0, Double_t &theta, Double_t &phi,Double_t& p,Int_t s,Int_t pid)
{
    // returns track state in HParticleCand format  z[mm],r[mm],theta [deg],phi[deg, 0-360],p [MeV/c]
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.

    getMdcSeg(zm,r0,theta,phi,s);
    Int_t q = HPhysicsConstants::charge(pid);

    theta*=TMath::RadToDeg();
    phi  *=TMath::RadToDeg();
    p = (1./TMath::Abs(fqp))*TMath::Abs(q) ;
}

void HKalOutput::getCovMatrixCand(TMatrixD& covLab, Int_t s,Int_t pid)
{
    // returns 4 param track state covariance ala HParticleCand
    // (z[mm],r[mm],theta [deg],phi[deg, 0-360],qp [MeV/c]) (Lab system)
    // by recalulation from 5 param state (x,y,tx,ty,qp) (sector system)
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.

    // check input dimensions
    if(covLab.GetNrows()!= NDIM || covLab.GetNcols() != NDIM)
    {
	covLab.ResizeTo(NDIM,NDIM);
	Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
    }

    // get state vector in lab coordinate system
    TVectorD sv(6);
    getStateVecLab(sv, s);
    Int_t q = HPhysicsConstants::charge(pid);

    Double_t x  = sv[0];
    Double_t y  = sv[1];
    Double_t tx = sv[3];
    Double_t ty = sv[4];
    Double_t qp = sv[5] * TMath::Abs(q);

    // get covariance matrix in sector coordinate system
    TMatrixD covSec(5,5);
    getCovMatrix(covSec);

    // compute Jacobian
    //The equations for this transformation
    // kalman track state -> HParticleCand:
    // Let dZ = 1
    // dX     = tx
    // dY     = ty
    // lenXY  = sqrt(tx^2+ty^2)
    // tx     = tx/sqrt(tx^2+ty^2)
    // ty     = ty/sqrt(tx^2+ty^2)

    // theta  = arctan(sqrt(tx^2+ty^2))
    // phi    = arctan^2(ty,tx)
    // z      = z-(x*tx+y*ty)
    // r      = y*tx-x*ty


    TMatrixD jacTrackLab(5,5);
    jacTrackLab.Zero();


    Double_t tx2ty2  = tx*tx+ty*ty;
    jacTrackLab(0,0) = -ty;                                  // dr/dx
    jacTrackLab(0,1) =  tx;                                  // dr/dy
    jacTrackLab(0,2) = -x;                                   // dr/dtx
    jacTrackLab(0,3) =  y;                                   // dr/dty
    jacTrackLab(0,4) =  0;                                   // dr/dqp

    jacTrackLab(1,0) = -tx;                                  // dz/dx
    jacTrackLab(1,1) = -ty;                                  // dz/dy
    jacTrackLab(1,2) = -x;                                   // dz/dtx
    jacTrackLab(1,3) = -y;                                   // dz/dty
    jacTrackLab(1,4) =  0;                                   // dz/dqp

    jacTrackLab(2,0) =  0;                                   // dtheta/dx
    jacTrackLab(2,1) =  0;                                   // dtheta/dy
    jacTrackLab(2,2) =  tx/(TMath::Sqrt(tx2ty2)*(tx2ty2+1)); // dtheta/dtx
    jacTrackLab(2,3) =  ty/(TMath::Sqrt(tx2ty2)*(tx2ty2+1)); // dtheta/dty
    jacTrackLab(2,4) =  0;                                   // dtheta/dqp

    jacTrackLab(3,0) =  0;                                   // dphi/dx
    jacTrackLab(3,1) =  0;                                   // dphi/dy
    jacTrackLab(3,2) =  ty/(tx2ty2);                         // dphi/dtx
    jacTrackLab(3,3) =  tx/(tx2ty2);                         // dphi/dty
    jacTrackLab(3,4) =  0;                                   // dphi/dqp

    jacTrackLab(4,0) =  0;                                   // dp/dx
    jacTrackLab(4,1) =  0;                                   // dp/dy
    jacTrackLab(4,2) =  0;                                   // dp/dtx
    jacTrackLab(4,3) =  0;                                   // dp/dty
    jacTrackLab(4,4) =  1./(qp*qp);                          // dp/dqp

    // covariance transformation
    TMatrixD jacTrackLabTrans = TMatrixD(TMatrixD::kTransposed, jacTrackLab);
    covLab = jacTrackLab * covSec * jacTrackLabTrans;
}

void HKalOutput::setCovMatrix(const TMatrixD& m)
{
    // stores the lower left triangle of symmetric
    // 5x5 Matrix to the local half matrix

    if(m.GetNrows()!= NDIM || m.GetNcols() != NDIM) {
	Error("getCovMatrix()","Dimension of matrix wrong (has to be 5x5)");
	return;
    }
    Int_t ct = 0;
    for(Int_t row = 0; row < NDIM; row++){
	for(Int_t col = 0; col <= row; col++){
	    fcovariance[ct] = m(row,col);
	    ct++;
	}
    }
}

void HKalOutput::get6DimStateLab(Double_t& x,Double_t& y,Double_t& z,
				 Double_t& px,Double_t& py,Double_t& pz,
				 Int_t s, Int_t pid,Bool_t convert)
{
    // returns 6 param track state ala KFParticle
    // (x[cm],y[cm],z[cm],px[GeV/c],py[GeV/c],pz[GeV/c]) (Lab system)
    // by recalulation from 5 param state (x,y,tx,ty,qp) (sector system)
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.

    TVectorD svLab(6);
    getStateVecLab(svLab,s,convert); // Lab system  -> [cm,1/GeV/c]

    Double_t fqpLoc = svLab[5];
    Double_t tx = svLab[3];
    Double_t ty = svLab[4];

    Int_t q = HPhysicsConstants::charge(pid);

    Double_t c2 = 1. / (1. + tx * tx + ty * ty);
    Double_t pq = (1. / fqpLoc) * TMath::Abs(q);
    Double_t p2 = pq * pq;
    pz = sqrt(p2 * c2);
    px = tx * pz;
    py = ty * pz;
    x = svLab[0];  // x
    y = svLab[1];  // y
    z = svLab[2];  // z
}

void HKalOutput::get6DimStateLab(TVectorD& sv,Int_t s, Int_t pid,Bool_t convert)
{
    // returns 6 param track state ala KFParticle
    // (x[cm],y[cm],z[cm],px[GeV/c],py[GeV/c],pz[GeV/c]) (Lab system)
    // by recalulation from 5 param state (x,y,tx,ty,qp) (sector system)
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.

    if(sv.GetNoElements() != 6){
	sv.ResizeTo(6);
        Error("getStateVec()","Wrong dimension of StateVector!");
    }

    Double_t x,y,z,px,py,pz;
    get6DimStateLab(x,y,z,px,py,pz,s,pid,convert);

    sv[0] = x;  // x
    sv[1] = y;  // y
    sv[2] = z;  // z
    sv[3] = px;
    sv[4] = py;
    sv[5] = pz;

}

void HKalOutput::convert6DimStateLab(TVectorD& sv, Float_t scalePos,Float_t scaleP)
{
    // returns 6 param track state ala KFParticle
    // (x[cm],y[cm],z[cm],px[GeV/c],py[GeV/c],pz[GeV/c]) (Lab system)
    // converted to HADES units [mm][MeV/c] (default: scalePos=10,scaleP=1000.)

    if(sv.GetNoElements() != 6){
	::Error("getStateVec()","Wrong dimension of StateVector!");
        return;
    }

    sv[0] *= scalePos;  // x
    sv[1] *= scalePos;  // y
    sv[2] *= scalePos;  // z
    sv[3] *= scaleP;    // px
    sv[4] *= scaleP;    // py
    sv[5] *= scaleP;    // pz

}

void HKalOutput::get6DimHalfCovMatrixLab(Double_t cov[],Int_t s,Int_t pid,Bool_t convert)
{
    // returns 6 param track state covariance ala KFParticle (x,y,z,px,py,pz) (Lab system)
    // by recalulation from 5 param state (x,y,tx,ty,qp) (sector system)
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.
    // Size of array has to be 21! No range check!


    TVectorD svLab(6);        // x,y,z, tx,ty qp
    getStateVecLab (svLab,s,convert); // Lab system  [mm,1/MeV/c] => [cm,1/GeV/c]

    TMatrixD covLab(5,5);
    getCovMatrixLab(covLab,s,convert); // Lab system  -> [cm,1/GeV/c])


    Double_t tx = svLab[3];
    Double_t ty = svLab[4];
 
    Int_t q = HPhysicsConstants::charge(pid);

    Double_t fqpLoc = svLab[5];
    //https://git.cbm.gsi.de/computing/cbmroot/-/blob/master/reco/KF/CbmKFParticleFinder.cxx
    //CbmKFParticleFinder::FillKFPTrackVector

    //calculate covariance matrix
    /*
    Int_t q = HPhysicsConstants::charge(pid);
    Double_t c2 = 1. / (1. + tx * tx + ty * ty);
    Double_t pq = (1. / fqpLoc) * TMath::Abs(q);
    Double_t p2 = pq * pq;
    pz = sqrt(p2 * c2); ==>  sqrt(pq * pq  *  1. / (1. + tx * tx + ty * ty) )
    px = tx * pz;       ==>  tx *  sqrt(pq * pq  *  1. / (1. + tx * tx + ty * ty) )
    py = ty * pz;       ==>  ty *  sqrt(pq * pq  *  1. / (1. + tx * tx + ty * ty) )
    x = svLab[0];  // x
    y = svLab[1];  // y
    z = svLab[2];  // z
    */

    Double_t t      = sqrt(1.f + tx * tx + ty * ty);
    Double_t t3     = t * t * t;
    Double_t dpxdtx =  q / fqpLoc * (1.f + ty * ty) / t3;
    Double_t dpxdty = -q / fqpLoc * tx * ty / t3;
    Double_t dpxdqp = -q / (fqpLoc * fqpLoc) * tx / t;
    Double_t dpydtx = -q / fqpLoc * tx * ty / t3;
    Double_t dpydty =  q / fqpLoc * (1.f + tx * tx) / t3;
    Double_t dpydqp = -q / (fqpLoc * fqpLoc) * ty / t;
    Double_t dpzdtx = -q / fqpLoc * tx / t3;
    Double_t dpzdty = -q / fqpLoc * ty / t3;
    Double_t dpzdqp = -q / (fqpLoc * fqpLoc * t);

    Double_t F[6][5] = {
	{1.f, 0.f, 0.f, 0.f, 0.f},
	{0.f, 1.f, 0.f, 0.f, 0.f},
	{0.f, 0.f, 0.f, 0.f, 0.f},
	{0.f, 0.f, dpxdtx, dpxdty, dpxdqp},
	{0.f, 0.f, dpydtx, dpydty, dpydqp},
	{0.f, 0.f, dpzdtx, dpzdty, dpzdqp}
    };

    Double_t VFT[5][6];
    for (Int_t i = 0; i < 5; i++)  {
	for (Int_t j = 0; j < 6; j++) {
	    VFT[i][j] = 0;
	    for (Int_t k = 0; k < 5; k++) {
		VFT[i][j] += covLab(i, k) * F[j][k];
	    }
	}
    }

    for (Int_t i = 0, l = 0; i < 6; i++) {
	for (Int_t j = 0; j <= i; j++, l++) {
	    cov[l] = 0;
	    for (Int_t k = 0; k < 5; k++) {
		cov[l] += F[i][k] * VFT[k][j];
	    }
	}
    }
}

void HKalOutput::get6DimCovMatrixLab(TMatrixD& m,Int_t s,Int_t pid,Bool_t convert)
{
    // returns 6 param track state covariance ala KFParticle (x,y,z,px,py,pz) (Lab system)
    // by recalulation from 5 param state (x,y,tx,ty,qp) (sector system)
    // pid is Geant3 format. default pid is 14, if no pid is provided
    // momentum for charge other than +- 1 would be wrong.

    if(m.GetNrows()!= 6 || m.GetNcols() != 6) {
        Error("getCovMatrix()","Dimension of matrix wrong (has to be 6x6)");
        return;
    }

    Double_t cov[21];
    get6DimHalfCovMatrixLab(cov,s,pid,convert);

    Int_t ct = 0;
    for(Int_t row = 0; row < 6; row++){
	for(Int_t col = 0; col <= row; col++){
	    m(row,col) = cov[ct];
	    m(col,row) = cov[ct];
            ct++;
	}
    }
}

void HKalOutput::convert6DimCovMatrixLab(TMatrixD& m, Float_t scalePos,Float_t scaleP)
{
    // returns 6 param track state covariance ala KFParticle (x,y,z,px,py,pz) (Lab system)
    // converting [cm][GeV/c] -> [mm][MeV/c] (default scalePos=10,scaleP=1000

    if(m.GetNrows()!= 6 || m.GetNcols() != 6) {
	::Error("getCovMatrix()","Dimension of matrix wrong (has to be 6x6)");
        return;
    }
    TMatrixD scale(6,6);
    scale.Zero();

    scale(0,0) = scalePos;    // x  cm -> mm
    scale(1,1) = scalePos;    // y  cm -> mm
    scale(2,2) = scalePos;    // z  cm -> mm
    scale(3,3) = scaleP;      // px GeV/c -> MeV/c
    scale(4,4) = scaleP;      // py GeV/c -> MeV/c
    scale(5,5) = scaleP;      // pz GeV/c -> MeV/c


    TMatrixD scaleTrans = TMatrixD(TMatrixD::kTransposed, scale);
    m = scale * m * scaleTrans;

}

void HKalOutput::printStateVec()
{
    cout<<"trackstate : x "<<setw(10)<<fx
	<<" y " <<setw(15) <<fy
	<<" z " <<setw(15) <<fz
	<<" tx "<<setw(15) <<ftx
	<<" ty "<<setw(15) <<fty
	<<" qp "<<setw(15) <<fqp
	<<endl;
}

void HKalOutput::printStateVec(TVectorD& v)
{
    if(v.GetNoElements() != NDIMSTATE) {
	Error("setStateVec()","Wrong dimension of StateVector!");
        return;
    }

    cout<<"trackstate : x "<<setw(10)<<v[0]
	<<" y " <<setw(15) <<v[1]
	<<" z " <<setw(15) <<v[2]
	<<" tx "<<setw(15) <<v[3]
	<<" ty "<<setw(15) <<v[4]
	<<" qp "<<setw(15) <<v[5]
	<<endl;
}


void HKalOutput::printCovMatrix()
{
    // print symmetic 5x5 matrix from
    // the stored half matrix

    TMatrixD m(5,5);
    Int_t ct = 0;
    for(Int_t row = 0; row < NDIM; row++){
	for(Int_t col = 0; col <= row; col++){
	    m(row,col) = fcovariance[ct];
	    m(col,row) = fcovariance[ct];
	    ct++;
	}
    }
    m.Print();
}


void HKalOutput::Streamer(TBuffer &R__b)
{
   // Stream an object of class HKalOutput.

   UInt_t R__s, R__c;
   if (R__b.IsReading()) {
      Version_t R__v = R__b.ReadVersion(&R__s, &R__c); if (R__v) { }
      TObject::Streamer(R__b);
      if(R__v == 1 ) {  // Double32_t
	  {float R_Dummy; R__b >> R_Dummy; fx=Double32_t(R_Dummy);}
	  {float R_Dummy; R__b >> R_Dummy; fy=Double32_t(R_Dummy);}
	  {float R_Dummy; R__b >> R_Dummy; fz=Double32_t(R_Dummy);}
	  {float R_Dummy; R__b >> R_Dummy; ftx=Double32_t(R_Dummy);}
	  {float R_Dummy; R__b >> R_Dummy; fty=Double32_t(R_Dummy);}
	  {float R_Dummy; R__b >> R_Dummy; fqp=Double32_t(R_Dummy);}
	  R__b.ReadStaticArrayDouble32(fcovariance);
      } else {  // Double_t
	  R__b >> fx;
	  R__b >> fy;
	  R__b >> fz;
	  R__b >> ftx;
	  R__b >> fty;
	  R__b >> fqp;
	  R__b.ReadStaticArray((double*)fcovariance);
      }
      R__b.CheckByteCount(R__s, R__c, HKalOutput::IsA());
   } else {
      R__c = R__b.WriteVersion(HKalOutput::IsA(), kTRUE);
      TObject::Streamer(R__b);
      R__b << fx;
      R__b << fy;
      R__b << fz;
      R__b << ftx;
      R__b << fty;
      R__b << fqp;
      R__b.WriteArray(fcovariance, 15);
      R__b.SetByteCount(R__c, kTRUE);
   }
}