/**
 * @RDE_HEADER@
 * TrajectoryTransform
 * $Id$
 * $HeadURL$
 * $Author$
 * Created by Gerlinde Rudolphus, ESZS, 2026
 */

#include "trajectory_transform.h"

// Algorithmus: Zeitgewichtete Bézier-Korrektur (empfohlen)
//               Kerngedanke
//               Die ursprüngliche Trajektorie bleibt die Basis
//               Eine glatte Korrekturkurve wird addiert
//               Die Korrektur ist:
//                                   0 am Start
//                                   maximal am Ziel
//                                   zeitlich C¹-stetig
//                                   Formel
//                                   Für jeden Punkt:
//                                                     Pneu(t) = Palt(t) + w(t) · ΔZ
//       mit
//       ΔZ = Zneu − Zalt
//     w(t) = 3s² − 2s³    (smoothstep)
//     s = (t − t0) / (tEnd − t0)
//  Dadurch:
//              Start bleibt exakt gleich
//              Zielpunkt wird exakt erreicht
//              Bewegung bleibt glatt

namespace AreaEffectSimulation
{
namespace TrajectoryTransform
{


double smoothStep(double x)
{
  return x * x * (3.0 - 2.0 * x);
}

template <typename T>
T clamp(T v, T lo, T hi) {
  return (v < lo) ? lo : (v > hi) ? hi : v; }



QVector<TrajectoryPoint> transformToNewTarget(
    const QVector<TrajectoryPoint>& originalTrajMetrical,
    const LtsUtils::QPoint3D& newTargetMetrical)
{
  if (originalTrajMetrical.size() < 2)
    return originalTrajMetrical;

  QVector<TrajectoryPoint> result = originalTrajMetrical;

  const LtsUtils::QPoint3D &oldTarget = originalTrajMetrical.last().position;
  const LtsUtils::QPoint3D &delta = newTargetMetrical - oldTarget;

  const double t0 = originalTrajMetrical.first().time;
  const double tEnd = originalTrajMetrical.last().time;
  const double dt = tEnd - t0;

  for (int i = 0; i < result.size(); ++i)
  {
    double s = (result[i].time - t0) / dt;
    s = clamp(s, 0.0, 1.0);

    double w = smoothStep(s);
    result[i].position += delta * w;
  }

  return result;
}

} // namespace TrajectoryTransform
} // namespace AreaEffectSimulation


std::ostream &operator<< (std::ostream   &stream, const AreaEffectSimulation::TrajectoryPoint &value)
{
  stream << "TrajectoryPoint(" << "position=" << value.position << "; time=" << value.time;
  return stream;
}