MySpacePointMaker.cpp

// This file is part of the ACTS project.
//
// Copyright (C) 2016 CERN for the benefit of the ACTS project
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.

#include "MySpacePointMaker.hpp"

#include "Acts/Definitions/Algebra.hpp"
#include "Acts/EventData/SourceLink.hpp"
#include "Acts/SpacePointFormation/SpacePointBuilderConfig.hpp"
#include "Acts/SpacePointFormation/SpacePointBuilderOptions.hpp"
#include "ActsExamples/EventData/GeometryContainers.hpp"
#include "ActsExamples/EventData/IndexSourceLink.hpp"
#include "ActsExamples/EventData/Measurement.hpp"
#include "ActsExamples/EventData/SimSpacePoint.hpp"
#include "ActsExamples/Framework/AlgorithmContext.hpp"
#include "Acts/Surfaces/Surface.hpp"
//#include "ActsExamples/Utilities/GroupBy.hpp"

#include <algorithm>
#include <functional>
#include <iterator>
#include <ostream>
#include <stdexcept>
#include <utility>
#include <list>
#include <map>

#include "TMath.h"
//#include "tracker_config.h"
#include "Math/Functor.h"
#include "Math/RootFinder.h"

namespace straw_helpers
{
  using ISL = ActsExamples::IndexSourceLink;
  using FtdLayerTypes = MyFtdGeo::FtdLayerTypes;
  using PreCandidate = ActsExamples::MySpacePointMaker::PreCandidate;

  struct CandBuildCtx {
    std::vector<std::list<ISL>> const& mesPerLay;
    std::vector<int> const& layerTypes;
    std::vector<int> const& nTubesPerLayer;
    std::shared_ptr<MyFtdGeo> ftdGeo;
    int nLayers = 0;
    int nLayPerSt = 0;

    int maxLoDeltaPStrawId = 0;
    int maxLoDeltaMStrawId = 0;
    int maxHiDeltaPStrawId = 0;
    int maxHiDeltaMStrawId = 0;
    int minMeasPerCand = 0;
  };

  static inline ISL const* getRefByIdx(PreCandidate const& cand, int idx) {
    if (idx < 0 || idx >= static_cast<int>(cand.sourceLinks.size())) {
      return nullptr;
    }
    return &cand.sourceLinks[idx];
  }

  static inline int findRefIdxForType(PreCandidate const& cand, int typeKey) {
    if (cand.refTypeA == typeKey) return cand.refIdxA;
    if (cand.refTypeB == typeKey) return cand.refIdxB;
    if (cand.refTypeC == typeKey) return cand.refIdxC;
    return -1;
  }

  static inline ISL const* findRefForType(PreCandidate const& cand, int typeKey) {
    return getRefByIdx(cand, findRefIdxForType(cand, typeKey));
  }

  static inline int findOtherRefIdxA(PreCandidate const& cand, int typeKey) {
    if (cand.refTypeA != -1 && cand.refTypeA != typeKey) return cand.refIdxA;
    if (cand.refTypeB != -1 && cand.refTypeB != typeKey) return cand.refIdxB;
    if (cand.refTypeC != -1 && cand.refTypeC != typeKey) return cand.refIdxC;
    return -1;
  }

  static inline int findOtherRefIdxB(PreCandidate const& cand, int typeKey) {
    int found = 0;
    if (cand.refTypeA != -1 && cand.refTypeA != typeKey) {
      if (found == 1) return cand.refIdxA;
      ++found;
    }
    if (cand.refTypeB != -1 && cand.refTypeB != typeKey) {
      if (found == 1) return cand.refIdxB;
      ++found;
    }
    if (cand.refTypeC != -1 && cand.refTypeC != typeKey) {
      if (found == 1) return cand.refIdxC;
      ++found;
    }
    return -1;
  }

  static inline ISL const* findOtherRefA(PreCandidate const& cand, int typeKey) {
    return getRefByIdx(cand, findOtherRefIdxA(cand, typeKey));
  }

  static inline ISL const* findOtherRefB(PreCandidate const& cand, int typeKey) {
    return getRefByIdx(cand, findOtherRefIdxB(cand, typeKey));
  }

