BmnAligner.cxx

Go to the documentation of this file.

/*
    BM@N alignment routine
    BM@N experiment at NICA complex, JINR, 2025
 
    Department: Math & Soft Group of HEP lab
    Author:     Igor Polev, polev@jinr.ru
 
    BmnAligner class implementation
*/
 
#undef BMN_TEST_ALGORITHM   // special test mode
 
#include "BmnAligner.h"
 
#include "BmnAlignDefines.h"
#include "BmnDataIterator.h"
#include "Math/Functor.h"
#include "TCanvas.h"
#include "TDecompLU.h"
#include "TDecompSVD.h"
#include "TError.h"
#include "TPolyLine3D.h"
#include "TRandom.h"
#include "fairlogger/Logger.h"
 
#include <fstream>
#include <iomanip>
#include <iostream>
#include <nlohmann/json.hpp>
 
#define BMN_MSE_SLOW_THRESHOLD 2.0   // time threshold in seconds to show MSE calculation progress bar
 
template<typename HitType>
BmnAligner<HitType>::BmnAligner(const BmnMeasureModel* measureModel,
                                const BmnDetectorModel* modelDetector,
                                const char* filesListPath,
                                const char* constraintsPath,
                                Int_t nThreads)
    : BmnAligner(measureModel, modelDetector, constraintsPath, nThreads)
{
    TStopwatch timer;
    LOG(state) << " > BmnAligner : initialization started";
 
    // initialize ROOT file iterator
    LOG(info) << "    ROOT files list: " << filesListPath;
    fpRootIterator = new BmnRootIterator(filesListPath, *fpDetModel, &fProgressBar);
    if (!fpRootIterator->Initialized()) {
        LOG(error) << "<!> ROOT iterator initialization failed";
        return;
    }
    fpRootIterator->CountElements();
    fTotalTracks = fpRootIterator->GetTotalTracks();
    fTotalHits = fpRootIterator->GetTotalHits();
    fpIteratorMain = fpRootIterator;
 
    // all track are usable by default
    // TODO : replace std::vector with TBits
    fTracksUsableCnt = fTotalTracks;
    fpTrackUsable = new std::vector<Bool_t>(fTracksUsableCnt, kTRUE);
 
    // RAM mode:
    // If RAM requiered to store all hits is reasonable, we will read
    // all hits from files and itarate them in RAM during calculations.
    // In this case we need special iterator.
    auto ram_size = fTotalHits * sizeof(HitType);
    if (ram_size <= BMN_MAX_HIT_RAM_SIZE) {
        fpFirstHits = new Int_t[fTotalTracks + 1];   // one extra element for iterator convenience
        fpHitsData = new Byte_t[ram_size];
        fpHits = reinterpret_cast<HitType*>(fpHitsData);
        fpRamIterator = new BmnRamIterator(fpHits, fpFirstHits, fTotalTracks, &fProgressBar);
        fpIteratorMain = fpRamIterator;
    };
    LOG(info) << Form("    %.1f GB of RAM required", ram_size / 1073741824.0);
    LOG_IF(info, fpIteratorMain == fpRamIterator) << "    RAM storage selected";
    LOG_IF(info, fpIteratorMain == fpRootIterator) << "    file storage selected";
 
    // prepare set of iterators for parallelization by data
    InitIterators();
 
    fInitialized = kTRUE;
    DoneInTime(timer, "initialization");
}
 
template<typename HitType>
BmnAligner<HitType>::~BmnAligner()
{
    if (fpTrackUsable)
        delete fpTrackUsable;
    if (fpRootIterator)
        delete fpRootIterator;
    if (fpRamIterator)
        delete fpRamIterator;
    if (fpHitsData)
        delete[] fpHitsData;
    if (fpFirstHits)
        delete[] fpFirstHits;
    if (fThreadsCnt > 1)
        for (auto pItr : fpIterators)
            delete pItr;
}
 
// usage of option tracksPath is not implemented in this class
template<typename HitType>
Bool_t BmnAligner<HitType>::PrepareData(const char* tracksPath)
{
    TStopwatch timer;
    LOG(state) << " > BmnAligner : data preparation started";
    if (!fInitialized) {
        LOG(error) << "   <!> BmnAligner is not initialized";
        return kFALSE;
    }
    LOG_IF(info, fpIteratorMain == fpRamIterator) << "    loading data from ROOT files into RAM...";
    LOG_IF(info, fpIteratorMain == fpRootIterator) << "    parsing ROOT data files...";
    fProgressBar.Init();
 
    std::vector<SVectLC> arrAlpha(fTotalTracks);   // zero approximation of linear tracks
 
    Int_t detID, iTrack{0}, iHit{0};   // global hit index
    Double_t Dx{0.0}, Dy{0.0};
    fpRootIterator->ResetAll();
    while (!fpRootIterator->EndOfTracks()) {
        // in RAM mode we store index of first hits in track for memory optimization purposes
        if (fpIteratorMain == fpRamIterator)
            fpFirstHits[iTrack] = iHit;
        // init linear regression terms
        Double_t xSum{0.0}, ySum{0.0}, zSum{0.0}, z2Sum{0.0}, zxSum{0.0}, zySum{0.0};
 
        // iterate hits in track
        while (!fpRootIterator->EndOfHits()) {
            detID = fpRootIterator->HitDetectorID();
            // check if detector element encoding is valid
            if (fpDetModel->UnknownID(detID)) {
                const BmnRootIterator::Hit_t* hit{fpRootIterator->GetHit()};
                LOG(error) << "   <!> Unknown detector module encoding:"
                           << "\n       iterator::HitDetectorID() = " << detID
                           << "\n       FairHit::GetDetectorID()  = " << hit->GetDetectorID()
                           << "\n       CbmStsHit::GetSystemId()  = " << hit->GetSystemId()
                           << "\n       CbmStsHit::GetStationNr() = " << hit->GetStationNr()
                           << "\n       CbmStsHit::GetSectorNr()  = " << hit->GetSectorNr()
                           << "\n       CbmStsHit::GetSensorNr()  = " << hit->GetSensorNr();
                return kFALSE;
            }
            // count hits for each detector element
            fHitsPerDetector[fpDetModel->Idx(detID)]++;
 
            // inplace hit copy
            if (fpIteratorMain == fpRamIterator)
                new (&fpHits[iHit]) HitType(fpRootIterator->GetHit(), *fpDetModel);
 
            Double_t x{fpRootIterator->HitX()}, y{fpRootIterator->HitY()}, z{fpRootIterator->HitZ()};
            // calculate linear regression terms
            xSum += x;
            ySum += y;
            zSum += z;
            z2Sum += z * z;
            zxSum += z * x;
            zySum += z * y;
            // find max Z value (for visualization only)
            if (z > fMaxZvalue)
                fMaxZvalue = z;
            // calculate average hit position errors
            Dx += fpRootIterator->GetHit()->GetDx();
            Dy += fpRootIterator->GetHit()->GetDy();
 
            iHit++;
            fpRootIterator->NextHit();
        }   // loop by hits
 
        // calculate zero approximation of tracks as linear regression
        Double_t hitCnt{static_cast<Double_t>(fpRootIterator->HitsInTrack())},
            factor{1.0 / (hitCnt * z2Sum - zSum * zSum)};
        arrAlpha[iTrack] = SVectLC((xSum * z2Sum - zSum * zxSum) * factor, (hitCnt * zxSum - zSum * xSum) * factor,
                                   (ySum * z2Sum - zSum * zySum) * factor, (hitCnt * zySum - zSum * ySum) * factor);
 
        iTrack++;
        fpRootIterator->NextTrack();
    }   // loop by tracks
    if (fpIteratorMain == fpRamIterator)
        fpFirstHits[iTrack] = iHit;   // last syntetic element for iterator convenience
    StoreZeroSolution(arrAlpha);
 
    // MSE scale factors calculation
    Dx /= iHit;
    Dy /= iHit;
    fOmegaScaleFactor = (Dx + Dy) * 0.5;
    fOmegaScaleFactor *= fOmegaScaleFactor;
 
    // evaluate MSE calculation time
    TStopwatch timerMSE;
    timerMSE.Stop();
    timerMSE.Reset();
    SVectGLT point;
    for (Int_t i = 0; i < BMN_GLOBAL_PARAMS_PD; i++) {
        BMN_RND.RndmArray(BMN_GLPARAMS_TOTAL, point.Array());
        timerMSE.Start(kFALSE);
        CalculateMSE(point);
        timerMSE.Stop();
    }
    fMSEclcSlow = timerMSE.RealTime() / BMN_GLOBAL_PARAMS_PD > BMN_MSE_SLOW_THRESHOLD;
    if (!fMSEclcSlow)
        fMSEclcTick = static_cast<UInt_t>(BMN_MSE_SLOW_THRESHOLD / timerMSE.RealTime() * BMN_GLOBAL_PARAMS_PD);
 
    fProgressBar.Clear();
    if (fpIteratorMain == fpRamIterator) {
        LOG(info) << "    " << iTrack << " tracks loaded";
        LOG(info) << "    " << iHit << " hits loaded";
    }
    LOG(info) << "    average hit position errors:  Dx = " << Dx << ", Dy = " << Dy;
    DoneInTime(timer, "data preparation");
    return LoadConstraints();
}
 
