#include "trajectorywarp.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

QVector<TrajectoryPoint> TrajectoryWarp::warpToNewTarget(
    const QVector<TrajectoryPoint>& original,
    const QVector3D& newTarget)
{
    if (original.size() < 2)
        return original;

    QVector<TrajectoryPoint> result = original;

    const QVector3D& oldTarget = original.last().position;
    QVector3D delta = newTarget - oldTarget;

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

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

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

    return result;
}

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