  static inline void setFirstRefForType(PreCandidate& cand, int typeKey, int idx) {
    if (cand.refTypeA == typeKey) {
      if (cand.refIdxA < 0) cand.refIdxA = idx;
      return;
    }
    if (cand.refTypeB == typeKey) {
      if (cand.refIdxB < 0) cand.refIdxB = idx;
      return;
    }
    if (cand.refTypeC == typeKey) {
      if (cand.refIdxC < 0) cand.refIdxC = idx;
      return;
    }
    if (cand.refTypeA == -1) {
      cand.refTypeA = typeKey;
      cand.refIdxA = idx;
      return;
    }
    if (cand.refTypeB == -1) {
      cand.refTypeB = typeKey;
      cand.refIdxB = idx;
      return;
    }
    if (cand.refTypeC == -1) {
      cand.refTypeC = typeKey;
      cand.refIdxC = idx;
    }
  }

  static inline int cyclicDelta(int cur, int ref, int nStraws) {
    int d = cur - ref;
    if (d >  nStraws / 2) d -= nStraws;
    if (d < -nStraws / 2) d += nStraws;
    return d;
  }

  static inline bool passesAgainstRefStraw(int strawId, int refStrawId,
                                           int maxDeltaP, int maxDeltaM,
                                           int nStraws) {
    const int delta = cyclicDelta(strawId, refStrawId, nStraws);
    if (delta >= 0 && delta > maxDeltaP) return false;
    if (delta <  0 && -delta > maxDeltaM) return false;
    return true;
  }

  void constructCands(int layerId,
                      int station,
                      CandBuildCtx const& ctx,
                      std::list<PreCandidate>& cands,
                      PreCandidate& cand)
  {
    const int layerInStation = layerId % ctx.nLayPerSt;
    const int layersLeft = ctx.nLayPerSt - layerInStation;
    const int missing = ctx.minMeasPerCand - static_cast<int>(cand.sourceLinks.size());

    if (missing > 0 && layersLeft < missing)
      return;

    if (layerId >= ctx.nLayers || ctx.ftdGeo->GetLayerStation(layerId) != station) {
      if (static_cast<int>(cand.sourceLinks.size()) >= ctx.minMeasPerCand &&
          cand.refTypeA != -1 && cand.refTypeB != -1 && cand.refTypeC != -1)
        cands.emplace_back(cand);
      return;
    }

    if (ctx.ftdGeo->GetLayerType(layerId) == FtdLayerTypes::kPixel) {
      constructCands(layerId + 1, station, ctx, cands, cand);
      return;
    }

    const auto& curLayerList = ctx.mesPerLay[layerId];
    const int layTypeKey = static_cast<int>(ctx.layerTypes[layerId]);
    const int nStraws = ctx.nTubesPerLayer[layerId];

    ISL const* sameTypeRef = findRefForType(cand, layTypeKey);
    ISL const* otherTypeRefA = findOtherRefA(cand, layTypeKey);
    ISL const* otherTypeRefB = findOtherRefB(cand, layTypeKey);

    const int sameRefStrawId = (sameTypeRef != nullptr) ? sameTypeRef->geometryId().sensitive() : -1;
    const int otherRefStrawIdA = (otherTypeRefA != nullptr) ? otherTypeRefA->geometryId().sensitive() : -1;
    const int otherRefStrawIdB = (otherTypeRefB != nullptr) ? otherTypeRefB->geometryId().sensitive() : -1;

    for (const ISL& isl : curLayerList) {
      const int strawId = isl.geometryId().sensitive();
      bool ok = true;
      if (sameTypeRef != nullptr) ok = passesAgainstRefStraw(strawId, sameRefStrawId, ctx.maxLoDeltaPStrawId, ctx.maxLoDeltaMStrawId, nStraws);
      if (ok && otherTypeRefA != nullptr) ok = passesAgainstRefStraw(strawId, otherRefStrawIdA, ctx.maxHiDeltaPStrawId, ctx.maxHiDeltaMStrawId, nStraws);
      if (ok && otherTypeRefB != nullptr) ok = passesAgainstRefStraw(strawId, otherRefStrawIdB, ctx.maxHiDeltaPStrawId, ctx.maxHiDeltaMStrawId, nStraws);
      if (!ok) continue;
      const int oldRefTypeA = cand.refTypeA;
      const int oldRefTypeB = cand.refTypeB;
      const int oldRefTypeC = cand.refTypeC;
      const int oldRefIdxA  = cand.refIdxA;
      const int oldRefIdxB  = cand.refIdxB;
      const int oldRefIdxC  = cand.refIdxC;
      cand.sourceLinks.push_back(isl);
      if (sameTypeRef == nullptr) setFirstRefForType(cand, layTypeKey, static_cast<int>(cand.sourceLinks.size()) - 1);
      constructCands(layerId + 1, station, ctx, cands, cand);
      cand.sourceLinks.pop_back();
      cand.refTypeA = oldRefTypeA;
      cand.refTypeB = oldRefTypeB;
      cand.refTypeC = oldRefTypeC;
      cand.refIdxA  = oldRefIdxA;
      cand.refIdxB  = oldRefIdxB;
      cand.refIdxC  = oldRefIdxC;
    }

    constructCands(layerId + 1, station, ctx, cands, cand);
  }
}