template<typename HitType>
Bool_t BmnAligner<HitType>::LoadConstraints()
{
    TStopwatch timer;
    LOG(state) << " > BmnAligner : loading constraints...";
    if (!fpConstraintsPath) {
        LOG(warning) << "    constraints file in not provided, proceeding without constraints";
        return kTRUE;
    }
    std::ifstream ifs(fpConstraintsPath);
    if (!ifs.good()) {
        LOG(error) << " <!> LoadConstraints: failed to open JSON file " << *fpConstraintsPath;
        return kFALSE;
    }
    nlohmann::json jsonData;
    try {
        ifs >> jsonData;
    } catch (const std::exception& e) {
        LOG(error) << " <!> LoadConstraints: failed to parse JSON file " << *fpConstraintsPath;
        LOG(error) << e.what();
        return kFALSE;
    }
    if (!jsonData.is_array()) {
        LOG(error) << " <!> LoadConstraints: top-level JSON is not an array in " << *fpConstraintsPath;
        return kFALSE;
    }
    fConstraintsCnt = jsonData.size();
    if (fConstraintsCnt <= 0) {
        LOG(error) << " <!> LoadConstraints: empty constraints array in " << *fpConstraintsPath;
        return kFALSE;
    }
 
    fConstraints.resize(fConstraintsCnt);
    for (Int_t i = 0; i < fConstraintsCnt; i++) {
        const auto& jsonLine = jsonData[i];
        if (!jsonLine.is_array()) {
            LOG(error) << " <!> LoadConstraints: constraint " << i << " is not an array in " << fpConstraintsPath;
            return kFALSE;
        }
        Int_t lineSize = jsonLine.size();
        if (lineSize > BMN_GLPARAMS_TOTAL) {
            LOG(warning) << "    constraint " << i << " size exceeds global parameters count, truncating";
            lineSize = BMN_GLPARAMS_TOTAL;
        } else if (lineSize < BMN_GLPARAMS_TOTAL)
            LOG(warning) << "    constraint " << i << " size is less then global parameters count, padding with zeroes";
        fConstraints[i].ResizeTo(BMN_GLPARAMS_TOTAL);
        for (Int_t k = 0; k < lineSize; k++)
            fConstraints[i](k) = jsonLine[k].get<Double_t>();
    }
 
    LOG(info) << "    constraints loaded: " << fConstraintsCnt;
    DoneInTime(timer, "loading constraints");
    return kTRUE;
}
 
template<typename HitType>
Bool_t BmnAligner<HitType>::LoadSolution(const char* jsonPath, Bool_t withAlpha, Bool_t withMSE)
{
    if (!jsonPath)
        return kFALSE;
    TStopwatch timer;
    LOG(state) << " > BmnAligner : loading solution from file " << jsonPath;
 
    std::ifstream ifs(jsonPath);
    if (!ifs.good()) {
        LOG(error) << " <!> LoadSolution: failed to open JSON file";
        return kFALSE;
    }
    using namespace nlohmann;
    json js;
    Int_t elementsLoaded{0};
    try {
        ifs >> js;
 
        // check if solution is suitable
        json jsAlignmentType = BMN_CORRECTIONS;
        if (js.contains("CorrectionValues")) {
            if (js["CorrectionValues"] != jsAlignmentType) {
                LOG(error) << " <!> LoadSolution: wrong alignment type in JSON file\n"
                           << "     expected: " << jsAlignmentType << ", in file: " << js["CorrectionValues"];
                return kFALSE;
            }
        } else
            LOG(warning) << "    CorrectionValues key is missing: assuming alignment type is correct";
        if (js.contains("DetectorElements")) {
            if (static_cast<Int_t>(js["DetectorElements"]) != BMN_MODULE_COUNT) {
                LOG(error) << " <!> LoadSolution: wrong detector configuration in JSON file\n"
                           << "     modules expected: " << BMN_MODULE_COUNT << ", in file: " << js["DetectorElements"];
                return kFALSE;
            }
        } else
            LOG(warning) << "    DetectorElements key is missing: assuming detector configuration is correct";
        if (!js.contains("CorrectionsPerDetector") || !js["CorrectionsPerDetector"].is_array()) {
            LOG(error) << "   LoadSolution: CorrectionsPerDetector key is missing or not an array";
            return kFALSE;
        }
 
        // load solution for each stored detector element
        Int_t elementsCount{static_cast<Int_t>(js["CorrectionsPerDetector"].size())};
        for (Int_t i = 0; i < elementsCount; i++) {
            json& element = js["CorrectionsPerDetector"][i];
            if (!element.contains("detector_id")) {
                LOG(warning) << "   ATTENTION! DetectorID is missing for some element, skipping.";
                continue;
            }
            Int_t detID = element["detector_id"].get<Int_t>();
            if (fpDetModel->UnknownID(detID)) {
                LOG(warning) << "   ATTENTION! Unregistered detector element ID: " << detID << ", skipping.";
                continue;
            }
            if (!element.contains("values") || !element["values"].is_array()
                || element["values"].size() != BMN_GLOBAL_PARAMS_PD)
            {
                LOG(warning) << "   ATTENTION! Correction values for the element " << detID
                             << " are invalid, skipping.";
                continue;
            }
            if (!elementsLoaded)
                AddBlankSolution();
            Int_t detIdx = fpDetModel->Idx(detID);
            json& values = element["values"];
            SVectGL& A = fpResultCurrent->A(detIdx);
            for (Int_t j = 0; j < BMN_GLOBAL_PARAMS_PD; j++)
                A(j) = values[j].get<Double_t>();
            if (element.contains("isFixed")) {
                Bool_t isFixed = element["isFixed"].get<Bool_t>();
                if (fDetectorActive[detIdx] == isFixed) {
                    fDetectorActive[detIdx] = not isFixed;
                    if (isFixed)
                        fActiveDetCount--;
                    else
                        fActiveDetCount++;
                }
            }
            elementsLoaded++;
        }
        fpResultCurrent->ConcatenateA();
        LOG(info) << "      alignment corrections loaded for " << elementsLoaded << " detector elements";
 
        // TODO: load tracks approximation form JSON file if available
        if (withAlpha) {
            if (fpResultLast) {
                LOG(info) << "      loading tracks approximation from last knownsolution";
                fpResultCurrent->Alpha() = fpResultLast->Alpha();
            } else
                LOG(warning) << "   ATTENTION! Local parameters approximation not found.";
        }
    } catch (std::exception& e) {
        LOG(error) << " <!> LoadSolution: failed to parse JSON file " << jsonPath;
        LOG(error) << e.what();
        return kFALSE;
    }
    if (withMSE)
        CalculateMSE(kTRUE);
 
    DoneInTime(timer, "solution loaded");
    return kTRUE;
}
 
