//*-- Author  : A.Zinchenko <alexander.zinchenko@jinr.ru>
//*-- Created : 2016
//*-- Modified: R. Lalik <Rafal.Lalik@ph.tum.de> 2016-2022

//_HADES_CLASS_DESCRIPTION
/////////////////////////////////////////////////////////////
//  HForwardCand
//
//  Reconstructed vector
//
/////////////////////////////////////////////////////////////

#include "hforwardcand.h"

#include "hforwardtools.h"
#include "hfrpchit.h"
#include "hmdcsizescells.h"

#include "TBuffer.h"

ClassImp(HForwardCand);

HForwardCand::HForwardCand() : fFlags(0), fNhits(0), fNDF(0), fTot(-1.0), fFRpcHitIdx(-1), fDistance(0.0), fTof(-1.0)
{
    for (Int_t i = 0; i < STS_MAX_VPLANES; ++i)
    {
        fStsHitIdx[i] = -1;
        fStsDriftRadius[i] = 2.5;
    }
    for (Int_t i = 0; i < 10; ++i)
        fCovMatrix[i] = 0;
}

void HForwardCand::setFlagBit(UInt_t bit)
{
    // set given bit in flag (0-32)
    if (bit < sizeof(fFlags) * 8)
        fFlags |= (0x01 << bit);
    else { Error("HForwardCand::setFlagBit(Int_t)", "Bit number out of range : %i!", bit); }
}

void HForwardCand::unsetFlagBit(UInt_t bit)
{
    // unset given bit in flag (0-32)
    if (bit < sizeof(fFlags) * 8)
        fFlags &= ~(0x01 << bit);
    else { Error("HForwardCand::unsetFlagBit(Int_t)", "Bit number out of range : %i!", bit); }
}

Bool_t HForwardCand::isFlagBit(UInt_t bit) const
{
    // check given bit in flag (0-32)
    // return kTRUE if bit is set
    if (bit < sizeof(fFlags) * 8)
        return (fFlags >> bit) & 0x01;
    else
    {
        Error("HForwardCand::isFlagBit(Int_t)", "Bit number out of range : %i!", bit);
        return kFALSE;
    }
}

Bool_t HForwardCand::isFlagOR(Int_t num, ...) const
{
    // check given number of bits in argument in flag (0-32)
    // return kTRUE if any bit is set
    va_list ap;
    va_start(ap, num);

    for (Int_t i = 0; i < num; i++)
    {
        Int_t bit = va_arg(ap, Int_t);
        if (bit < 0 || bit > 31)
        {
            Error("HForwardCand::isFlagOR()", "Bit number out of range : i=%i ,%i!", i, bit);
            va_end(ap);
            return kFALSE;
        }
        if (isFlagBit(bit))
        {
            va_end(ap);
            return kTRUE;
        }
    }
    va_end(ap);
    return kFALSE;
}

Bool_t HForwardCand::isFlagAND(Int_t num, ...) const
{
    // check given number of bits in argument in flag (0-32)
    // return kTRUE if all bits are set
    va_list ap;
    va_start(ap, num);

    for (Int_t i = 0; i < num; i++)
    {
        Int_t bit = va_arg(ap, Int_t);
        if (bit < 0 || bit > 31)
        {
            Error("HForwardCand::isFlagAND()", "Bit number out of range : i=%i ,%i!", i, bit);
            va_end(ap);
            return kFALSE;
        }
        if (!isFlagBit(bit))
        {
            va_end(ap);
            return kFALSE;
        }
    }
    va_end(ap);
    return kTRUE;
}

Int_t HForwardCand::addStsHit(Int_t idx)
{
    // Add hit with index indx to the track
    fStsHitIdx[(Int_t)fNhits] = idx;
    return ++fNhits;
}

TMatrixDSym HForwardCand::getCovMatrix() const
{
    // Return covariance matrix as TMatrixDSym
    TMatrixDSym cov(4);
    Int_t ipos = 0;
    for (Int_t i = 0; i < 4; ++i)
    {
        for (Int_t j = 0; j <= i; ++j)
        {
            cov(i, j) = fCovMatrix[ipos++];
            cov(j, i) = cov(i, j);
        }
    }
    return cov;
}

void HForwardCand::setCovMatrix(TMatrixDSym cov)
{
    // Set covariance matrix
    Int_t ipos = 0;
    for (Int_t i = 0; i < 4; ++i)
    {
        for (Int_t j = 0; j <= i; ++j)
        {
            fCovMatrix[ipos++] = cov(i, j);
        }
    }
}