ActsExamples::MySpacePointMaker::MySpacePointMaker(Config cfg, Acts::Logging::Level lvl)
  : IAlgorithm("MySpacePointMaker", lvl), m_cfg(std::move(cfg)) {
  m_inputMeasurements.initialize(m_cfg.inputMeasurements);
  m_outputSpacePoints.initialize(m_cfg.outputSpacePoints);
  m_inputMeasurementParticlesMap.initialize(m_cfg.inputMeasurementParticlesMap);

  auto spConstructor =
      [](const Acts::Vector3& pos, std::optional<double> t,
         const Acts::Vector2& cov, std::optional<double> varT,
         boost::container::static_vector<Acts::SourceLink, 2> slinks)
      -> SimSpacePoint {
    return SimSpacePoint(pos, t, cov[0], cov[1], varT, std::move(slinks));
  };

  auto spBuilderConfig = Acts::SpacePointBuilderConfig();
  spBuilderConfig.trackingGeometry = m_cfg.detector->GetTrackingGeometry();
  m_slSurfaceAccessor.emplace(IndexSourceLink::SurfaceAccessor{*spBuilderConfig.trackingGeometry});
  spBuilderConfig.slSurfaceAccessor.connect<&IndexSourceLink::SurfaceAccessor::operator()>(&m_slSurfaceAccessor.value());
  m_spacePointBuilder = Acts::SpacePointBuilder<SimSpacePoint>(spBuilderConfig, spConstructor, Acts::getDefaultLogger("SpacePointBuilder", lvl));
}