template<typename HitType>
Bool_t BmnAligner<HitType>::SaveSolution(const char* jsonPath, Bool_t withAlpha)
{
    TStopwatch timer;
    LOG(state) << " > BmnAligner : saving solution to file " << jsonPath;
    std::ofstream ofs(jsonPath);
    if (!ofs.good()) {
        LOG(error) << " <!> SaveSolution: failed to open output file";
        return kFALSE;
    }
 
    BmnAlignResult& solution{*GetCurrentResult()};
    using namespace nlohmann;
    json js;   // all data to be saved
 
    // alignment solution
    js["CorrectionValues"] = BMN_CORRECTIONS;
    js["DetectorElements"] = BMN_MODULE_COUNT;
    js["CorrectionsPerDetector"] = json::array();
    for (Int_t i = 0; i < BMN_MODULE_COUNT; i++) {
        json element;
        element["detector_id"] = fpDetModel->EncodedID(i);
        element["values"] = json::array();
        for (Int_t j = 0; j < BMN_GLOBAL_PARAMS_PD; j++)
            element["values"].push_back(solution.A(i)(j));
        js["CorrectionsPerDetector"].push_back(element);
    }
 
    // tracks approximation
    if (withAlpha) {
        js["MinHitsPerTrack"] = BMN_MIN_HITS_PER_TRACK;
        js["TracksCount"] = fTotalTracks;
        js["TracksParameters"] = json::array();
        for (Int_t i = 0; i < fTotalTracks; i++) {
            json element = json::array();
            for (Int_t j = 0; j < BMN_LOCAL_PARAMS_PT; j++)
                element.push_back(solution.Alpha(i)(j));
            js["TracksParameters"].push_back(element);
        }
    }
 
    // write data
    try {
        ofs << std::setw(4) << js << std::endl;
    } catch (std::exception& e) {
        LOG(error) << " <!> SaveSolution: failed to write JSON data to file\n" << e.what();
        return kFALSE;
    }
 
    DoneInTime(timer, "solution save");
    return kTRUE;
}
 
/*
 *  Main alignment loop.
 *  Due to non-linearity of dependence between measurements and alignment parameters,
 *  iterative alignment is required.
 *  fixEvery - after main alignmenet procedure fix corrections for detector elements
 *             one by one and repeat alignment (seems to be useless)
 */
template<typename HitType>
Bool_t BmnAligner<HitType>::Align(Bool_t fixEvery)
{
    TStopwatch timer;
    LOG(state) << " > BmnAligner : iterative alignment started";
 
    // check detector elements for data sufficiency
    Int_t minHits{static_cast<Int_t>(BMN_MIN_HITS_PORTION * fTotalTracks / fpDetModel->MaxModulesInStation())};
    for (Int_t i = 0; i < BMN_MODULE_COUNT; i++) {
        if (!fDetectorActive[i])
            continue;
        if (fHitsPerDetector[i] < minHits) {
            LOG(warning) << Form("    detector element %d has only %d hits of %d required, it will be fixed",
                                 fpDetModel->EncodedID(i), fHitsPerDetector[i], minHits);
            fDetectorActive[i] = kFALSE;
            fActiveDetCount--;
        }
    }
 
    // iterative detector elements deactivation - optional feature
    while (fActiveDetCount >= BMN_MIN_ACTIVE_DETECTORS) {
        LOG(info) << "    " << fActiveDetCount << " detector elements active of " << BMN_MODULE_COUNT << " total";
 
        // main alignment loop
        Int_t regularRetries{BMN_MAX_REGULAR_RETRY}, retriesLast;
        Double_t progressAbs{0.0}, progressRel{BMN_MIN_REL_PROGRESS};
        while (std::abs(progressRel) >= BMN_MIN_REL_PROGRESS || regularRetries > 0)
        // regularRetries is never negative, but it is more safe - in case of future code changes
        {
            if (progressAbs < 0.0) {   // no progress at all
                if (!fRegularIters)    // regularization as a last resort
                    fRegularIters = BMN_MIN_REGULAR_ITERS;
                else {
                    LOG(error) << " <!> BmnAligner::Align : algorithm is diverging";
                    return kFALSE;
                }
            } else if (progressRel < BMN_MIN_REL_PROGRESS && !fRegularIters)   // progress is poor
                fRegularIters = BMN_MIN_REGULAR_ITERS;                         // but we have regularization to try
 
            // perform alignment iteration
            if (!IterateAlignment()) {
                LOG(error) << " <!> BmnAligner::Align : iteration failed";
                return kFALSE;
            }
            progressAbs = fpResultLast->GetValueMSE() - fpResultCurrent->GetValueMSE();
            progressRel = progressAbs / fpResultLast->GetValueMSE();
            LOG(info) << Form("    = %+.3f%% relative progress, MSE change : %.3e -> %.3e\n", progressRel * 100.0,
                              fpResultLast->GetValueMSE(), fpResultCurrent->GetValueMSE());
 
            retriesLast = regularRetries;
            if (fRegularIters) {
                // decide about regularization
                if (progressRel >= BMN_REL_PROGRESS_GOOD)
                    fRegularIters = BMN_MIN_REGULAR_ITERS;   // keep going!
                else
                    fRegularIters--;   // attention! be careful to keep it non-negative
                if (!fRegularIters && regularRetries)
                    regularRetries--;
            }
            LOG_IF(info, retriesLast != regularRetries) << "      " << regularRetries << " regularization retries left";
        }   // main alignment loop
 
        // fix best aligned detector element (optional)
        if (fixEvery) {
            BmnAlignResult::IdxValuePair_t MSElist{fpResultCurrent->GetSortedMSE()};
            for (const auto& [idx, mse] : MSElist)
                if (fDetectorActive[idx]) {
                    fDetectorActive[idx] = kFALSE;
                    fActiveDetCount--;
                    break;
                }
        } else
            break;
    }   // detector elements deactivation loop
 
    LOG(info) << "    " << fIterationResults.size() - 1 << " iterations until algorithm convergence criteria reached";
    DoneInTime(timer, "iterative alignment");
    ReportResults();
    return kTRUE;
}
 
