TimePixLocalRoadSearchProcessor.cc

Go to the documentation of this file.

// #ifdef USE_TPROADS
#include "TimePixLocalRoadSearchProcessor.h"

#include "SimpleHelixTrackModel.h"

#include <iostream>
#include <string>
#include <ctime>
#include <cmath>

//GEAR
#include <gear/TPCParameters.h>
#include <gear/BField.h>
#include <gearimpl/Vector3D.h>

// include what you need from lcio
#include <EVENT/LCEvent.h>
#include <IMPL/LCCollectionVec.h>
#include <UTIL/BitField64.h>
#include <UTIL/ILDConf.h>
#include <IMPL/TrackerHitImpl.h>
#include <IMPL/TrackImpl.h>
#include <IMPL/LCFlagImpl.h>
#include <Exceptions.h>

// this is the inserted part

using namespace lcio;
using namespace marlin;
using namespace std;
using namespace gear;

namespace marlintpc {

static bool comparePixelsZ(tp_Pixel* one, tp_Pixel* two) {
        return ((one->getZ()) < (two->getZ())); // increasing
}

static bool comparePixelsS(tp_Pixel* one, tp_Pixel* two) {
        return ((one->getArcLength()) < (two->getArcLength())); // increasing
}

static bool compareSegments(tp_PixelSegment* one, tp_PixelSegment* two) {
        return ((one->getLength()) > (two->getLength())); // decreasing
}

TimePixLocalRoadSearchProcessor aTimePixLocalRoadSearchProcessor;

TimePixLocalRoadSearchProcessor::TimePixLocalRoadSearchProcessor() :
                Processor("TimePixLocalRoadSearchProcessor") {
        // the processor description
        _description = "TimePixLocalRoadSearchProcessor is intended to ...";

        registerInputCollection(LCIO::TRACKERHIT, "InputHits", "Name of the input Hit Collection", _inputColName, string("TPCHits"));
        registerOutputCollection(LCIO::TRACK, "OutputTracks", "Name of the output Track Collection", _outputColName,
                        string("RoadSearchTracks"));

        registerOptionalParameter("BFieldScaleFactor", "Scales magnetic field (map), use 1.0 (default) for field ON or 0.0 for field OFF",
                        _bfieldScaleFactor, double(1.0));

        registerProcessorParameter("MinTrackLevel", "Min. track level", _minTrackLevel, int(1));

        registerProcessorParameter("MacroPixelSize", "Macro pixel (bin) size", _mpSize, int(32));

        registerProcessorParameter("MinZmeas", "Min. measured Z value", _minZmeas, double(0.001));

        registerProcessorParameter("MaxZmeas", "Max. measured Z value", _maxZmeas, double(600.));

        registerProcessorParameter("SigmaZ", "Resolution in Z (for clustering)", _sigmaZ, double(3.0));

        registerProcessorParameter("Xoffset", "Offset in x of (local) coordinate system", _xOffset, double(0.));

        registerProcessorParameter("Yoffset", "Offset in y of (local) coordinate system", _yOffset, double(1500.));

        registerProcessorParameter("MaxMult", "Max. multiplicity for road seeding bins", _maxMult, int(3));

        registerProcessorParameter("MinPixels", "Min. number of pixels for road seeding macro pixel", _minPixels, int(3));

        registerOptionalParameter("MinDist", "Min. distance for road seeding bins", _minDist, int(2));

        registerOptionalParameter("MaxPull2XY", "Max. pull squared to road in XY", _maxPull2XY, double(15.));

        registerOptionalParameter("MaxPull2Z", "Max. pull squared to road in Z", _maxPull2Z, double(15.));

        registerOptionalParameter("MaxChi2", "Max. Chi2/Ndf for road", _maxChi2, double(5.));

        registerOptionalParameter("MaxSvar", "Max. relative arc-length variance for road", _maxSvar, double(0.15));

        registerOptionalParameter("MaxGap", "Max. (arc-length) gap for road", _maxGap, double(4.));

        registerOptionalParameter("OctoboardOffset", "Octoboard number offset (in module=(octo+offset)/scale)", _octoOffset, int(8));

        registerOptionalParameter("OctoboardScale", "Octoboard number scale (in module=(octo+offset)/scale)", _octoScale, int(12));

        registerOptionalParameter("Chi2CutChip", "Max. Chi2/Ndf for chip segment matching", _chi2CutChip, double(30.));

        registerOptionalParameter("Chi2CutOcto", "Max. Chi2/Ndf for octoboard segment matching", _chi2CutOcto, double(30.));

        registerOptionalParameter("Chi2CutMod", "Max. Chi2/Ndf for module segment matching", _chi2CutMod, double(50.));

        registerOptionalParameter("DistCut", "Max. relative (to track length sum) distance (of centers) for segment matching", _distCut,
                        double(2.));

        registerOptionalParameter("ReferencePointAtPca", "Use PCA as reference point, else 1. hit", _refAtPCA, bool(false));

}

void TimePixLocalRoadSearchProcessor::init() {
        streamlog_out(DEBUG) << " init() called  " << endl;
}

void TimePixLocalRoadSearchProcessor::processRunHeader(LCRunHeader* run) {
        // do something for each run
}

void TimePixLocalRoadSearchProcessor::processEvent(LCEvent* evt) {
        std::clock_t start1 = std::clock();

        streamlog_out(DEBUG) << "Event Number: " << evt->getEventNumber() << endl;

        //get input collection
        LCCollection* inputCol = 0;

        LCCollectionVec* outputCol = new LCCollectionVec(LCIO::TRACK);
        LCFlagImpl trkFlag(0);
        trkFlag.setBit(LCIO::TRBIT_HITS);
        outputCol->setFlag(trkFlag.getFlag());

        // do a try / catch in case there is an empty event
        try {
                inputCol = evt->getCollection(_inputColName);
        } catch (DataNotAvailableException &e) {
                // no input data, just return and process the next event
                return;
        }

        Vector3D theTPCCenter(_Xcenter, _Ycenter, 0.);
        Vector3D bfield = marlin::Global::GEAR->getBField().at(theTPCCenter);

        const bool bOff(bfield.z() * _bfieldScaleFactor == 0.);
        if (bOff) {
                streamlog_out(DEBUG) << "BField(GEAR) is OFF" << std::endl;
        } else {
                streamlog_out(DEBUG) << "BField(GEAR) is ON" << std::endl;
        }
        // constant magnetic field (in z direction)
        _Bzc = bfield.z() * _bfieldScaleFactor * 0.0002998; // Bz*c

        int iCuts[] = { _maxMult, _minPixels, _minDist };
        double dCuts[] = { _maxPull2XY, _maxPull2Z, _maxChi2, _maxSvar, _maxGap };

        chipPixelMapType pixelData;

        int nInput = inputCol->getNumberOfElements();

        /*std::map<int,int> chipMap;
         for (int iInput = 0; iInput < nInput; iInput++) {
         TrackerHit* Hit = dynamic_cast<TrackerHit*>(inputCol->getElementAt(iInput));
         int chip = Hit->getCellID1();
         chipMap[chip]++;
         }
         for (std::map<int,int>::iterator itChip = chipMap.begin(); itChip != chipMap.end(); ++itChip) {
         //std::cout << " chip Map " << itChip->first << " " << itChip->second << std::endl;
         //pixelData[itChip->first].reserve(itChip->second);
         } */

        int nAccepted = 0;
        for (int iInput = 0; iInput < nInput; iInput++) {
                TrackerHit* Hit = dynamic_cast<TrackerHit*>(inputCol->getElementAt(iInput));
                int chip = Hit->getCellID1();
                double zMeas = Hit->getPosition()[2];
                if (zMeas > _minZmeas && zMeas < _maxZmeas) {
                        nAccepted++;
                        tp_Pixel* somePixel = new tp_Pixel(iInput, *Hit, _xOffset, _yOffset);
                        //somePixel->print();
                        pixelData[chip].push_back(somePixel);
                }
        }

        streamlog_out(DEBUG) << "Event " << evt->getEventNumber() << " in run " << evt->getRunNumber() << ", number of input hits: "
                        << nInput << ", accepted (in Z): " << nAccepted << endl;

        pixelChipListType pixelChips;
        for (chipPixelMapType::iterator itChip = pixelData.begin(); itChip != pixelData.end(); ++itChip) {
                tp_PixelChip* someChip = new tp_PixelChip(itChip->first, itChip->second, _mpSize, _sigmaZ);
                //someChip->print();
                if (someChip->getNumBins() > 1)
                        pixelChips.push_back(someChip);
                else
                        delete someChip;
        }

        int numSeg = 0;
        for (pixelChipListType::iterator itChip = pixelChips.begin(); itChip != pixelChips.end(); ++itChip) {
                //(*itChip)->print();
                numSeg += (*itChip)->findSegments(_Bzc, iCuts, dCuts);
        }
        streamlog_out(DEBUG) << "  found " << pixelChips.size() << " TimePix chips with " << numSeg << " segments " << endl;
        tp_PixelSegmentCombiner segmentCombiner(_Bzc);
        pixelSegmentListMapType chipSegmentMap, octoSegmentMap;
        pixelSegmentListType modSegmentList;
        for (pixelChipListType::iterator itChip = pixelChips.begin(); itChip != pixelChips.end(); ++itChip) {
                const int iOcto = (*itChip)->getId() / 8;
                pixelSegmentListType segments = (*itChip)->getPixelSegments();
                if (segments.size() > 0) chipSegmentMap[iOcto].insert(chipSegmentMap[iOcto].end(), segments.begin(), segments.end());
                //for (pixelSegmentListType::iterator itSeg = segments.begin(); itSeg != segments.end(); ++itSeg) (*itSeg)->print();
        }
        // combine chips on octoboards
        for (pixelSegmentListMapType::iterator itOcto = chipSegmentMap.begin(); itOcto != chipSegmentMap.end(); ++itOcto) {
                const int iOcto = itOcto->first;
                // std::cout << " combine octoboard " << iOcto << " " << itOcto->second.size() << std::endl;
                pixelSegmentListType combinedSegments = segmentCombiner.run(1, iOcto, itOcto->second, _chi2CutChip, _distCut);
                // \todo variable octoboard to module mapping
                const int iMod = (iOcto + _octoOffset) / _octoScale;
                octoSegmentMap[iMod].insert(octoSegmentMap[iMod].end(), combinedSegments.begin(), combinedSegments.end());
        }
        // combine octoboards per module
        for (pixelSegmentListMapType::iterator itMod = octoSegmentMap.begin(); itMod != octoSegmentMap.end(); ++itMod) {
                const int iMod = itMod->first;
                // std::cout << " combine module " << iMod << " " << itMod->second.size() << std::endl;
                pixelSegmentListType combinedSegments = segmentCombiner.run(2, iMod, itMod->second, _chi2CutOcto, _distCut);
                modSegmentList.insert(modSegmentList.end(), combinedSegments.begin(), combinedSegments.end());
        }
        // combine modules
        // std::cout << " combine modules " << modSegmentList.size() << std::endl;
        pixelSegmentListType trackCandidates = segmentCombiner.run(3, 0, modSegmentList, _chi2CutMod, _distCut);
        // sort by length
        sort(trackCandidates.begin(), trackCandidates.end(), compareSegments);
        // std::cout << " === track condidates ===" << std::endl;
        // for (pixelSegmentListType::iterator itSeg = trackCandidates.begin(); itSeg != trackCandidates.end(); ++itSeg) (*itSeg)->print();

        streamlog_out(DEBUG) << "  found " << trackCandidates.size() << " track candidates " << endl;

        for (pixelSegmentListType::iterator itSeg = trackCandidates.begin(); itSeg != trackCandidates.end(); ++itSeg) {
                if ((*itSeg)->getLevel() < _minTrackLevel) continue;
                // new track
                TrackImpl* theTrack = new TrackImpl();
                // get hits/pixels
                pixelListType allPixels;
                macroPixelListType macroPixels = (*itSeg)->getMacroPixels();
                for (macroPixelListType::iterator itMpix = macroPixels.begin(); itMpix != macroPixels.end(); ++itMpix) {
                        pixelListType pixels = (*itMpix)->getPixels();
                        allPixels.insert(allPixels.end(), pixels.begin(), pixels.end());
                }
                // sort by arc length
                sort(allPixels.begin(), allPixels.end(), comparePixelsS);
                // position of 1. hit
                double posFirst[3];
                allPixels.front()->getPos(posFirst);
                // reference point, 1. hit or PCA (point of closest approach (to origin))
                double refPos[] = { 0., 0., 0. };
                if (!_refAtPCA) {
                        refPos[0] = posFirst[0];
                        refPos[1] = posFirst[1];
                        refPos[2] = posFirst[2];
                }
                float refPoint[3] = { refPos[0] - _xOffset, refPos[1] - _yOffset, refPos[2] };
                // track parameters and covariance matrix
                TVectorD parameters(5);
                TMatrixDSym covariance(5);
                (*itSeg)->getLCIOStateAtRefPoint(refPos, parameters, covariance); // ( d0, phi0, Omega, z0, tanLambda)

                theTrack->setOmega(parameters[2]);
                theTrack->setTanLambda(parameters[4]);
                theTrack->setPhi(parameters[1]);
                theTrack->setD0(parameters[0]);
                theTrack->setZ0(parameters[3]);
                theTrack->setChi2((*itSeg)->getChi2());
                theTrack->setNdf((*itSeg)->getNdf());
                theTrack->setReferencePoint(refPoint);
                /* prepare LCIO output; unknown parts:
                 *
                 * theTrack->setTypeBit (int index, bool val=true) // ?
                 * theTrack->setdEdx (float dEdx)
                 * theTrack->setdEdxError (float dEdxError) */
                float rInner = sqrt(pow(posFirst[0], 2) + pow(posFirst[1], 2));
                theTrack->setRadiusOfInnermostHit(rInner);
                // compressed covariance matrix
                float compressedCovariance[15];
                int ind = 0;
                for (int i = 0; i < 5; ++i)
                        for (int j = 0; j <= i; ++j)
                                compressedCovariance[ind++] = covariance[i][j];
                theTrack->setCovMatrix(compressedCovariance);
                // and finally add the hits to the track
                for (pixelListType::iterator itP = allPixels.begin(); itP != allPixels.end(); ++itP) {
                        TrackerHit* Hit = dynamic_cast<TrackerHit*>(inputCol->getElementAt((*itP)->getHitNum()));
                        theTrack->addHit(Hit);
                }

                streamlog_out(DEBUG) << " +++ the found track has a chi^2 value of " << theTrack->getChi2() << " for ndf = "
                                << theTrack->getNdf() << " and the parameters [omega, tanLambda, phi0, d0, z0] = [" << theTrack->getOmega() << ","
                                << theTrack->getTanLambda() << "," << theTrack->getPhi() << "," << theTrack->getD0() << "," << theTrack->getZ0()
                                << "] at the reference position [" << refPoint[0] << "," << refPoint[1] << "," << refPoint[2] << "]." << endl;

                outputCol->addElement(theTrack);
        }

        streamlog_out(DEBUG) << "TimePixLocalRoadSearch ... done!" << endl;

        if (!outputCol->empty()) {
                evt->addCollection(outputCol, _outputColName);
        }

        streamlog_out(DEBUG) << ((std::clock() - start1) / (double) CLOCKS_PER_SEC) << " sec" << endl;

        // cleanup
        for (pixelSegmentListType::iterator itSeg = trackCandidates.begin(); itSeg != trackCandidates.end(); ++itSeg) {
                delete *itSeg;
        }
        trackCandidates.clear();

        for (pixelChipListType::iterator itChip = pixelChips.begin(); itChip != pixelChips.end(); ++itChip) {
                delete *itChip;
        }
        pixelChips.clear();

        for (chipPixelMapType::iterator itChip = pixelData.begin(); itChip != pixelData.end(); ++itChip) {
                for (pixelListType::iterator itP = itChip->second.begin(); itP != itChip->second.end(); ++itP) {
                        delete *itP;
                }
        }
        pixelData.clear();

}

void TimePixLocalRoadSearchProcessor::check(LCEvent* evt) {
        // check what is needed here
}

void TimePixLocalRoadSearchProcessor::end() {
        // do something at the end of the processing, e.g. cleaning up

}


tp_Pixel::tp_Pixel(const int iHit, const TrackerHit& aHit, const double xOffset, const double yOffset) :
                _hitNum(iHit), _chipNum(aHit.getCellID1()), _rowNum(aHit.getCellID0() / 256), _colNum(aHit.getCellID0() % 256), _posX(
                                aHit.getPosition()[0] + xOffset), _posY(aHit.getPosition()[1] + yOffset), _posZ(aHit.getPosition()[2]), _sigma2XY(
                                0.5 * (aHit.getCovMatrix()[0] + aHit.getCovMatrix()[2])), _sigma2Z(aHit.getCovMatrix()[5]), _used(false), _sarc(0.) {
}

void tp_Pixel::print() const {
        std::cout << " tp_Pixel " << _hitNum << ", " << _chipNum << ", " << _rowNum << ", " << _colNum << ", " << _posX << ", " << _posY
                        << ", " << _posZ << "; " << _sigma2XY << ", " << _sigma2Z << std::endl;
}

int tp_Pixel::getHitNum() const {
        return _hitNum;
}

int tp_Pixel::getChip() const {
        return _chipNum;
}

int tp_Pixel::getRow() const {
        return _rowNum;
}

int tp_Pixel::getCol() const {
        return _colNum;
}

double tp_Pixel::getX() const {
        return _posX;
}

double tp_Pixel::getY() const {
        return _posY;
}

double tp_Pixel::getZ() const {
        return _posZ;
}


void tp_Pixel::getPos(double* position) const {
        position[0] = _posX;
        position[1] = _posY;
        position[2] = _posZ;
}

double tp_Pixel::getSigma2XY() const {
        return _sigma2XY;
}

double tp_Pixel::getSigma2Z() const {
        return _sigma2Z;
}

bool tp_Pixel::getUsed() const {
        return _used;
}

double tp_Pixel::getArcLength() const {
        return _sarc;
}

void tp_Pixel::setUsed() {
        _used = true;
}

void tp_Pixel::setArcLength(const double sArc) {
        _sarc = sArc;
}


tp_MacroPixel::tp_MacroPixel(const int mpId, pixelListType& pixelList, const int mpSize) :
                _mpId(mpId), _size(mpSize), _entries(pixelList.size()), _pixels(pixelList), _used(false) {
        // analyze cluster, avaerge over pixels
        int sr, sc;
        sr = sc = 0;
        double sx, sy, sz, sxx, sxy, syy, szz, srxy, srz;
        sx = sy = sz = sxx = sxy = syy = szz = srxy = srz = 0.;
        double pos[3];
        for (pixelListType::iterator itP = _pixels.begin(); itP != _pixels.end(); ++itP) {
                int row = (*itP)->getRow();
                int col = (*itP)->getCol();
                sr += row;
                sc += col;
                (*itP)->getPos(pos);
                sx += pos[0];
                sy += pos[1];
                sz += pos[2];
                sxx += pos[0] * pos[0];
                sxy += pos[0] * pos[1];
                syy += pos[1] * pos[1];
                szz += pos[2] * pos[2];
                srxy += (*itP)->getSigma2XY();
                srz += (*itP)->getSigma2Z();
        }
        // local position
        _locRow = sr / _entries;
        _locCol = sc / _entries;
        // (global) position
        _posX = sx / _entries;
        _posY = sy / _entries;
        _posZ = sz / _entries;
        // shape (variance)
        _varXX = sxx / _entries - _posX * _posX;
        _varXY = sxy / _entries - _posX * _posY;
        _varYY = syy / _entries - _posY * _posY;
        _varZZ = szz / _entries - _posZ * _posZ;
        // resolution
        _sigma2XY = srxy / _entries;
        _sigma2Z = srz / _entries;
}

tp_MacroPixel::~tp_MacroPixel() {
        //std::cout << " destroying tp_MacroPixel " << _mpId << std::endl; 
        _pixels.clear();
}

void tp_MacroPixel::print() const {
        std::cout << " tp_MacroPixel " << _mpId << ", " << _entries << ", " << _locRow << ", " << _locCol << ", " << _posX << ", "
                        << _posY << ", " << _posZ << "; " << _varXX << ", " << _varXY << ", " << _varYY << ", " << _varZZ << std::endl;
}

int tp_MacroPixel::getId() const {
        return _mpId;
}

int tp_MacroPixel::getSize() const {
        return _size;
}

int tp_MacroPixel::getEntries() const {
        return _entries;
}

int tp_MacroPixel::getRow() const {
        return _locRow;
}

int tp_MacroPixel::getCol() const {
        return _locCol;
}

double tp_MacroPixel::getSigma2XY() const {
        return _sigma2XY;
}

double tp_MacroPixel::getSigma2Z() const {
        return _sigma2Z;
}

double tp_MacroPixel::getX() const {
        return _posX;
}

double tp_MacroPixel::getY() const {
        return _posY;
}


void tp_MacroPixel::getPos(double* position) const {
        position[0] = _posX;
        position[1] = _posY;
        position[2] = _posZ;
}

double tp_MacroPixel::getVarXY(const double ex, const double ey) const {
        return ex * ex * _varXX + 2. * ex * ey * _varXY + ey * ey * _varYY;
}

double tp_MacroPixel::getVarZ() const {
        return _varZZ;
}

bool tp_MacroPixel::getUsed() const {
        return _used;
}

void tp_MacroPixel::setUsed() {
        _used = true;
}

pixelListType tp_MacroPixel::getPixels() const {
        return _pixels;
}


tp_PixelBin::tp_PixelBin(const int iBin, const int binId, pixelListType& pixelList, const int mpSize, const double zCoreWidth,
                const double zRoadWidth) :
                _binId(binId), _size(mpSize), _numPix(0), _macroPixels() {
        // check for cluster (core) in Z (using sliding window)
        int numMp = 0;
        int ntot = pixelList.size();
        //std:: cout << " ntot " << ntot << " " << binId <<std::endl;
        pixelListType::iterator itFirst = pixelList.begin();
        pixelListType::iterator itLast = pixelList.end();
        int ntry = 0;

        while (ntot > 1) {
                ntry++;
                int bestNumPix = 0;
                double bestZPos = 0.;
                pixelListType::iterator itStart = itFirst;
                double zStart = (*itStart)->getZ();
                pixelListType::iterator itBest = itStart;
                int numPix = 1;
                double sumZ = zStart;
                // sliding window
                pixelListType::iterator itP = itStart;
                while (++itP < itLast) {
                        if ((*itP)->getUsed()) continue;
                        // unused
                        double z = (*itP)->getZ();
                        if (z - zStart > 2. * zCoreWidth) {
                                if (numPix > bestNumPix) {
                                        itBest = itStart;
                                        bestNumPix = numPix;
                                        bestZPos = sumZ / numPix;
                                }
                                // new window start
                                while (++itStart < itLast) {
                                        if ((*itStart)->getUsed()) continue;
                                        // unused
                                        numPix--;
                                        sumZ -= zStart;
                                        zStart = (*itStart)->getZ();
                                        if (z - zStart < 2. * zCoreWidth) break;
                                }
                        }
                        numPix++;
                        sumZ += z;
                }
                if (numPix > bestNumPix) {
                        itBest = itStart;
                        bestNumPix = numPix;
                        bestZPos = sumZ / numPix;
                }
                //std::cout << " zbest1 " << itBest-pixelList.begin() << " " << itFirst-pixelList.begin() << " " << bestNumPix << " " << bestZPos << std::endl;
                // too few?
                if (bestNumPix < 2) break;

                pixelListType pixels;
                // check for noisy row or column
                int firstRow = -1;
                int firstCol = -1;
                bool singleRow = true;
                bool singleCol = true;
                for (pixelListType::iterator itP = itFirst; itP != itLast; ++itP) {
                        if ((*itP)->getUsed()) continue;
                        if (abs((*itP)->getZ() - bestZPos) < zRoadWidth) {
                                pixels.push_back(*itP);
                                (*itP)->setUsed();
                                if (firstRow < 0) {
                                        firstRow = (*itP)->getRow();
                                        firstCol = (*itP)->getCol();
                                        itBest = itP;
                                } else {
                                        singleRow = singleRow && (firstRow == (*itP)->getRow());
                                        singleCol = singleCol && (firstCol == (*itP)->getCol());
                                }
                        }
                }
                bestNumPix = pixels.size();
                //std::cout << " zbest2 " << itBest-pixelList.begin() << " " << itFirst-pixelList.begin() << " " << bestNumPix << " " << bestZPos << std::endl;
                // new macro pixel (minimal noise cut >1)
                if (bestNumPix > 1 && !singleRow && !singleCol) {
                        tp_MacroPixel* someMacroPixel = new tp_MacroPixel(iBin * 1000 + numMp, pixels, mpSize);
                        //someMacroPixel->print();
                        _macroPixels.push_back(someMacroPixel);
                        _numPix += pixels.size();
                        numMp += 1;
                }
                ntot -= bestNumPix;
                // road at front?
                if (itBest == itFirst) itFirst += bestNumPix;
                // road at back?
                if (itBest + bestNumPix == itLast) itLast -= bestNumPix;
        }
}

tp_PixelBin::~tp_PixelBin() {
        //std::cout << " destroying tp_PixelBin " << _binId << std::endl; 
        for (macroPixelListType::iterator itMpix = _macroPixels.begin(); itMpix != _macroPixels.end(); ++itMpix) {
                delete *itMpix;
        }
        _macroPixels.clear();
}

void tp_PixelBin::print() const {
        std::cout << " tp_PixelBin " << _binId << ", " << _numPix << ", " << _macroPixels.size() << std::endl;
}

int tp_PixelBin::getId() const {
        return _binId;
}

int tp_PixelBin::getNumPix() const {
        return _numPix;
}

int tp_PixelBin::getNumMPix() const {
        return _macroPixels.size();
}

macroPixelListType tp_PixelBin::getMacroPixels() const {
        return _macroPixels;
}


tp_PixelRoad::tp_PixelRoad(macroPixelPairType mPixPair, pixelBinListType& pixelBins, const double* cuts) :
                _mpId1(mPixPair.first->getId()), _mpId2(mPixPair.second->getId()), _valid(false), _numPix(0), _chi2ByNdf(0.), _sList(), _macroPixels() {
        // get positions
        double pos1[3], pos2[3];
        mPixPair.first->getPos(pos1); // reference point
        mPixPair.second->getPos(pos2);
        // direction in XY
        const double distX = pos2[0] - pos1[0];
        const double distY = pos2[1] - pos1[1];
        const double dist2D = sqrt(distX * distX + distY * distY);
        const double ex = distX / dist2D;
        const double ey = distY / dist2D;
        // slope in (S,Z)
        const double distZ = pos2[2] - pos1[2];
        const double dzds = distZ / dist2D;
        // check unused macro pixels
        const double maxPull2XY = cuts[0];
        const double maxPull2Z = cuts[1];
        const double maxChi2 = cuts[2];
        const double maxSvar = cuts[3];
        const double maxGap = cuts[4];
        // loop over bins and unused macro pixels
        double sumChi2 = 0.;
        for (pixelBinListType::iterator itBin = pixelBins.begin(); itBin != pixelBins.end(); ++itBin) {
                macroPixelListType mPixList = (*itBin)->getMacroPixels();
                for (macroPixelListType::iterator itMpix = mPixList.begin(); itMpix != mPixList.end(); ++itMpix) {
                        if ((*itMpix)->getUsed()) continue;
                        //position matching
                        (*itMpix)->getPos(pos2);
                        const double resXY = (pos2[0] - pos1[0]) * ey - (pos2[1] - pos1[1]) * ex;
                        const double sArc = (pos2[0] - pos1[0]) * ex + (pos2[1] - pos1[1]) * ey;
                        const double resZ = pos2[2] - (pos1[2] + sArc * dzds);
                        // std::cout << " res " << resXY << " " << resZ << " " << dist2D << " " << sArc << " " << _sumChi2 << std::endl;
                        // include macro pixel variance
                        const double pull2XY = (resXY * resXY + (*itMpix)->getVarXY(ey, -ex)) / (*itMpix)->getSigma2XY();
                        const double pull2Z = (resZ * resZ + (*itMpix)->getVarZ()) / (*itMpix)->getSigma2Z();
                        if (pull2XY > maxPull2XY || pull2Z > maxPull2Z) continue;
                        // accept macro pixel
                        const int nPix = (*itMpix)->getEntries();
                        _numPix += nPix;
                        _sList.push_back(sArc);
                        _macroPixels.push_back(*itMpix);
                        // Chi2
                        sumChi2 += nPix * (pull2XY + pull2Z);
                }
        }
        //std::cout << " (m)pix " << _numPix << " " << _macroPixels.size() << " " << sumChi2 << std::endl;
        // minimal content
        if (_macroPixels.size() < 3) return;
        // normalize to Ndf
        _chi2ByNdf = sumChi2 / (2 * _numPix - 4);
        if (_chi2ByNdf > maxChi2) return;
        // rel. variance in arc length, max. gap
        std::sort(_sList.begin(), _sList.end());
        const double sLen = _sList.back() - _sList.front();
        double sumS0 = 0.;
        double sumS1 = 0.;
        double sumS2 = 0.;
        double mxGap = 0.;
        for (std::vector<double>::iterator itS = _sList.begin(); itS != _sList.end(); ++itS) {
                const double sArc = *itS;
                sumS0 += 1.0;
                sumS1 += sArc;
                sumS2 += sArc * sArc;
                if (itS != _sList.begin()) {
                        const double gap = sArc - *(itS - 1);
                        mxGap = max(mxGap, gap);
                }
        }
        sumS1 /= sumS0;
        sumS2 /= sumS0;
        const double sVar = (sumS2 - sumS1 * sumS1) / (sLen * sLen);
        //std::cout << " sVar " << sVar << " " << sLen << " " << mxGap << std::endl;
        if (sVar > maxSvar || mxGap > maxGap) return;
        // valid road
        _valid = true;
}

tp_PixelRoad::~tp_PixelRoad() {
        //std::cout << " destroying tp_PixelRoad " << _binId << std::endl; 
        _macroPixels.clear();
}

void tp_PixelRoad::print() const {
        std::cout << " tp_PixelRoad " << _mpId1 << " " << _mpId2 << ": " << _valid << ", " << _numPix << ", " << _macroPixels.size()
                        << ", " << _chi2ByNdf << std::endl;
}

bool tp_PixelRoad::isValid() const {
        return _valid;
}

int tp_PixelRoad::getNumPix() const {
        return _numPix;
}

int tp_PixelRoad::getNumMPix() const {
        return _macroPixels.size();
}

double tp_PixelRoad::getChi2() const {
        return _chi2ByNdf;
}

macroPixelListType tp_PixelRoad::getMacroPixels() const {
        return _macroPixels;
}


tp_PixelSegment::tp_PixelSegment(const int segId, const int level, const double Bzc, macroPixelListType& macroPixels) :
                _segId(segId), _level(level), _bzc(Bzc), _npar((Bzc != 0.) ? 5 : 4), _macroPixels(macroPixels), _parameters(_npar), _covariance(
                                _npar) {
        // reference point
        double sumX = 0.;
        double sumY = 0.;
        for (macroPixelListType::iterator itMpix = macroPixels.begin(); itMpix != macroPixels.end(); ++itMpix) {
                sumX += (*itMpix)->getX();
                sumY += (*itMpix)->getY();
        }
        _refX = sumX / macroPixels.size();
        _refY = sumY / macroPixels.size();
        _refZ = 0.;
        double refPoint[] = { _refX, _refY, _refZ };
        //
        // fit segment
        //
        double pos[3], sArc, chi2XY, chi2ZS;
        double parameters[] = { 0., 0., 0., 0., 0. };
        int nParXY, nPointXY, nPointZS;
        // xy fit
        simpleFitXY xyFit((Bzc != 0.), _refX, _refY);
        for (macroPixelListType::iterator itMpix = macroPixels.begin(); itMpix != macroPixels.end(); ++itMpix) {
                pixelListType pixels = (*itMpix)->getPixels();
                for (pixelListType::iterator itP = pixels.begin(); itP != pixels.end(); ++itP) {
                        xyFit.addPoint((*itP)->getX(), (*itP)->getY(), 1.0 / (*itP)->getSigma2XY());
                }
        }
        nParXY = xyFit.fit(chi2XY, nPointXY);
        _parameters.SetSub(0, xyFit.getPar());
        _covariance.SetSub(0, 0, xyFit.getCov());
        for (int i = 0; i < nParXY; ++i)
                parameters[3 - nParXY + i] = _parameters[i];
        // simple helix
        simpleHelix hlx(parameters, refPoint, Bzc);
        // zs fit 
        double sMin = 1.;
        double sMax = 0.;
        simpleFitZS zsFit;
        for (macroPixelListType::iterator itMpix = macroPixels.begin(); itMpix != macroPixels.end(); ++itMpix) {
                pixelListType pixels = (*itMpix)->getPixels();
                for (pixelListType::iterator itP = pixels.begin(); itP != pixels.end(); ++itP) {
                        (*itP)->getPos(pos);
                        sArc = hlx.getArcLengthXY(pos);
                        (*itP)->setArcLength(sArc);
                        zsFit.addPoint(sArc, pos[2] - _refZ, 1.0 / (*itP)->getSigma2Z());
                        if (sMin > sMax) {
                                sMin = sMax = sArc;
                        } else {
                                sMin = min(sMin, sArc);
                                sMax = max(sMax, sArc);
                        }
                }
        }
        _length = sMax - sMin;
        zsFit.fit(chi2ZS, nPointZS);
        _parameters.SetSub(nParXY, zsFit.getPar());
        _covariance.SetSub(nParXY, nParXY, zsFit.getCov());
        // combine
        _ndf = nPointXY + nPointZS - _npar;
        _chi2 = chi2XY + chi2ZS;
        // cout << " cov " << endl; _covariance.Print();
}

tp_PixelSegment::~tp_PixelSegment() {
        //std::cout << " destroying tp_PixelSegment " << _segId << std::endl; 
        _macroPixels.clear();
}

void tp_PixelSegment::print() const {
        std::cout << " tp_PixelSegment " << _segId << " " << _level << ": " << _macroPixels.size() << " (" << _refX << ", " << _refY
                        << ") " << _length << ", " << _npar << ", " << _ndf << ", " << _chi2 << "; ";
        for (int i = 0; i < _npar; ++i)
                std::cout << _parameters[i] << " ";
        std::cout << std::endl;
        //_parameters.Print();
        //_covariance.Print();
}

double tp_PixelSegment::getLength() const {
        return _length;
}

double tp_PixelSegment::getChi2() const {
        return _chi2;
}

int tp_PixelSegment::getNdf() const {
        return _ndf;
}

int tp_PixelSegment::getNumPar() const {
        return _npar;
}

int tp_PixelSegment::getLevel() const {
        return _level;
}


void tp_PixelSegment::getRefPoint(double* position) const {
        position[0] = _refX;
        position[1] = _refY;
        position[2] = _refZ;
}


void tp_PixelSegment::getPar(double* par) const {
        par[0] = 0.;
        for (int i = 0; i < _npar; ++i)
                par[5 - _npar + i] = _parameters[i];
}

const TVectorD& tp_PixelSegment::getPar() const {
        return _parameters;
}

const TMatrixDSym& tp_PixelSegment::getCov() const {
        return _covariance;
}

macroPixelListType tp_PixelSegment::getMacroPixels() const {
        return _macroPixels;
}


void tp_PixelSegment::getLCIOStateAtRefPoint(const double* refPos, TVectorD& newPar, TMatrixDSym& newCov) const {
        double refPoint[3], par[5];
        this->getRefPoint(refPoint);
        this->getPar(par);
        simpleHelix hlx(par, refPoint, _bzc);
        TVectorD tmpPar(_npar);
        TMatrixDSym cov(this->getCov()), tmpCov(_npar);
        // track state at reference point
        hlx.getStateAt(refPos[0], refPos[1], refPos[2], cov, tmpPar, tmpCov);
        TMatrixD hlx2LCIO = hlx.helixToLCIOJacobian().GetSub(0, 4, 5 - _npar, 4);
        newPar = hlx2LCIO * tmpPar;
        newCov = tmpCov.Similarity(hlx2LCIO);
}

// Match segment with other segment.
bool tp_PixelSegment::match(tp_PixelSegment* seg, const double chi2Cut, const double distCut) const {
        // check distance
        double refPoint1[3], refPoint2[3];
        this->getRefPoint(refPoint1);
        seg->getRefPoint(refPoint2);
        const double combinedLength = this->getLength() + seg->getLength();
        const double distX = refPoint2[0] - refPoint1[0];
        const double distY = refPoint2[1] - refPoint1[1];
        const double distZ = refPoint2[2] - refPoint1[2];
        // relative distance
        const double dist2D = sqrt(distX * distX + distY * distY) / combinedLength;
        // std::cout << " match " << dist2D << " " << combinedLength << std::endl;
        // minimal distance (avoid too much overlap)
        if (dist2D < 0.25) return false;
        // maximal distance (avoid too much extrapolation)
        if (dist2D > distCut) return false;
        // compare segment parameters at mid point
        double par1[5], par2[5];
        this->getPar(par1);
        simpleHelix hlx1(par1, refPoint1, _bzc);
        seg->getPar(par2);
        simpleHelix hlx2(par2, refPoint2, _bzc);
        // (weighted) mid point
        const double fraction = this->getLength() / combinedLength;
        const double xav = refPoint1[0] + fraction * distX;
        const double yav = refPoint1[1] + fraction * distY;
        const double zav = refPoint1[2] + fraction * distZ;
        TVectorD newPar1(_npar), newPar2(_npar);
        TMatrixDSym cov1(this->getCov()), cov2(seg->getCov()), newCov1(_npar), newCov2(_npar);
        // track states at midpoint
        hlx1.getStateAt(xav, yav, zav, cov1, newPar1, newCov1);
        hlx2.getStateAt(xav, yav, zav, cov2, newPar2, newCov2);
        TVectorD diffPar(newPar1 - newPar2);
        TMatrixDSym diffPrec(newCov1 + newCov2);
        diffPrec.Invert();
        // chi2
        double chi2 = diffPrec.Similarity(diffPar);
        // cout << " seg-match chi2 " << chi2 << " " << _npar << " " << dist2D << " " << this->getLength() << " " << seg->getLength() << " " << this->getLevel() << " " << seg->getLevel() << endl;
        return (chi2 < chi2Cut * _npar);
}


tp_PixelChip::tp_PixelChip(const int chipId, pixelListType& pixelList, const int mpSize, const double sigmaZ) :
                _chipId(chipId), _size(mpSize), _numPix(0), _numMPix(0), _pixelBins(), _pixelSegments() {
        // bin pixels into 2D histogram
        //std::cout << " >>> chip " << chipId << std::endl;
        std::map<int, pixelListType> pixHist;
        const int nBin = 256 / _size;
        const int nBorder = (256 % _size) / 2;

        for (pixelListType::iterator itP = pixelList.begin(); itP != pixelList.end(); ++itP) {
                int macroRow = min(max(((*itP)->getRow() - nBorder) / _size, 0), nBin - 1);
                int macroCol = min(max(((*itP)->getCol() - nBorder) / _size, 0), nBin - 1);
                int iBin = macroRow + 1000 * macroCol;
                pixHist[iBin].push_back(*itP);
        }

        // define macro pixels (needs at least 2 pixels)
        int numBin = 0;
        for (std::map<int, pixelListType>::iterator itBin = pixHist.begin(); itBin != pixHist.end(); ++itBin) {
                int iBin = itBin->first;
                pixelListType pixels = itBin->second;
                // minimal noise cut
                if (pixels.size() < 2) continue;
                // create bin with macro pixels separated in Z
                sort(pixels.begin(), pixels.end(), comparePixelsZ);
                tp_PixelBin* someBin = new tp_PixelBin(numBin, iBin, pixels, mpSize, 2.0 * sigmaZ, 3.5 * sigmaZ);
                //someBin->print();
                if (someBin->getNumMPix() > 0) {
                        _pixelBins.push_back(someBin);
                        _numMPix += someBin->getNumMPix();
                        _numPix += someBin->getNumPix();
                        numBin += 1;
                } else
                        delete someBin;
        }
}

tp_PixelChip::~tp_PixelChip() {
        //std::cout << " destroying tp_PixelChip " << _chipId << std::endl;
        for (pixelBinListType::iterator itBin = _pixelBins.begin(); itBin != _pixelBins.end(); ++itBin) {
                delete *itBin;
        }
        _pixelBins.clear();
        // segments are partially deleted by combiner
        _pixelSegments.clear();
}

void tp_PixelChip::print() const {
        std::cout << " tp_PixelChip " << _chipId << ", " << _numPix << ", " << _numMPix << ", " << _pixelBins.size() << std::endl;
}

int tp_PixelChip::getId() const {
        return _chipId;
}

int tp_PixelChip::getNumPix() const {
        return _numPix;
}

int tp_PixelChip::getNumMPix() const {
        return _numMPix;
}

int tp_PixelChip::getNumBins() const {
        return _pixelBins.size();
}

// Get pixel segments.
pixelSegmentListType tp_PixelChip::getPixelSegments() const {
        return _pixelSegments;
}


int tp_PixelChip::findSegments(const double bfac, const int* iCuts, const double* dCuts) {
        // std::cout << " findSegments: "; this->print();
        int maxMult = iCuts[0];
        int minPixels = iCuts[1];
        int minDist = iCuts[2];
        // available macro pixels
        int freeMPix = _numMPix;
        while (freeMPix > 1) {
                macroPixelPairListMapType rankedPairs;
                // rank pairs according to distance (max(row, col))
                // bin 1
                for (pixelBinListType::iterator itBin1 = _pixelBins.begin(); itBin1 != _pixelBins.end(); ++itBin1) {
                        if ((*itBin1)->getNumMPix() > maxMult) continue;
                        // row and column
                        int row1 = (*itBin1)->getId() % 1000;
                        int col1 = (*itBin1)->getId() / 1000;
                        // macro pixels
                        macroPixelListType mPixList1 = (*itBin1)->getMacroPixels();
                        // bin 2
                        for (pixelBinListType::iterator itBin2 = itBin1 + 1; itBin2 != _pixelBins.end(); ++itBin2) {
                                if ((*itBin2)->getNumMPix() > maxMult) continue;
                                // row and column
                                int row2 = (*itBin2)->getId() % 1000;
                                int col2 = (*itBin2)->getId() / 1000;
                                // rank (distance)
                                int rank = max(abs(row1 - row2), abs(col1 - col2));
                                if (rank < minDist) continue;
                                // macro pixels
                                macroPixelListType mPixList2 = (*itBin2)->getMacroPixels();
                                // macro pixels from bin 1
                                for (macroPixelListType::iterator itMpix1 = mPixList1.begin(); itMpix1 != mPixList1.end(); ++itMpix1) {
                                        if ((*itMpix1)->getUsed() || (*itMpix1)->getEntries() < minPixels) continue;
                                        // macro pixels from bin 2
                                        for (macroPixelListType::iterator itMpix2 = mPixList2.begin(); itMpix2 != mPixList2.end(); ++itMpix2) {
                                                if ((*itMpix2)->getUsed() || (*itMpix2)->getEntries() < minPixels) continue;
                                                rankedPairs[rank].push_back(make_pair(*itMpix1, *itMpix2));
                                        }
                                }
                        }
                }
                //std::cout << " ranked pairs " << rankedPairs.size() << std::endl;
                tp_PixelRoad* bestRoad = NULL;
                int stopRank = 0;
                // road search with pairs
                for (macroPixelPairListMapType::reverse_iterator itPairs = rankedPairs.rbegin(); itPairs != rankedPairs.rend(); ++itPairs) {
                        int rank = itPairs->first;
                        macroPixelPairListType pairList = itPairs->second;
                        /* std::cout << " rank " << rank << " " << pairList.size();
                         for (macroPixelPairListType::iterator itPair = pairList.begin(); itPair != pairList.end(); ++itPair) {
                         std::cout << " (" << itPair->first->getId() << "," << itPair->second->getId() << "),";
                         }
                         std::cout << std::endl; */
                        // check roads
                        for (macroPixelPairListType::iterator itPair = pairList.begin(); itPair != pairList.end(); ++itPair) {
                                tp_PixelRoad* someRoad = new tp_PixelRoad(*itPair, _pixelBins, dCuts);
                                // someRoad->print();
                                if (someRoad->isValid()) {
                                        if (bestRoad != NULL) {
                                                // minimize variance 'chi2/#Mpix'
                                                if (someRoad->getChi2() / someRoad->getNumMPix() < bestRoad->getChi2() / bestRoad->getNumMPix()) {
                                                        delete bestRoad;
                                                        bestRoad = someRoad; // new best road
                                                } else
                                                        delete someRoad;
                                        } else
                                                bestRoad = someRoad; // first valid road
                                        stopRank = max(stopRank, rank - 1); // stop after next rank
                                        // all free macro pixels in this road?
                                        if (bestRoad->getNumMPix() == freeMPix) {
                                                stopRank = rank;
                                                break;
                                        } // stop road search
                                } else
                                        delete someRoad; // not valid
                        }
                        // stop rank loop?
                        if (rank <= stopRank) break;
                }
                // nothing found?
                if (bestRoad == NULL) break;
                // something found
                // std::cout << " best " << stopRank << " "; bestRoad->print();
                macroPixelListType macroPixels = bestRoad->getMacroPixels();
                // mark macro pixels as used
                for (macroPixelListType::iterator itMpix = macroPixels.begin(); itMpix != macroPixels.end(); ++itMpix)
                        (*itMpix)->setUsed();
                // create (straight line) segment
                tp_PixelSegment* someSegment = new tp_PixelSegment(_chipId, 0, bfac, macroPixels);
                //someSegment->print();
                _pixelSegments.push_back(someSegment);
                // remaining free macro pixels
                freeMPix -= bestRoad->getNumMPix();
                delete bestRoad;
        }
        return _pixelSegments.size();
}


tp_PixelSegmentCombiner::tp_PixelSegmentCombiner(const double Bzc) :
                _bzc(Bzc) {
}


pixelSegmentListType tp_PixelSegmentCombiner::run(const int level, const int newId, pixelSegmentListType& segments,
                const double chi2Cut, const double distCut) {
        // std::cout << " combine for " << level << " " << newId << std::endl;
        // combined segments
        pixelSegmentListType combinedSegments;
        // equivalence classes for segment matching
        simpleEquiClasses segmentEquiClass;
        // define indices
        for (pixelSegmentListType::iterator itSeg = segments.begin(); itSeg != segments.end(); ++itSeg) {
                segmentEquiClass.addIndex(itSeg - segments.begin());
        }
        // look for matching pairs
        for (pixelSegmentListType::iterator itSeg1 = segments.begin(); itSeg1 != segments.end(); ++itSeg1) {
                for (pixelSegmentListType::iterator itSeg2 = itSeg1 + 1; itSeg2 != segments.end(); ++itSeg2) {
                        if ((*itSeg1)->match((*itSeg2), chi2Cut, distCut))
                                segmentEquiClass.addMatch(make_pair(itSeg1 - segments.begin(), itSeg2 - segments.begin()));
                }
        }
        // get classes with indices
        std::map<int, std::vector<int> > segmentClasses = segmentEquiClass.getClasses();
        for (std::map<int, std::vector<int> >::iterator itC = segmentClasses.begin(); itC != segmentClasses.end(); ++itC) {
                // std::cout << " class " << itC->first << " " << itC->second.size() << std::endl;
                if (itC->second.size() > 1) {
                        // combine segments
                        macroPixelListType allMacroPixels;
                        for (std::vector<int>::iterator itInd = itC->second.begin(); itInd != itC->second.end(); ++itInd) {
                                macroPixelListType macroPixels = segments[*itInd]->getMacroPixels();
                                allMacroPixels.insert(allMacroPixels.end(), macroPixels.begin(), macroPixels.end());
                                delete segments[*itInd];
                        }
                        // create combined segment
                        tp_PixelSegment* someSegment = new tp_PixelSegment(newId, level, _bzc, allMacroPixels);
                        // someSegment->print();
                        combinedSegments.push_back(someSegment);
                } else {
                        // uncombined segments
                        combinedSegments.push_back(segments[itC->second.front()]);
                }
        }
        return combinedSegments;
}

} // close namespace brackets

// #endif //#ifdef USE_TPROADS