ActsExamples::ProcessCode ActsExamples::MySpacePointMaker::execute(const AlgorithmContext& ctx) const {
  ACTS_INFO("Starting my space point maker for event " << ctx.eventNumber);
  using ISL = IndexSourceLink;
  using FtdLayerTypes = MyFtdGeo::FtdLayerTypes;

  std::shared_ptr<MyFtdGeo> ftdGeo = m_cfg.detector->FtdGeo();
  std::shared_ptr<MyFtdDetector> det = m_cfg.detector;

  const auto& measurements = m_inputMeasurements(ctx);
  const auto& measurementParticlesMap = m_inputMeasurementParticlesMap(ctx);

  std::vector<int> particleIds(measurements.size());
  for (auto meas : measurements) {
    for (const auto& [_, barcode] : ActsExamples::makeRange(measurementParticlesMap.equal_range(meas.index()))) {
      particleIds[meas.index()] = barcode.particle();
    }
  }

  // function to access measurement parameters using source links
  auto accessor = [&measurements](Acts::SourceLink slink) {
    const auto islink = slink.get<ISL>();
    const ConstVariableBoundMeasurementProxy meas = measurements.getMeasurement(islink.index());
    return std::make_pair(meas.fullParameters(), meas.fullCovariance());
  };

  // fill front and back strip vectors of pairs(source link, pair<stripEnd1, stripEnd2>)
  std::vector<std::pair<Acts::SourceLink, std::pair<Acts::Vector3, Acts::Vector3>>> frontStrips;
  std::vector<std::pair<Acts::SourceLink, std::pair<Acts::Vector3, Acts::Vector3>>> backStrips;
  std::vector<Acts::SourceLink> twoDimMeasurements;

  int nStations = ftdGeo->GetNumberOfStations();
  int nLayPerSt = ftdGeo->GetNLayersPerStation();
  int nLayers = ftdGeo->GetNumberOfLayers();

  std::vector<std::array<double, 5>> cacheZSCGD(measurements.size());
  std::vector<std::list<ISL>> mesPerLay(nLayers);
  for (const auto& isl : measurements.orderedIndices()) {
    const auto geoId = isl.geometryId();
    int iLayer = det->GeoIdToFtdLayer(geoId);
    if (ftdGeo->GetLayerType(iLayer) == FtdLayerTypes::kPixel) continue;
    mesPerLay[iLayer].emplace_back(isl);
    Acts::SourceLink slink{isl};
    // tube center shifted by the measured distance to wire
    const Acts::Surface* surface = m_slSurfaceAccessor.value()(slink);
    const auto [par, cov] = accessor(slink);
    auto xyz = surface->localToGlobal(ctx.geoContext, Acts::Vector2(par[0],0), Acts::Vector3());
    auto rot = surface->localToGlobalTransform(ctx.geoContext).rotation();
    double x = xyz[0];
    double y = xyz[1];
    double z = xyz[2];
    double cosp = rot(0,1);
    double sinp = rot(0,0);
    cacheZSCGD[isl.index()][0] = z;                // z
    cacheZSCGD[isl.index()][1] = z*sinp;           // s
    cacheZSCGD[isl.index()][2] = z*cosp;           // c
    cacheZSCGD[isl.index()][3] = -x*sinp + y*cosp; // g
    cacheZSCGD[isl.index()][4] = sqrt(cov(0,0));   // d
  }

  const std::vector<int>& nTubesPerLayer = ftdGeo->GetLayerNumbersOfTubes();
  const std::vector<int>& layerTypes = ftdGeo->GetLayerTypes();
  auto vActsLayerToFtdLayer = det->GetActsLayerToFtdLayer();

  std::vector<int> vFirstLayInStation{};
  int firstLay = -1;
  int prevStation = -1;
  for (int iL = 0; iL < nLayers; ++iL) {
    int station = ftdGeo->GetLayerStation(iL);
    if (ftdGeo->GetLayerType(iL) == FtdLayerTypes::kPixel) continue;
    if (station != prevStation) {
      prevStation = station;
      firstLay = iL;
      vFirstLayInStation.push_back(firstLay);
    }
  }

  // construct straw spacepoints
  std::vector<std::list<PreCandidate>> preCandidates(nStations);
  for (auto iL : vFirstLayInStation) {
    int station = ftdGeo->GetLayerStation(iL);
    if (station==1 || station==3) continue;
    auto& candList = preCandidates[station];
    PreCandidate cand{};  // starts empty
    int maxLoDeltaPStrawId, maxHiDeltaPStrawId;
    int maxLoDeltaMStrawId, maxHiDeltaMStrawId;
    if (station == 0) { maxLoDeltaPStrawId = m_cfg.maxLoDeltaPStrawId1; maxHiDeltaPStrawId = m_cfg.maxHiDeltaPStrawId1; }
    if (station == 2) { maxLoDeltaPStrawId = m_cfg.maxLoDeltaPStrawId2; maxHiDeltaPStrawId = m_cfg.maxHiDeltaPStrawId2; }
    if (station == 4) { maxLoDeltaPStrawId = m_cfg.maxLoDeltaPStrawId3; maxHiDeltaPStrawId = m_cfg.maxHiDeltaPStrawId3; }
    if (station == 0) { maxLoDeltaMStrawId = m_cfg.maxLoDeltaMStrawId1; maxHiDeltaMStrawId = m_cfg.maxHiDeltaMStrawId1; }
    if (station == 2) { maxLoDeltaMStrawId = m_cfg.maxLoDeltaMStrawId2; maxHiDeltaMStrawId = m_cfg.maxHiDeltaMStrawId2; }
    if (station == 4) { maxLoDeltaMStrawId = m_cfg.maxLoDeltaMStrawId3; maxHiDeltaMStrawId = m_cfg.maxHiDeltaMStrawId3; }
    straw_helpers::CandBuildCtx buildCtx{mesPerLay, layerTypes, nTubesPerLayer,
                                         ftdGeo, nLayers, nLayPerSt,
                                         maxLoDeltaPStrawId, maxLoDeltaMStrawId,
                                         maxHiDeltaPStrawId, maxHiDeltaMStrawId,
                                         m_cfg.minMeasPerCand};
    straw_helpers::constructCands(iL, station, buildCtx, candList, cand);
  }

  // construct everything else
  double rMax = ftdGeo->GetLayerRMax(ftdGeo->GetNumberOfLayers()-1);
  for (auto& isl : measurements.orderedIndices()) {
    const auto geoId = isl.geometryId();
    const auto volumeId = geoId.volume();
    const auto layerId = geoId.layer();
    int iLayer = det->GeoIdToFtdLayer(geoId);
    int iStation = ftdGeo->GetLayerStation(iLayer);
    Acts::SourceLink slink{isl};
    int layerType = ftdGeo->GetLayerType(iLayer);
    if (layerType == FtdLayerTypes::kPixel) {
      twoDimMeasurements.emplace_back(slink);
    } else if (0) {
      const auto [par, cov] = accessor(slink);
      const Acts::Surface *surface = m_slSurfaceAccessor.value()(slink);
      // TODO: more realistic strip dimensions including inner radii and half-station splitting
      auto gpos1 = surface->localToGlobal(ctx.geoContext, Acts::Vector2(par[0], -rMax), Acts::Vector3());
      auto gpos2 = surface->localToGlobal(ctx.geoContext, Acts::Vector2(par[0], rMax), Acts::Vector3());
      if (layerId % 4 == 2) {
        frontStrips.emplace_back(slink, std::make_pair(gpos1, gpos2));
      } else if (layerId % 4 == 0) {
        backStrips.emplace_back(slink, std::make_pair(gpos1, gpos2));
      }
    }
  }

  // return ActsExamples::ProcessCode::SUCCESS;
  ACTS_VERBOSE("making space points from straws");

  SimSpacePointContainer spacePoints;

  for (int iStation=0;iStation<nStations;iStation++) {
    if (iStation==1 || iStation==3) continue;

    auto& preCandidatesSt = preCandidates[iStation];

    ACTS_INFO("Station " << iStation << ": number of precandidates " << preCandidatesSt.size());

    std::vector<Candidate> selectedUnfiltCandidates;
    for (auto& preCandidate: preCandidatesSt){
      int nlinks = preCandidate.sourceLinks.size();
      ACTS_VERBOSE("  candidate.size=" << nlinks);
      auto [chi2par, tx, ty, k] = parabolic(preCandidate, cacheZSCGD);
      double chi2ndf = nlinks > 3 ? chi2par / (nlinks - 3) : chi2par;
      ACTS_VERBOSE("par:  nlinks=" << nlinks << " tx=" << tx <<" ty=" << ty << " chi2/ndf=" << chi2ndf);
      if (chi2ndf > m_cfg.maxChi2) continue;
      if (fabs(k)>0.0002) continue;
      auto& refSelCand = selectedUnfiltCandidates.emplace_back(Candidate({preCandidate.sourceLinks, iStation, {}, chi2par, chi2ndf, tx, ty, k}));
      // uncomment to debug precandidates
      // printf("station %d: chi2/ndf=%e particles: ", refSelCand.station, refSelCand.chi2ndf);
      // for (auto& isl : refSelCand.sourceLinks) printf("%d ", particleIds[isl.index()]);
      // printf("\n");
    }

    preCandidatesSt.clear();
    ACTS_INFO("Station " << iStation << ": precandidates after chi2=" << selectedUnfiltCandidates.size());

    std::vector<Candidate> selectedCandidates;
    std::set<int> selectedCandidateIds;
    std::map<int,std::set<int>> candidatesPerStraw;
    int iSelCand = 0;

    // sort from smallest to largest
    std::sort(selectedUnfiltCandidates.begin(), selectedUnfiltCandidates.end(),
              [](const Candidate& a, const Candidate& b) { return a.sourceLinks.size() < b.sourceLinks.size(); }); 
    
    for (auto it = selectedUnfiltCandidates.begin(); it != selectedUnfiltCandidates.end(); ++it) { // take smallest first
      bool isSub = false;
      for (auto jt = selectedUnfiltCandidates.rbegin(); jt != selectedUnfiltCandidates.rend(); ++jt) { // take largest first
        if (it == jt.base() - 1) break; // stop if jt = it
        std::size_t i = 0; // 
        auto& small = it->sourceLinks;
        auto& large = jt->sourceLinks;
        for (std::size_t j = 0; j < large.size() && i < small.size(); ++j) {
          if (large[j] == small[i]) {
            ++i;
          }
        }
        if (i == small.size()) {
          isSub = true;
          break;
        }
      }
      if (isSub) continue;
      auto& refSelCand = selectedCandidates.emplace_back(*it);

      for (auto& isl : refSelCand.sourceLinks) {
        int straw = isl.geometryId().layer() * 10000 + isl.geometryId().sensitive();
        refSelCand.straws.push_back(straw);
        candidatesPerStraw[straw].insert(iSelCand);
      }
      selectedCandidateIds.insert(iSelCand);
      ++iSelCand;
    }

    selectedUnfiltCandidates.clear();

    ACTS_INFO("Station " << iStation << ": number of filtered precandidates " << selectedCandidates.size());

    ACTS_VERBOSE("Start filtering...");
    ACTS_VERBOSE("Info on shared measurements...");
    for (auto& candidate : selectedCandidates) {
      for (auto straw : candidate.straws)
        if (candidatesPerStraw[straw].size() > m_cfg.maximumAllowedCandidatesPerStraw) candidate.sharedStraws++;
    }

    auto sharedStrawsComperator = [&selectedCandidates](std::size_t a, std::size_t b) {
      return selectedCandidates[a].sharedStraws < selectedCandidates[b].sharedStraws;
    };

    auto candidateComperator =  [&selectedCandidates](std::size_t a, std::size_t b) {
      int nMeasA = selectedCandidates[a].sourceLinks.size();
      int nMeasB = selectedCandidates[b].sourceLinks.size();
      auto relSharedA = 1.*selectedCandidates[a].sharedStraws/nMeasA;
      auto relSharedB = 1.*selectedCandidates[b].sharedStraws/nMeasB;
      if (relSharedA != relSharedB) return relSharedA < relSharedB;
      if (nMeasA == nMeasB) return selectedCandidates[a].chi2 < selectedCandidates[b].chi2;
      return nMeasA > nMeasB;
    };

    for (int i = 0; i < m_cfg.maximumIterations; i++) {
      if (selectedCandidateIds.empty()) break;
      auto maximumSharedStraws = *std::max_element(selectedCandidateIds.begin(), selectedCandidateIds.end(), sharedStrawsComperator);
      // printf("candidate %d with maximumSharedStraws=%d\n",maximumSharedStraws, selectedCandidates[maximumSharedStraws].sharedStraws);
      if (selectedCandidates[maximumSharedStraws].sharedStraws <= m_cfg.maximumSharedStraws) break;
      auto badCandidate = *std::max_element(selectedCandidateIds.begin(), selectedCandidateIds.end(), candidateComperator);
      // printf("badCandidate=%d %zu\n",badCandidate, selectedCandidates[badCandidate].sourceLinks.size());
      // remove bad track
      for (auto straw : selectedCandidates[badCandidate].straws) {
        candidatesPerStraw[straw].erase(badCandidate);
        if (candidatesPerStraw[straw].size()>m_cfg.maximumAllowedCandidatesPerStraw) continue;
        // reducing shared straws for corresponding candidates
        for (auto it : candidatesPerStraw[straw]) selectedCandidates[it].sharedStraws--;
      }
      ACTS_VERBOSE("Removing candidate " << badCandidate
                                         << " meas=" << selectedCandidates[badCandidate].sourceLinks.size()
                                         << " shared=" << selectedCandidates[badCandidate].sharedStraws
                                         << " chi2ndf=" << selectedCandidates[badCandidate].chi2ndf);
      selectedCandidateIds.erase(badCandidate);
    }

    ACTS_VERBOSE("Selected candidates...");
    for (auto& id : selectedCandidateIds) {
      auto& sp = selectedCandidates[id];
      int nlinks = sp.sourceLinks.size();

      // printf("station %d: chi2/ndf=%e particles: ", sp.station, sp.chi2ndf);
      // for (auto& isl : sp.sourceLinks) printf("%d ", particleIds[isl.index()]);
      // printf("\n");

      ACTS_VERBOSE("finding majority particle id and majority ratio for spacepoint" << id);
      auto identifyContributingParticles = [&sp, &particleIds]() {
        std::vector<std::pair<int, int>> particleCounts;
        for (auto& isl : sp.sourceLinks) {
          int particleId = particleIds[isl.index()];
          auto it = std::ranges::find_if(particleCounts, [=](const auto& a) { return (a.first == particleId); });
          if (it != particleCounts.end()) {
            it->second += 1u;
          } else {
            particleCounts.emplace_back(particleId, 1u);
          }
        }
        std::ranges::sort(particleCounts, std::greater{},[](const auto& p) { return p.second; });
        return particleCounts;
      };
      auto particleCounts = identifyContributingParticles();
      int majorityParticleId = particleCounts.front().first;
      int nMajority = particleCounts.front().second;
      double majorityCode = majorityParticleId + 0.1*nMajority/nlinks;
      ACTS_VERBOSE("majorityParticleId=" << majorityParticleId << " nMajourity=" << nMajority);

      Acts::SourceLink slink1{sp.sourceLinks[0]};
      Acts::SourceLink slink2{sp.sourceLinks[nlinks-1]};
      const Acts::Surface* surface = m_slSurfaceAccessor.value()(slink1);
      double z = surface->center(ctx.geoContext)[2];
      double x = sp.tx * z + sp.k * sp.ty * z * z;
      double y = sp.ty * z - sp.k * sp.tx * z * z;
      auto [chi2lin, txl, tyl, varxx, varyy, varxy] = linear(sp.sourceLinks, cacheZSCGD);
      ACTS_VERBOSE("lin:  n=" << nlinks << " tx=" << txl <<" ty=" << tyl << " chi2/ndf=" << chi2lin/(nlinks-2));
      // printf("Candidate %d, meas: %d, shared: %d, chi2=%f\n", id, sp.sourceLinks.size(), sp.sharedStraws, sp.chi2);
      ACTS_VERBOSE("SP: k=" << sp.k << " x=" << x << " y=" << y << " z=" << z << " var_xy=" << varxy*z*z);
      boost::container::static_vector<Acts::SourceLink, 2> slinks = {slink1, slink2};
      Acts::Vector3 pos{x, y, z};
      double var_r = (x*x*varxx + y*y*varyy + 2*x*y*varxy)/(x*x+y*y)*z*z;
      double var_z = 0.1;
      if (var_r>1.) continue;
      //std::back_inserter(spacePoints) = SimSpacePoint(pos, sp.chi2ndf*Acts::UnitConstants::ns, var_r, var_z, 0, slinks);
      std::back_inserter(spacePoints) = SimSpacePoint(pos, majorityCode*Acts::UnitConstants::ns, var_r, var_z, 0, slinks);
      // std::back_inserter(spacePoints) = SimSpacePoint(pos, sp.varxy*sp.z*sp.z*Acts::UnitConstants::ns, sp.varxx*sp.z*sp.z, sp.varyy*sp.z*sp.z, 0, slinks);
    }
    ACTS_INFO("Station " << iStation << ": number of spacepoints " << selectedCandidateIds.size());
  }

  ACTS_VERBOSE("making strip pairs:" << "back: " << frontStrips.size() << " front: " << backStrips.size());

  // make space points from strips
  for (auto& fstrip : frontStrips) {
    float fz = fstrip.second.second[2];
    for (auto& bstrip : backStrips) {
      float bz = bstrip.second.second[2];
      if (fabs(fz - bz) > 20) continue;
      std::vector<Acts::SourceLink> slinks = {fstrip.first, bstrip.first};
      auto strippair = std::make_pair(fstrip.second, bstrip.second);
      Acts::SpacePointBuilderOptions spOptStrips{strippair, accessor};
      m_spacePointBuilder.buildSpacePoint(ctx.geoContext, slinks, spOptStrips, std::back_inserter(spacePoints));
    }
  }

  // make space points from 2D measurements
  Acts::SpacePointBuilderOptions spOpt;
  spOpt.paramCovAccessor = accessor;

  for (auto& slink : twoDimMeasurements) {
    // m_spacePointBuilder.buildSpacePoint(ctx.geoContext, {slink}, spOpt, std::back_inserter(spacePoints));
  }

  spacePoints.shrink_to_fit();

  ACTS_INFO("Created " << spacePoints.size() << " space points");
  m_outputSpacePoints(ctx, std::move(spacePoints));

  return ActsExamples::ProcessCode::SUCCESS;
}