/*
 *  One iteration of alignment. Consists of two parts:
 *  1. parallel processing of tracks
 *  2. final calculation of (global) alignment parameters
 *
 *  Parallelized (intermediate) calculations implemented in ProcessTracks()
 *  method below, here only parallelization control is performed. Thus,
 *  calulations code is splitted between two methods: ProcessTracks() and
 *  IterateAlignment(). This code is marked by noticable comments.
 *
 *  Calculations in ProcessTracks() performed according to:
 *  Volker Blobel, Claus Kleinwort
 *  "A New Method for the High-Precision Alignment of Track Detectors"
 *  http://arxiv.org/abs/hep-ex/0208021v1
 *  also widely known as Millepede method.
 *  This paper is referred below as _VBCK_. Variable naming is also corresponds to _VBCK_.
 *
 *  Attention! In _VBCK_ (12) term z_k refers to track model which is incorrect
 *  in non-linear case. In general case it should refer to a _difference_ between
 *  measured value and track model. More accurate definition is given in documentation
 *  on Millepede-II software package published here:
 *  https://www.desy.de/~kleinwrt/MP2/doc/html/draftman_page.html
 */
template<typename HitType>
Bool_t BmnAligner<HitType>::IterateAlignment()
{
    TStopwatch timer;
    fIteration++;
    LOG(state) << "   > #" << fIteration + 1 << " iteration started";
    if (!fInitialized) {
        LOG(error) << "  <!> BmnAligner is not initialized";
        return kFALSE;
    }
    AddBlankSolution();
 
    // prepare storage for parallel calculations
    const Int_t ActiveParamsCnt{fActiveDetCount * BMN_GLOBAL_PARAMS_PD};
    TVectorD vB_dash(ActiveParamsCnt);                    // b' from _VBCK_ (17)
    TMatrixD mC_dash(ActiveParamsCnt, ActiveParamsCnt);   // C' from _VBCK_ (17)
 
    // run Millepede-like dimension reduction calculations in parallel
    LOG(info) << "      " << fThreadsCnt << " threads started to perform dimension reduction (Millepede)";
    fProgressBar.Init();
    TStopwatch timerMille;
    _BMN_ALIGNER_RUN_THREADS(&BmnAligner::ProcessTracks, this, &vB_dash, &mC_dash)
    LOG(info) << Form("      %.1fs (REAL) / %.1fs (CPU) took to complete dimension reduction", timerMille.RealTime(),
                      timerMille.CpuTime());
    fProgressBar.Clear();
 
    /*
     *  ---------------------------------------------------
     *  Final calculations of global parameters: _VBCK_(16)
     *  ---------------------------------------------------
     */
 
    /*
     *  Prepare system of linear equations for global parameters
     *  --------------------------------------------------------
     *  1. make a copy of vB_dash and mC_dash (original matrices will be used too)
     *  2. append with user defined constraints taking into account active detectors
     *  3. remove zero constraints if any (because of inactive detectors)
     *  4. iteratively solve linear systems with different regularization factors
     *  5. check results of iterative regularization
     *  6. select optimal regularization factor if possible
     */
 
    TStopwatch timerGlobal;
    fMSEclcCounter = 0u;
 
    TVectorD vBL(vB_dash);
    TMatrixD mCL(mC_dash);
    if (fConstraintsCnt) {
        // add user defined constraints to the system
        vBL.ResizeTo(ActiveParamsCnt + fConstraintsCnt);
        mCL.ResizeTo(ActiveParamsCnt + fConstraintsCnt, ActiveParamsCnt + fConstraintsCnt);
        Double_t weight;
        std::vector<Bool_t> constActive(fConstraintsCnt, false);
        for (Int_t i = 0, k = 0; i < BMN_GLPARAMS_TOTAL; i++) {
            if (!fDetectorActive[i / BMN_GLOBAL_PARAMS_PD])
                continue;
            for (Int_t j = 0; j < fConstraintsCnt; j++) {
                weight = fConstraints[j](i);
                if (weight) {
                    mCL(k, ActiveParamsCnt + j) = weight;
                    mCL(ActiveParamsCnt + j, k) = weight;
                    constActive[j] = kTRUE;
                }
            }
            k++;
        }
        // remove zero constraints if any
        TMatrixD row(1, mCL.GetNcols());
        Int_t dstIdx{ActiveParamsCnt - 1},   // one less then first constraint
            maxIdx{ActiveParamsCnt + fConstraintsCnt - 1};
        for (Int_t srcIdx = ActiveParamsCnt, i = 0; srcIdx <= maxIdx; srcIdx++, i++) {
            if (!constActive[i])
                continue;
            if (++dstIdx == srcIdx)
                continue;
            mCL.GetSub(srcIdx, srcIdx, 0, maxIdx, row);
            mCL.SetSub(dstIdx, 0, row);
            mCL.SetSub(0, dstIdx, row.T());   // mCL is symmetric
        }
        dstIdx++;   // total number of active rows in mCL
        mCL.ResizeTo(dstIdx, dstIdx);
        vBL.ResizeTo(dstIdx);
        LOG(info) << "      " << dstIdx - ActiveParamsCnt << " active parameter constraints applied";
    }
    // TODO: remove inactivated constraints from fConstraints
 
    if (!fRegularIters) {   // no regularization
        LOG(info) << "      solving non-regularized system for global parameters...";
        if (!SolveGlobal(mCL, vBL)) {
            LOG(warning) << "      iteration failed to improve MSE";
            *fpResultCurrent = *fpResultLast;
        }
    } else {   // Levenberg-Marquardt (LM) regularization
 
        LOG(info) << "      starting Levenberg-Marquardt regularization...";
        // find crude range for LM factor in which to perform 1D optimization
        Double_t valueMSEbest{fpResultLast->GetValueMSE()}, LMfactorBest{0.0}, LMfactor{1.0};
        Int_t LMiterarion{0};
        Bool_t LMworking{kTRUE};
        while ((LMworking || LMiterarion < BMN_LM_ITERATIONS_MIN) && LMiterarion < BMN_LM_ITERATIONS_MAX) {
            if (LMfactor > 1.0)
                // apply normalization factor to diagonal elements
                for (Int_t i = 0; i < ActiveParamsCnt; i++)
                    mCL(i, i) = mC_dash(i, i) * LMfactor;
 
            if (SolveGlobal(mCL, vBL)) {
                if (LMfactor == 1.0)
                    // at first check if non-regularized solution is good enough
                    if (1.0 - fpResultCurrent->GetValueMSE() / valueMSEbest >= BMN_REL_PROGRESS_GOOD) {
                        // cancel regularization mode but keep retry count
                        fRegularIters = 0;
                        break;
                    }
 
                // evaluate results of successful iteration
                if (fpResultCurrent->GetValueMSE() < valueMSEbest) {
                    valueMSEbest = fpResultCurrent->GetValueMSE();
                    LMfactorBest = LMfactor;
                } else
                    LMworking = kFALSE;   // stop if MSE is not improved
            } else
                LOG(warning) << Form("      LM regularization failed (F=%.3e)", LMfactor);
 
            LMfactor *= BMN_LM_MULT_STEP;
            LMiterarion++;
        }   // end of Levenberg-Marquardt regularization loop
 
        // regularization factor optimization function
        ROOT::Math::Functor1D fnLM([&mCL, &mC_dash, &vBL, this, ActiveParamsCnt](Double_t factor) {
            TMatrixD mCLcopy{mCL};
            for (Int_t i = 0; i < ActiveParamsCnt; i++)
                mCLcopy(i, i) = mC_dash(i, i) * factor;
            if (!SolveGlobal(mCLcopy, vBL))   // expected to allways succeed
                throw std::runtime_error(Form("Levenberg-Marquardt failed (F=%.3e)", factor));
            return fpResultCurrent->GetValueMSE();
        });
        // classification of Levenberg-Marquardt results
        if (!fRegularIters) {
            CalculateMSE(kTRUE);
            LOG(info) << Form(
                "      regularization cancelled : sufficient %+.1f%% of progress achieved using non-regularized method",
                100.0 * (fpResultCurrent->GetValueMSE() / fpResultLast->GetValueMSE() - 1.0));
 
        } else if (LMfactorBest < 1.0) {
            LOG(warning) << "      iteration failed to improve MSE";
            *fpResultCurrent = *fpResultLast;
 
        } else if (LMfactorBest == 1.0 || LMworking) {   // it works like XOR here
            // LMfactorBest has boundary values
            if (LMworking)
                LOG(info)   // max possible LMfactor
                    << Form("      F = %.3e : LM regularization approached gradient descent", LMfactorBest);
            else
                LOG(info)   // min possible LMfactor (1.0)
                    << "      F = 1.0 : LM regularization did not improve results";
            fnLM(LMfactorBest);
            CalculateMSE(kTRUE);
 
        } else {   // LMfactorBest > 1.0 and LMworking is false
            // search for optimal LMfactor between LMfactorBest and LMfactorBest * BMN_LM_MULT_STEP
 
            ROOT::Math::BrentMinimizer1D brent;
            brent.SetNpx(BMN_BRENT_GRID_SIZE);
            brent.SetFunction(fnLM, LMfactorBest, LMfactorBest * BMN_LM_MULT_STEP);
            Bool_t success{brent.Minimize(static_cast<Int_t>(0.1 / BMN_1D_STEP_TOLERANCE), BMN_1D_STEP_TOLERANCE * 10.0,
                                          BMN_1D_STEP_TOLERANCE)};
            if (success)
                LOG(info) << Form("      F = %.3e : optimal LM factor value", brent.XMinimum());
            else
                LOG(warning) << Form("      F = %.3e : rough LM factor value used, 1D optimization failed",
                                     LMfactorBest);
            fnLM(success ? brent.XMinimum() : LMfactorBest);
            CalculateMSE(kTRUE);
 
        }   // end if (LM results classification)
    }   // end if (regularization)
    LOG(info) << Form("      %d MSE value calculations performed during iteration", fMSEclcCounter);
    LOG(info) << Form("      %.1fs (REAL) / %.1fs (CPU) took to complete parameters estimation", timerGlobal.RealTime(),
                      timerGlobal.CpuTime());
 
    // don't call MarkTime() here - it will be done in Align()
    LOG(state) << Form("   < #%d iteration took %.1fs (REAL) / %.1fs (CPU)", fIteration + 1, timer.RealTime(),
                       timer.CpuTime());
    return kTRUE;
}
 