void HForwardCand::print(UInt_t /*selection*/) const
{
    printf("----- FORWARD VECTOR -----\n");
    printf("   nhits=%2d   ndf=%2d   QA chi2=%7.2f   chi2/ndf=%7.3f   tofrec=%1d   frpcidx=%2d   tof=%6.2f   len=%6.2f "
           "  tot=%6.2f\n",
           fNhits, fNDF, fChi2, fChi2 / fNDF, getTofRec(), getFRpcHitIndex(), getTof(), getDistance(), getTot());
    printf("   sts id=");
    for (Int_t i = 0; i < fNhits; ++i)
        printf("%2d,", fStsHitIdx[i]);
    printf("%*c R=%7.2f    Z=%8.2f   Phi=%6.2f    Theta=%5.2f\n", (16 - fNhits) * 3 + 12, ' ', getR(), getZ(), getPhi(),
           getTheta());
    printf("   sts dr=");
    for (Int_t i = 0; i < fNhits; ++i)
        printf(" %.3f", fStsDriftRadius[i]);
    printf("\n");
    printf("   flags: kIsUsed     : %d    kErrorTofRec : %d    kErrorLength : %d    kClusterVertex : %d\n", isFlagBit(HForward::kIsUsed),
           isFlagBit(HForward::kErrorTofRec), isFlagBit(HForward::kErrorLength), isFlagBit(HForward::kVertexCluster));
    printf("          kIsRejected : %d    kErrorTof    : %d    kErrorGamma  : %d\n", isFlagBit(HForward::kIsRejected),
           isFlagBit(HForward::kErrorTof), isFlagBit(HForward::kErrorGamma));
    //     Double32_t fCovMatrix[10];    // covar. matrix
}

void HForwardCand::calc4vectorProperties(Double_t m)
{
    unsetFlagBit(HForward::kIsUsed);
    if (!isFlagBit(HForward::kIsRejected))
    {
        Float_t mom = HForwardTools::calcMomentum(getDistance(), getTof(), m);

        if (mom < 0) { setFlagBit(HForward::kErrorGamma); }

        setMomentum(mom);

        TVector3 v3;
        v3.SetMagThetaPhi(mom, getTheta() * TMath::DegToRad(), getPhi() * TMath::DegToRad());
        SetVectM(v3, m);

        if (!isFlagBit(HForward::kErrorTof) and !isFlagBit(HForward::kErrorLength) and
            !isFlagBit(HForward::kErrorGamma))
        {
            setFlagBit(HForward::kIsUsed);
        }
    }
}

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

    UInt_t R__s, R__c;
    if (R__b.IsReading())
    {
        Version_t R__v = R__b.ReadVersion(&R__s, &R__c);
        if (R__v) {}
        HVirtualCand::Streamer(R__b);
        R__b >> fFlags;
        R__b >> fNhits;
        R__b >> fNDF;
        R__b.ReadStaticArray((short*)fStsHitIdx);
        R__b.ReadStaticArrayFloat16(fStsDriftRadius);
        {
            float R_Dummy;
            R__b >> R_Dummy;
            fTot = Float16_t(R_Dummy);
        }
        R__b >> fFRpcHitIdx;
        {
            float R_Dummy;
            R__b >> R_Dummy;
            fDistance = Float16_t(R_Dummy);
        }
        {
            float R_Dummy;
            R__b >> R_Dummy;
            fTof = Float16_t(R_Dummy);
        }
        R__b.ReadStaticArrayFloat16(fCovMatrix);
        R__b.CheckByteCount(R__s, R__c, HForwardCand::IsA());
    }
    else
    {
        R__c = R__b.WriteVersion(HForwardCand::IsA(), kTRUE);
        HVirtualCand::Streamer(R__b);
        R__b << fFlags;
        R__b << fNhits;
        R__b << fNDF;
        R__b.WriteArray(fStsHitIdx, 16);
        R__b.WriteArrayFloat16(fStsDriftRadius, 16);
        R__b << float(fTot);
        R__b << fFRpcHitIdx;
        R__b << float(fDistance);
        R__b << float(fTof);
        R__b.WriteArrayFloat16(fCovMatrix, 10);
        R__b.SetByteCount(R__c, kTRUE);
    }
}