qt5-legatus-legacy/Lts/Services/AreaEffectSimulation/TrajectorySplitter/trajectory_splitter.h

/**
  * @RDE_HEADER@
  * TrajectorySplitter
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Norbert Thoden, ESVG, 2026-04-10
  */

#pragma once

//#include <LtsCoordinate/ltscoordinate_export.h>

#include <Core/core.hpp>
#include <QVector>


namespace AreaEffectSimulation
{

// E01562-294: Falsche Darstellung Flugkurve SMArt
// At SMArt ADLER (or SAA?) generates a polyline which is "dividing":
//        x  x
//     x        x
//   x        x   x
// x         x     x
// | Start   | T1  | T2   (Target 1 & Target 2)
// Same diagram, but with order
//        x4  x5
//     x3        x6 (duplicate)
//   x2        x7   x10
// x1         x8     x11
//                    x12
// The relativeTime step backward between x8 and x10.

//using TRAJECTORY_POINT_TYPE = Core::RelativePositionRelativeTime;
//using TRAJECTORY_TYPE       = std::vector<Core::RelativePositionRelativeTime>;

//!
//! \brief The TrajectorySplitter class splits a combined/special vector into single trajectories.
//! See UnitTestTrajectorySplitter
//!
template<typename TRAJECTORY_TYPE>
class TrajectorySplitter
{
public:

//  friend class UnitTestTrajectorySplitter;

  explicit TrajectorySplitter(const TRAJECTORY_TYPE &trajectory)
    : m_trajectory(trajectory)
  {}

  TRAJECTORY_TYPE getTrunk() const
  {
    if (!init())
    {
      return {};
    }

    TRAJECTORY_TYPE trunk(m_trajectory.begin(), m_trajectory.begin()+m_branchIndex);
    return trunk;
  }

  TRAJECTORY_TYPE getBranch1() const
  {
    if (!init())
    {
      return {};
    }

    const int idx = findIndexOfEndOfFirstBranchInternal(m_trajectory);

    const TRAJECTORY_TYPE branch1(m_trajectory.begin()+m_branchIndex, m_trajectory.begin()+idx+1);
    return branch1;
  }

  TRAJECTORY_TYPE getBranch2() const
  {
    if (!init())
    {
      return {};
    }

    const int idx = findIndexOfEndOfFirstBranchInternal(m_trajectory);

    const TRAJECTORY_TYPE branch2(m_trajectory.begin()+idx+1, m_trajectory.end());
    return branch2;
  }

  TRAJECTORY_TYPE getTrajectory1() const
  {
    if (!init())
    {
      return {};
    }

    auto complete = getTrunk();
    const auto &branch = getBranch1();
    complete.insert(complete.end(), branch.begin(), branch.end());
    return complete;
  }

  TRAJECTORY_TYPE getTrajectory2() const
  {
    if (!init())
    {
      return {};
    }

    auto complete = getTrunk();
    const auto &branch = getBranch2();
    complete.insert(complete.end(), branch.begin(), branch.end());
    return complete;
  }

  size_t rawTrajectorySize() const { return m_trajectory.size(); }

protected:
  static qint64 findDuplicateTimeInternal(const TRAJECTORY_TYPE &trajectory)
  {
    // extract relativeTimes, sort, find adjacent duplicates
    QVector<qint64> relativeTimes;
    relativeTimes.reserve(static_cast<int>(trajectory.size()));

    for (const auto &posTimeAware : trajectory)
    {
      relativeTimes.append(posTimeAware.relativeTime());
    }

    std::sort(relativeTimes.begin(), relativeTimes.end());

    auto it = std::adjacent_find(relativeTimes.begin(), relativeTimes.end());

    if (it == relativeTimes.end())
    {
      return -1;   // not found
    }

    const qint64 duplicateRelativeTime = *it;
    return duplicateRelativeTime;
  }
  static int findDuplicateBranchIndexInternal(const TRAJECTORY_TYPE &trajectory)
  {
    const qint64 duplicateRelativeTime = findDuplicateTimeInternal(trajectory);

    if (duplicateRelativeTime < 0)
    {
      return -1;   // not found
    }

    // find first occurance:
    int idx = 0;
    {
      for (const auto &posTimeAware : trajectory)
      {
        const qint64 currentTime = posTimeAware.relativeTime();

        if (duplicateRelativeTime == currentTime)
        {
          break;
        }
        ++idx;
      } // for
    }
    return idx;
  }
  static int findIndexOfEndOfFirstBranchInternal(const TRAJECTORY_TYPE &trajectory)
  {
    int idx = -1;
    {
      qint64 currentTime = std::numeric_limits<qint64>::min();
      for (const auto &posTimeAware : trajectory)
      {
        if (posTimeAware.relativeTime() < currentTime)
        {
          break;
        }
        currentTime = posTimeAware.relativeTime();
        ++idx;
      } // for
    }
    return idx;
  }

  bool init() const
  {
    // already calculated?
    if (m_branchIndex > 0)
    {
      return true;
    }

    // sanity check:
    if (m_trajectory.size() < 3)
    {
      return false;
    }

    m_branchIndex = findDuplicateBranchIndexInternal(m_trajectory);

    return (m_branchIndex >= 0);
  }

private:
  TRAJECTORY_TYPE m_trajectory;
  mutable int m_branchIndex{0};
};

} // namespace AreaEffectSimulation