template<typename HitType>
Bool_t BmnAligner<HitType>::SolveGlobal(const TMatrixD& mCL, const TVectorD& vBL)
{
    /*
     *  Solve system of linear equations for global parameters
     *  ------------------------------------------------------
     *  1. get direct solution using LU or SVD decomposition
     *  2. store Lagrange multipliers and SVD singular values (for analysis if required)
     *  3. append solution with zeroes for inactive detectors
     *  4. find optimal step size in proposed direction
     *  5. calculate new approximation of global parameters
     *  6. in case of any failure proceed to next regularization factor
     */
    // TODO: not necessary to solve full system with large LMfactor
    const Int_t ActiveParamsCnt{fActiveDetCount * BMN_GLOBAL_PARAMS_PD};
 
    // Suppress ROOT error messages: LU decomposition expected to fail sometimes
    auto oldErrorLevel{gErrorIgnoreLevel};
    gErrorIgnoreLevel = kBreak;
    TDecompLU mLU{mCL};
    Bool_t success{mLU.Decompose()};
    TDecompSVD mSVD{mCL};   // we need SVD anyway to evaluate singularity of system
    mSVD.Decompose();
    gErrorIgnoreLevel = oldErrorLevel;   // Restore original error level
    fpResultCurrent->SVDSig().ResizeTo(mSVD.GetSig());
    fpResultCurrent->SVDSig() = mSVD.GetSig();
 
    TVectorD vAL{success ? mLU.Invert(success) * vBL : mSVD.Solve(vBL, success)};
    if (!success) {
        LOG(warning) << "      <!> global matrix inversion unsuccessful";
        return kFALSE;
    }
 
    // separate Lagrange multipliers
    Int_t activeConstCnt{vAL.GetNrows() - ActiveParamsCnt};
    if (activeConstCnt) {
        fpResultCurrent->L().ResizeTo(activeConstCnt);
        vAL.GetSub(ActiveParamsCnt, vAL.GetNrows() - 1, fpResultCurrent->L());
    }
    // expand vAL to full vector of global parameters
    Double_t *dst{fpResultCurrent->A().begin()}, *src{vAL.GetMatrixArray()};
    for (Int_t i = 0; i < BMN_MODULE_COUNT; i++)
        if (fDetectorActive[i])
            for (Int_t j = 0; j < BMN_GLOBAL_PARAMS_PD; j++)
                *dst++ = *src++;
        else
            dst += BMN_GLOBAL_PARAMS_PD;
 
    if (fRegularIters) {   // find optimal step in proposed direction
        // 1D function to minimize
        ROOT::Math::Functor1D fnMSE([this](Double_t step) {
            SVectGLT point{fpResultLast->A() + fpResultCurrent->A() * step};
            return CalculateMSE(point);
        });
        // search for initial segment containing the minimum
        Double_t valueMSE, stepMin{0.0}, stepMax{1.0};
        valueMSE = fnMSE(stepMax);
        while (valueMSE < fpResultLast->GetValueMSE()) {
            stepMin = stepMax;
            stepMax *= 2.0;
            valueMSE = fnMSE(stepMax);
        }
        // find minimum using Brent method
        constexpr static const Int_t iterations{static_cast<Int_t>(1.0 / BMN_1D_STEP_TOLERANCE)};
        fBrent.SetFunction(fnMSE, stepMin, stepMax);
        success = fBrent.Minimize(iterations, BMN_1D_STEP_TOLERANCE, BMN_1D_STEP_TOLERANCE * 0.1);
        if (!success) {
            LOG(warning) << "      Brent method failed to calculate step size";
            return kFALSE;
        }
 
        // select new approximation of global parameters
        fpResultCurrent->A() *= fBrent.XMinimum();
    }   // end if (regularization)
 
    fpResultCurrent->A() += fpResultLast->A();
    fpResultCurrent->SeparateA();
    CalculateMSE(kFALSE);
 
    return kTRUE;
}
 
/*
 *  Millepede-like dimension reduction algorithm.
 *  For detailes see:
 *  http://arxiv.org/abs/hep-ex/0208021v1
 *  https://www.desy.de/~kleinwrt/MP2/doc/html/draftman_page.html
 *
 *  Each thread processes separate subset of all tracks defined by iterator.
 *  Synchronization is necessary at result storage stage only.
 */