std::tuple<double,double,double,double,double,double>
  ActsExamples::MySpacePointMaker::linear(
    const std::vector<IndexSourceLink>& sourceLinks, const std::vector<std::array<double, 5>>& cacheZSCGD, bool debug) const
{
  int n = sourceLinks.size();

  double ss = 0.0;
  double cc = 0.0;
  double sc = 0.0;
  double gs = 0.0;
  double gc = 0.0;

  for (int i = 0; i < n; ++i){
    auto idx = sourceLinks[i].index();
    double s = cacheZSCGD[idx][1];
    double c = cacheZSCGD[idx][2];
    double g = cacheZSCGD[idx][3];
    double d = cacheZSCGD[idx][4];
    double w = 1.0 / (d * d);
    ss += w * s * s;
    cc += w * c * c;
    sc += w * s * c;
    gs += w * g * s;
    gc += w * g * c;
  }
  double dd = (cc * ss - sc * sc) > 1e-12 ? 1./(cc * ss - sc * sc) : 1e12;
  double tx = (sc * gc - cc * gs) * dd;
  double ty = (ss * gc - sc * gs) * dd;
  double varxx = cc * dd;
  double varyy = ss * dd;
  double varxy = sc * dd;

  double chi2 = 0;
  for (int i=0;i<n;i++){
    auto idx = sourceLinks[i].index();
    double s = cacheZSCGD[idx][1];
    double c = cacheZSCGD[idx][2];
    double g = cacheZSCGD[idx][3];
    double d = cacheZSCGD[idx][4];
    double chi = (s*tx - c*ty + g)/d;
    chi2 += chi*chi;
  }
  if (debug) printf("tx=%f ty=%f chi2=%f varxx=%e varyy=%e varxy=%e\n",tx, ty, chi2, varxx, varyy, varxy);

  return std::make_tuple(chi2, tx, ty, varxx, varyy, varxy);
}