template<typename HitType>
void BmnAligner<HitType>::ProcessTracks(TVectorD* pvB_dash, TMatrixD* pmC_dash, Int_t threadNum)
{
    // prepare vector of zeroes to be used for variables initialization
    constexpr static const Int_t SMatrMaxSize{
        std::max(std::max(static_cast<Int_t>(SMatrGL::kSize), static_cast<Int_t>(SMatrLC::kSize)),
                 static_cast<Int_t>(SMatrGP::kSize))};
    static const std::vector<Double_t> zeroes(SMatrMaxSize, 0.0);
    static const auto zStart = zeroes.begin();
    const Int_t ActiveParamsCnt{fActiveDetCount * BMN_GLOBAL_PARAMS_PD};
 
    // ROOT matrices and vectors are initialized with zeroes during construction
    // accelerated ROOT vectors and matrices
    SVectGL vGradG,
        vB[BMN_MODULE_COUNT];       // get memory for full set of detectors,
    SMatrGL mC[BMN_MODULE_COUNT];   // not for active only, for loops optimization
    SMatrGP mG[BMN_MODULE_COUNT];
    SMatrLC mGamma;
    SVectLC vBeta, vDelta, vGradL;
    // standard non-accelerated aliases to variables of accelerated types
    TVectorD vaDelta(BMN_LOCAL_PARAMS_PT);
    TMatrixD maGamma(BMN_LOCAL_PARAMS_PT, BMN_LOCAL_PARAMS_PT);
    vaDelta.Use(BMN_LOCAL_PARAMS_PT, vDelta.Array());
    maGamma.Use(BMN_LOCAL_PARAMS_PT, BMN_LOCAL_PARAMS_PT, mGamma.Array());
    // non-accelerated ROOT matrices and vectors due to runtime-defined sizes
    TMatrixD mGfull(ActiveParamsCnt, BMN_LOCAL_PARAMS_PT), mCfull(ActiveParamsCnt, ActiveParamsCnt),
        mC_part(ActiveParamsCnt, ActiveParamsCnt);
    TVectorD vBfull(ActiveParamsCnt), vB_part(ActiveParamsCnt);
 
    Double_t z, omega;
 
    // iterate tracks
    BmnDataIterator* pItr{fpIterators[threadNum]};
    Int_t iTrack{fFirstTracksIdx[threadNum]};
    pItr->ResetAll();
    while (!pItr->EndOfTracks()) {
 
        // skip unusable tracks
        if (!fpTrackUsable->at(iTrack)) {
            iTrack++;
            pItr->NextTrack();
            continue;
        }
 
        /*
         *  ---------------------------------------------------
         *  Local parameters section
         *  ---------------------------------------------------
         */
 
        // reset cumulative variables to zero
        vBeta.SetElements(zStart, SVectLC::kSize);
        mGamma.SetElements(zStart, SMatrLC::kSize);
 
        // get previous approximation of local parameters for this track
        SVectLC& lc{fpResultLast->Alpha(iTrack)};
        // iterate hits
        pItr->ResetHits();
        while (!pItr->EndOfHits()) {
            // TODO: cache gradient calculations - it does not depend on x and y
 
            // get previous approximation of global parameters for this hit (detector)
            const SVectGL& gl{fpResultLast->A(fpDetModel->Idx(pItr->HitDetectorID()))};
 
            // notice: the code below is independent of corrections sign selection
            z = pItr->HitZ();
 
            omega = fOmegaScaleFactor * pItr->HitWx();
            vGradL = fpMeasureModel->GradXlocal(z, gl, lc);
            mGamma += omega * ROOT::Math::TensorProd(vGradL, vGradL);
            vBeta += (omega * (pItr->HitX() - fpMeasureModel->X(z, gl, lc))) * vGradL;
 
            omega = fOmegaScaleFactor * pItr->HitWy();
            vGradL = fpMeasureModel->GradYlocal(z, gl, lc);
            mGamma += omega * ROOT::Math::TensorProd(vGradL, vGradL);
            vBeta += (omega * (pItr->HitY() - fpMeasureModel->Y(z, gl, lc))) * vGradL;
 
            pItr->NextHit();
        }   // loop by hits
 
        // local parameters alpha : _VBCK_(11)
        // pay attantion that inverse of Gamma is stored in mGamma
        // TODO: check if Cholesky inversion is aplicable
        if (!mGamma.Invert())
            throw std::runtime_error("matrix inversion failed while calculating local parameters");
        vDelta = mGamma * vBeta;
        // store updated local parameters (no sycn is needed between threads)
        fpResultCurrent->Alpha(iTrack) = fpResultLast->Alpha(iTrack) + vDelta;
 
        /*
         *  ---------------------------------------------------
         *  Global parameters section
         *  ---------------------------------------------------
         */
 
        // TODO: put global section together with local in same loop - can't use updated local parameter values
 
        // reset cumulative variables to zero
        for (Int_t i = 0; i < BMN_MODULE_COUNT; i++) {
            if (!fDetectorActive[i])
                continue;   // working only with active detectors
            vB[i].SetElements(zStart, SVectGL::kSize);
            mC[i].SetElements(zStart, SMatrGL::kSize);
            mG[i].SetElements(zStart, SMatrGP::kSize);
        }
 
        // iterate hits
        pItr->ResetHits();
        while (!pItr->EndOfHits()) {
            const Int_t iDet{fpDetModel->Idx(pItr->HitDetectorID())};
            if (!fDetectorActive[iDet]) {   // working only with active detectors
                pItr->NextHit();
                continue;
            }
 
            // We use global parameters approximation from previous iteration,
            // although we have new, more precise values, at this moment.
            // It is intended: usage of new values is not consistent with
            // mathematical model of MILLEPEDE method of dimemsionality reduction.
            const SVectGL& gl{fpResultLast->A(iDet)};
 
            // notice: the code below is independent of corrections sign selection
            z = pItr->HitZ();
 
            omega = fOmegaScaleFactor * pItr->HitWx();
            vGradL = fpMeasureModel->GradXlocal(z, gl, lc);
            vGradG = fpMeasureModel->GradXglobal(z, gl, lc);
            mG[iDet] += omega * ROOT::Math::TensorProd(vGradG, vGradL);
            mC[iDet] += omega * ROOT::Math::TensorProd(vGradG, vGradG);
            vB[iDet] += (omega * (pItr->HitX() - fpMeasureModel->X(z, gl, lc))) * vGradG;
 
            omega = fOmegaScaleFactor * pItr->HitWy();
            vGradL = fpMeasureModel->GradYlocal(z, gl, lc);
            vGradG = fpMeasureModel->GradYglobal(z, gl, lc);
            mG[iDet] += omega * ROOT::Math::TensorProd(vGradG, vGradL);
            mC[iDet] += omega * ROOT::Math::TensorProd(vGradG, vGradG);
            vB[iDet] += (omega * (pItr->HitY() - fpMeasureModel->Y(z, gl, lc))) * vGradG;
 
            pItr->NextHit();
        }   // loop by hits
 
        // collecting C_dash, b_dash : _VBCK_(17)
        TVectorD vaB(BMN_GLOBAL_PARAMS_PD);
        TMatrixD maG(BMN_GLOBAL_PARAMS_PD, BMN_LOCAL_PARAMS_PT), maC(BMN_GLOBAL_PARAMS_PD, BMN_GLOBAL_PARAMS_PD);
        for (Int_t i = 0, j = 0; i < BMN_MODULE_COUNT; i++) {
            if (!fDetectorActive[i])
                continue;   // working only with active detectors
            vaB.Use(BMN_GLOBAL_PARAMS_PD, vB[i].Array());
            maG.Use(BMN_GLOBAL_PARAMS_PD, BMN_LOCAL_PARAMS_PT, mG[i].Array());
            maC.Use(BMN_GLOBAL_PARAMS_PD, BMN_GLOBAL_PARAMS_PD, mC[i].Array());
            vBfull.SetSub(j, vaB);
            mGfull.SetSub(j, 0, maG);
            mCfull.SetSub(j, j, maC);
            j += BMN_GLOBAL_PARAMS_PD;   // destination index
        }
        TMatrixD mGfullT(TMatrixD::kTransposed, mGfull);
        mC_part += mCfull - mGfull * maGamma * mGfullT;
        vB_part += vBfull - mGfull * vaDelta;
 
        iTrack++;
        pItr->NextTrack();
    }   // loop by tracks
 
    // store final result
    std::lock_guard<std::mutex> lock(fMutexGlobal);
    *pmC_dash += mC_part;
    *pvB_dash += vB_part;
}
 
template<typename HitType>
Double_t BmnAligner<HitType>::CalculateMSE(const SVectGLT& solution)
{
    // to calculate MSE at arbitrary point we substitute this point in place of
    // final solution to keep cumbersome multithreaded MSE calculation scheme intact
    BmnAlignResult *pBackupSolution{fpResultCurrent}, tempSolution(*fpResultCurrent);
    tempSolution.A() = solution;
    tempSolution.SeparateA();
    fpResultCurrent = &tempSolution;
    Double_t valueMSE{CalculateMSE(kFALSE)};
    fpResultCurrent = pBackupSolution;
    return valueMSE;
}
 
template<typename HitType>
Double_t BmnAligner<HitType>::CalculateMSE(Bool_t withResiduals)
{
    fMSEclcCounter++;   // MSE calculation is expensive because it requires to traverse all hits
    fpResultCurrent->ResetValueMSE();
    if (withResiduals) {
        fpResultCurrent->ResidualsX().Reset();
        fpResultCurrent->ResidualsY().Reset();
    }
    if (fMSEclcSlow)
        fProgressBar.Init();
    _BMN_ALIGNER_RUN_THREADS(&BmnAligner::CalculateMSEpart, this, withResiduals)
    if (fMSEclcSlow)
        fProgressBar.Clear();
    else if (0 == fMSEclcCounter % fMSEclcTick)
        std::cout << " " << fMSEclcCounter << " MSE calculations\r" << std::flush;
    return fpResultCurrent->GetValueMSE();
}
 
template<typename HitType>
void BmnAligner<HitType>::CalculateMSEpart(Bool_t withResiduals, Int_t threadNum)
{
    Double_t delta, omega;
    std::array<Double_t, BMN_MODULE_COUNT> arrMSE;
    TH1D *pResX, *pResY;
 
    arrMSE.fill(0.0);
    if (withResiduals) {
        pResX = new TH1D(Form("residualsX_%lu", fpResultCurrent->GetHistCounter()), "Partial residuals by X",
                         BMN_RESIDUALS_BINS, -BMN_RESIDUALS_RANGE_X, BMN_RESIDUALS_RANGE_X);
        pResY = new TH1D(Form("residualsY_%lu", fpResultCurrent->GetHistCounter()), "Partial residuals by Y",
                         BMN_RESIDUALS_BINS, -BMN_RESIDUALS_RANGE_Y, BMN_RESIDUALS_RANGE_Y);
    }
 
    Int_t iTrack;
    BmnDataIterator* pItr;
    if (threadNum == -1) {   // single-thread computation
        iTrack = 0;
        pItr = fpIteratorMain;
    } else {   // multi-thread computation
        iTrack = fFirstTracksIdx[threadNum];
        pItr = fpIterators[threadNum];
    }
    pItr->ResetAll();
    while (!pItr->EndOfTracks()) {
        if (!fpTrackUsable->at(iTrack)) {
            iTrack++;
            pItr->NextTrack();
            continue;
        }
        // notice: the code below is independent of corrections sign selection
        const SVectLC& lc{fpResultCurrent->Alpha(iTrack)};
        while (!pItr->EndOfHits()) {
            Int_t detectorIdx{fpDetModel->Idx(pItr->HitDetectorID())};
            const SVectGL& gl{fpResultCurrent->A(detectorIdx)};
 
            omega = fOmegaScaleFactor * pItr->HitWx();
            delta = pItr->HitX() - fpMeasureModel->X(pItr->HitZ(), gl, lc);
            arrMSE[detectorIdx] += omega * delta * delta;
            if (withResiduals && TMath::Abs(delta) > BMN_ALMOST_ZERO)
                // ignore almost precise zero residuals which occure
                // by construction of local parameters zero approximation
                pResX->Fill(delta);
 
            omega = fOmegaScaleFactor * pItr->HitWy();
            delta = pItr->HitY() - fpMeasureModel->Y(pItr->HitZ(), gl, lc);
            arrMSE[detectorIdx] += omega * delta * delta;
            if (withResiduals && TMath::Abs(delta) > BMN_ALMOST_ZERO)
                // ignore almost precise zero residuals which occure
                // by construction of local parameters zero approximation
                pResY->Fill(delta);
 
            pItr->NextHit();
        }
        iTrack++;
        pItr->NextTrack();
    }
 
    // store results
    std::lock_guard<std::mutex> lock(fMutexGlobal);
    for (Int_t iDet = 0; iDet < BMN_MODULE_COUNT; iDet++)
        fpResultCurrent->AddValueMSE(iDet, arrMSE[iDet]);
    if (withResiduals) {
        fpResultCurrent->ResidualsX().Add(pResX);
        fpResultCurrent->ResidualsY().Add(pResY);
        delete pResX;
        delete pResY;
    }
}
 
template<typename HitType>
void BmnAligner<HitType>::ReportResults() const
{
    BmnAlignResult &before{*GetResult(kFALSE)}, &after{*GetResult(kTRUE)};
    Int_t idxLast{after.SVDSig().GetNrows() - 1};
    static const char* drawLine{"================================================================================"};
    LOG(info) << "";
    LOG(info) << drawLine;
    LOG(info) << Form("%.2e -> %.2e (%+.1f%%) : MSE value change", before.GetValueMSE(), after.GetValueMSE(),
                      100.0 * (after.GetValueMSE() - before.GetValueMSE()) / before.GetValueMSE());
    LOG(info) << Form(
        "%.2e -> %.2e (%+.1f%%) : mean error in X change", before.ResidualsX().GetMean(), after.ResidualsX().GetMean(),
        100.0 * (after.ResidualsX().GetMean() - before.ResidualsX().GetMean()) / before.ResidualsX().GetMean());
    LOG(info) << Form(
        "%.2e -> %.2e (%+.1f%%) : mean error in Y change", before.ResidualsY().GetMean(), after.ResidualsY().GetMean(),
        100.0 * (after.ResidualsY().GetMean() - before.ResidualsY().GetMean()) / before.ResidualsY().GetMean());
    LOG(info) << Form("%.2e -> %.2e (%+.1f%%) : error stddev in X change", before.ResidualsX().GetStdDev(),
                      after.ResidualsX().GetStdDev(),
                      100.0 * (after.ResidualsX().GetStdDev() - before.ResidualsX().GetStdDev())
                          / before.ResidualsX().GetStdDev());
    LOG(info) << Form("%.2e -> %.2e (%+.1f%%) : error stddev in Y change", before.ResidualsY().GetStdDev(),
                      after.ResidualsY().GetStdDev(),
                      100.0 * (after.ResidualsY().GetStdDev() - before.ResidualsY().GetStdDev())
                          / before.ResidualsY().GetStdDev());
    LOG(info) << Form("SVD minimum singular value : %.2e", after.SVDSig()(idxLast));
    LOG(info) << Form("SVD condition number       : %.2e", after.SVDSig()(0) / after.SVDSig()(idxLast));
    LOG(info) << drawLine;
    Int_t cpuTime{static_cast<Int_t>(fTotalCpuTime)}, realTime{static_cast<Int_t>(fTotalRealTime)};
    LOG(info) << Form("total CPU time spent .......... %dh %02dm %02ds", cpuTime / 3600, (cpuTime / 60) % 60,
                      cpuTime % 60);
    LOG(info) << Form("total real time spent ......... %dh %02dm %02ds", realTime / 3600, (realTime / 60) % 60,
                      realTime % 60);
    LOG(info) << Form("multithreading boost factor ... x%.1f", fTotalCpuTime / fTotalRealTime);
}
 