std::tuple<double,double,double,double> ActsExamples::MySpacePointMaker::parabolic(
  PreCandidate& cand, const std::vector<std::array<double, 5>>& cacheZSCGD, bool debug) const
{
  int n = cand.sourceLinks.size();
  std::vector<double> sk(n, 0.);
  std::vector<double> ck(n, 0.);

  double tx, ty;

  auto fk = [n,&cand,&cacheZSCGD,&sk,&ck,&tx,&ty](double kkk) {
    double ss = 0;
    double sc = 0;
    double cc = 0;
    double gs = 0;
    double gc = 0;
    for (int i=0;i<n;i++){
      auto idx = cand.sourceLinks[i].index();
      double z = cacheZSCGD[idx][0];
      double s = cacheZSCGD[idx][1];
      double c = cacheZSCGD[idx][2];
      double g = cacheZSCGD[idx][3];
      sk[i] = (s + kkk*z*c);
      ck[i] = (c - kkk*z*s);
      ss += sk[i]*sk[i];
      sc += sk[i]*ck[i];
      cc += ck[i]*ck[i];
      gs += g*sk[i];
      gc += g*ck[i];
    }
    const double det = ss * cc - sc * sc;
    if (std::abs(det) < 1e-12) {
      return 1e12;
    }
    tx = (gc * sc - gs * cc) / det;
    ty = (gc * ss - gs * sc) / det;
    double sum = 0;
    for (int i=0;i<n;i++){
      auto idx = cand.sourceLinks[i].index();
      double z = cacheZSCGD[idx][0];
      double s = cacheZSCGD[idx][1];
      double c = cacheZSCGD[idx][2];
      double g = cacheZSCGD[idx][3];
      sum+=(tx*sk[i]-ty*ck[i]+g)*(ty*z*s+tx*z*c);
    }
    return sum;
  };

  double f0 = fk(0.);
  double dk = 1e-5;
  double dfdk = (fk(dk)-f0)/dk;
  double kk = -2*f0/dfdk;
  double fkk = fk(kk);
  if (!(f0 * fkk < 0.)) {
    return {1e12,-1,-1,-1};
  }
  double kmin = kk>0 ? 0  : kk;
  double kmax = kk>0 ? kk :  0;
  if (debug) printf("%f %f %f %f\n",kmin, kmax, fk(kmin), fk(kmax));

  ROOT::Math::Functor1D functor(fk);
  ROOT::Math::RootFinder rf(ROOT::Math::RootFinder::kBRENT);
  rf.SetFunction(functor, kmin, kmax);
  double k;
  if (rf.Solve()) {
    k = rf.Root();
    double froot = fk(k);
    if (debug) printf("k=%e %f iterations=%d\n", k, froot, rf.Iterations());
  } else {
    return {1e12,-1,-1,-1};
  }

  if (debug) { 
    printf("checking tx=%f ty=%f k=%f\n",tx, ty, k);
    double sum_tx = 0;
    double sum_ty = 0;
    for (int i=0;i<n;i++){
      auto idx = cand.sourceLinks[i].index();
      double z = cacheZSCGD[idx][0];
      double s = cacheZSCGD[idx][1];
      double c = cacheZSCGD[idx][2];
      double g = cacheZSCGD[idx][3];
      double a = s + k*z*c;
      double b = c - k*z*s;
      double d = a*tx - b*ty + g;
      sum_tx += d*a;
      sum_ty += d*b;
    }
    printf("sum_tx=%f\n",sum_tx);
    printf("sum_ty=%f\n",sum_ty);
  }

  double chi2 = 0;
  for (int i=0;i<n;i++){
    auto idx = cand.sourceLinks[i].index();
    double g = cacheZSCGD[idx][3];
    double d = cacheZSCGD[idx][4];
    double chi = (sk[i]*tx - ck[i]*ty + g)/d;
    chi2 += chi*chi;
  }
  return {chi2, tx, ty, k};
}