template<typename HitType>
void BmnAligner<HitType>::InitIterators()
{
    // check number of requested threads
    Int_t max_threads = fTotalTracks / BMN_MIN_TRACKS_PER_THREAD;
    if (!max_threads)
        max_threads = 1;
    if (max_threads < fThreadsCnt) {
        LOG(warning) << "      too many threads requested for the size of dataset, using " << max_threads
                     << " threads instead of " << fThreadsCnt;
        fThreadsCnt = max_threads;
    }
    if (fpIteratorMain == fpRootIterator && fThreadsCnt > 1) {
        LOG(warning) << "      parallelization implemented for RAM mode only";
        fThreadsCnt = 1;
    }
 
    if (fThreadsCnt > 1) {
        // create separate RamIterator for each thread
        fFirstTracksIdx.resize(fThreadsCnt);
        fpIterators.resize(fThreadsCnt);
        Int_t tracksPerThread = fTotalTracks / fThreadsCnt;
        for (Int_t i = 0, iTrack = 0; i < fThreadsCnt; i++, iTrack += tracksPerThread) {
            fFirstTracksIdx[i] = iTrack;
            fpIterators[i] = new BmnRamIterator(fpRamIterator->Slice(
                iTrack,
                i == fThreadsCnt - 1                       // last thread case
                    ? fTotalTracks - tracksPerThread * i   // the rest of tracks for the last thread
                    : tracksPerThread));
        }
    } else {
        fFirstTracksIdx.resize(1);
        fpIterators.resize(1);
        fFirstTracksIdx[0] = 0;
        fpIterators[0] = fpIteratorMain;
    }
}
 
template<typename HitType>
void BmnAligner<HitType>::DoneInTime(TStopwatch& timer, const char* stage)
{
    MarkTime(timer);
    LOG(state) << Form(" < BmnAligner : %s completed in %.1fs (REAL) / %.1fs (CPU)", stage, timer.RealTime(),
                       timer.CpuTime());
}
 
// visualization works in ROOT macro mode only
template<typename HitType>
void BmnAligner<HitType>::PrepareDrawing()
{
    // prepare canvas and pads
    static const UInt_t ww{1900}, wh{1010};
    TCanvas* pCnv = new TCanvas(   // RAM is not released in macro mode
        "canvas", "BM@N alignment");
    pCnv->SetRealAspectRatio();
    pCnv->SetFixedAspectRatio();
    pCnv->SetCanvasSize(ww, wh);
    pCnv->SetWindowSize(ww + 20, wh + 70);
 
    // divide canvas into pads
    // pay attention: dynamic RAM is not released in ROOT macro mode
    fpPad3D = new TPad("pad3D", "3D", 0.0, 0.34, 0.6, 1.0);
    fpPadG1 = new TPad("padG1", "G1", 0.0, 0.0, 0.6, 0.34);
    fpPadG2 = new TPad("padG2", "G2", 0.6, 0.67, 1.0, 1.0);
    fpPadG3 = new TPad("padG3", "G3", 0.6, 0.34, 1.0, 0.67);
    fpPadG4 = new TPad("padG4", "G4", 0.6, 0.0, 1.0, 0.34);
    fpPad3D->Draw();
    fpPadG1->Draw();
    fpPadG2->Draw();
    fpPadG3->Draw();
    fpPadG4->Draw();
}
 
template<typename HitType>
void BmnAligner<HitType>::DrawResiduals()
{
    // Draw residuals histograms
    TH1D* pHist;
 
    fpPadG3->cd();
    fpPadG3->SetTitle("Residuals by Y");
    // fpPadG3->SetLogy();
    pHist = &GetResult(kFALSE)->ResidualsY();
    pHist->SetFillColor(kCyan);
    pHist->SetLineColor(kCyan);
    pHist->GetYaxis()->SetRangeUser(0.0, GetResult(kTRUE)->ResidualsY().GetMaximum() * 1.1);
    pHist->Draw();
    pHist = &GetResult(kTRUE)->ResidualsY();
    pHist->SetLineColor(kRed);
    pHist->Draw("CSAME");
 
    fpPadG2->cd();
    fpPadG2->SetTitle("Residuals by X");
    // fpPadG2->SetLogy();
    pHist = &GetResult(kFALSE)->ResidualsX();
    pHist->SetFillColor(kTeal);
    pHist->SetLineColor(kTeal);
    pHist->GetYaxis()->SetRangeUser(0.0, GetResult(kTRUE)->ResidualsX().GetMaximum() * 1.1);
    pHist->Draw();
    pHist = &GetResult(kTRUE)->ResidualsX();
    pHist->SetLineColor(kRed);
    pHist->Draw("CSAME");
}
 
template<typename HitType>
void BmnAligner<HitType>::Draw()   // virtual
{
    PrepareDrawing();
 
    // draw some tracks in 3D pad
 
    fpPad3D->cd();
    fpPad3D->SetTitle("Some random tracks");
    const Double_t maxZdraw{fMaxZvalue * 1.1};
    const Double_t trackProbability{static_cast<Double_t>(BMN_DRAW_TRACKS_CNT)
                                    / static_cast<Double_t>(fTracksUsableCnt)};
    TRandom trueRnd(0);
    TPolyLine3D *pTrackLines = new TPolyLine3D[BMN_DRAW_TRACKS_CNT],
                *pApproxLines = new TPolyLine3D[BMN_DRAW_TRACKS_CNT];
 
    fProgressBar.Init();
    Int_t iTrack{0}, iTrackDrawn{0};
    fpIteratorMain->ResetAll();
    while (iTrackDrawn < BMN_DRAW_TRACKS_CNT && !fpIteratorMain->EndOfTracks()) {
        // skip tracks according to probability
        if (!fpTrackUsable->at(iTrack) || trueRnd.Uniform() > trackProbability) {
            fpIteratorMain->NextTrack();
            iTrack++;
            continue;
        }
 
        // draw track itself
        Int_t iHit{0};
        while (!fpIteratorMain->EndOfHits()) {
            // x and z are swapped for visual convenience
            pTrackLines[iTrackDrawn].SetPoint(iHit, fpIteratorMain->HitZ(), fpIteratorMain->HitY(),
                                              fpIteratorMain->HitX());
            fpIteratorMain->NextHit();
            iHit++;
        }   // loop by hits
        pTrackLines[iTrackDrawn].SetLineColor(kBlue);
        pTrackLines[iTrackDrawn].Draw();
 
        // draw liner approximation of the track
        const SVectLC& Alpha{GetResult()->Alpha(iTrack)};
        pApproxLines[iTrackDrawn].SetPoint(0, 0.0, Alpha(iY0), Alpha(iX0));
        pApproxLines[iTrackDrawn].SetPoint(1, maxZdraw, Alpha(iY0) + maxZdraw * Alpha(iY1),
                                           Alpha(iX0) + maxZdraw * Alpha(iX1));
        pApproxLines[iTrackDrawn].SetLineColor(kRed);
        pApproxLines[iTrackDrawn].SetLineStyle(kDotted);
        pApproxLines[iTrackDrawn].Draw();
 
        iTrackDrawn++;
        fpIteratorMain->NextTrack();
        iTrack++;
    }   // loop by tracks
    fProgressBar.Clear();
 
    DrawResiduals();
}
 
// Instantiate the template
template class BmnAligner<>;
 
#include "BmnATestHit.h"
template class BmnAligner<BmnATestHit>;