// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later

// Trajectory.hpp — Joint-space trajectory generation
#ifndef CALX_ROBOTICS_TRAJECTORY_HPP
#define CALX_ROBOTICS_TRAJECTORY_HPP

#include <vector>
#include <cmath>
#include <algorithm>
#include <stdexcept>

namespace calx::robotics {

    // 軌道上の一点: 時刻・位置・速度・加速度
    template<typename T>
    struct TrajectoryPoint {
        T time, position, velocity, acceleration;
    };

    // 3次多項式軌道 (端点の位置・速度を指定)
    // q(t) = a0 + a1*t + a2*t² + a3*t³
    template<typename T>
    std::vector<TrajectoryPoint<T>> cubicTrajectory(T q0, T qf, T v0, T vf, T tf, int nPoints) {
        if (nPoints < 2 || tf <= T(0)) {
            throw std::invalid_argument("cubicTrajectory: nPoints >= 2 and tf > 0 required");
        }

        // 境界条件から係数を決定
        const T tf2 = tf * tf;
        const T tf3 = tf2 * tf;
        const T a0 = q0;
        const T a1 = v0;
        const T a2 = (T(3) * (qf - q0) - (T(2) * v0 + vf) * tf) / tf2;
        const T a3 = (T(2) * (q0 - qf) + (v0 + vf) * tf) / tf3;

        std::vector<TrajectoryPoint<T>> traj(nPoints);
        for (int i = 0; i < nPoints; ++i) {
            const T t = tf * T(i) / T(nPoints - 1);
            const T t2 = t * t;
            traj[i].time         = t;
            traj[i].position     = a0 + a1 * t + a2 * t2 + a3 * t2 * t;
            traj[i].velocity     = a1 + T(2) * a2 * t + T(3) * a3 * t2;
            traj[i].acceleration = T(2) * a2 + T(6) * a3 * t;
        }
        return traj;
    }

    // 5次多項式軌道 (端点の位置・速度・加速度を指定)
    // q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
    template<typename T>
    std::vector<TrajectoryPoint<T>> quinticTrajectory(T q0, T qf, T v0, T vf,
                                                       T a0, T af, T tf, int nPoints) {
        if (nPoints < 2 || tf <= T(0)) {
            throw std::invalid_argument("quinticTrajectory: nPoints >= 2 and tf > 0 required");
        }

        const T tf2 = tf * tf;
        const T tf3 = tf2 * tf;
        const T tf4 = tf3 * tf;
        const T tf5 = tf4 * tf;

        const T c0 = q0;
        const T c1 = v0;
        const T c2 = a0 / T(2);
        const T dq = qf - q0;
        const T c3 = (T(20) * dq - (T(8) * vf + T(12) * v0) * tf - (T(3) * a0 - af) * tf2) / (T(2) * tf3);
        const T c4 = (T(-30) * dq + (T(14) * vf + T(16) * v0) * tf + (T(3) * a0 - T(2) * af) * tf2) / (T(2) * tf4);
        const T c5 = (T(12) * dq - T(6) * (vf + v0) * tf + (af - a0) * tf2) / (T(2) * tf5);

        std::vector<TrajectoryPoint<T>> traj(nPoints);
        for (int i = 0; i < nPoints; ++i) {
            const T t = tf * T(i) / T(nPoints - 1);
            const T t2 = t * t, t3 = t2 * t, t4 = t3 * t, t5 = t4 * t;
            traj[i].time         = t;
            traj[i].position     = c0 + c1 * t + c2 * t2 + c3 * t3 + c4 * t4 + c5 * t5;
            traj[i].velocity     = c1 + T(2) * c2 * t + T(3) * c3 * t2 + T(4) * c4 * t3 + T(5) * c5 * t4;
            traj[i].acceleration = T(2) * c2 + T(6) * c3 * t + T(12) * c4 * t2 + T(20) * c5 * t3;
        }
        return traj;
    }

    // 台形速度プロファイル
    template<typename T>
    std::vector<TrajectoryPoint<T>> trapezoidalProfile(T q0, T qf, T vmax, T amax, int nPoints) {
        if (nPoints < 2 || vmax <= T(0) || amax <= T(0)) {
            throw std::invalid_argument("trapezoidalProfile: invalid parameters");
        }

        const T dir = (qf >= q0) ? T(1) : T(-1);
        const T dist = std::abs(qf - q0);

        // 加速に必要な距離が全体の半分以上 → 三角形プロファイル
        T ta = vmax / amax;                // 加速時間
        T da = T(0.5) * amax * ta * ta;    // 加速区間距離

        T tc, tf;
        if (T(2) * da >= dist) {
            // 三角形 (最大速度に到達しない)
            ta = std::sqrt(dist / amax);
            tc = T(0);
            tf = T(2) * ta;
        } else {
            // 台形
            tc = (dist - T(2) * da) / vmax; // 等速区間時間
            tf = T(2) * ta + tc;
        }

        const T vpeak = amax * ta; // 実際の最大速度

        std::vector<TrajectoryPoint<T>> traj(nPoints);
        for (int i = 0; i < nPoints; ++i) {
            const T t = tf * T(i) / T(nPoints - 1);
            T pos, vel, acc;

            if (t <= ta) {
                // 加速フェーズ
                acc = amax;
                vel = amax * t;
                pos = T(0.5) * amax * t * t;
            } else if (t <= ta + tc) {
                // 等速フェーズ
                acc = T(0);
                vel = vpeak;
                pos = T(0.5) * amax * ta * ta + vpeak * (t - ta);
            } else {
                // 減速フェーズ
                const T td = t - ta - tc;
                acc = -amax;
                vel = vpeak - amax * td;
                pos = T(0.5) * amax * ta * ta + vpeak * tc + vpeak * td - T(0.5) * amax * td * td;
            }

            traj[i].time         = t;
            traj[i].position     = q0 + dir * pos;
            traj[i].velocity     = dir * vel;
            traj[i].acceleration = dir * acc;
        }
        return traj;
    }

    // S-curve (7 セグメント) プロファイル
    template<typename T>
    std::vector<TrajectoryPoint<T>> sCurveProfile(T q0, T qf, T vmax, T amax, T jmax, int nPoints) {
        if (nPoints < 2 || vmax <= T(0) || amax <= T(0) || jmax <= T(0)) {
            throw std::invalid_argument("sCurveProfile: invalid parameters");
        }

        const T dir = (qf >= q0) ? T(1) : T(-1);
        const T dist = std::abs(qf - q0);

        // ジャーク制限による加速時間
        T tj = amax / jmax;               // ジャーク適用時間
        T ta = vmax / amax;               // 加速時間 (ジャークなし近似)

        // 簡易化: tj < ta を仮定して 7 セグメント生成
        if (tj > ta) tj = ta;

        const T aActual = jmax * tj;       // 実際の最大加速度
        const T vReach = aActual * (ta - tj) + jmax * tj * tj / T(2) + jmax * tj * tj / T(2);
        // v_accel = jmax*tj*ta (簡易)
        const T vAccel = std::min(vmax, aActual * ta);

        // 全体時間を台形近似
        const T totalAccelDist = vAccel * ta;
        T tc = T(0);
        if (dist > T(2) * totalAccelDist) {
            tc = (dist - T(2) * totalAccelDist) / vAccel;
        }
        const T tf = T(2) * ta + tc;

        // 台形フォールバック (S-curve の完全な 7 セグメントは複雑なため、
        // ジャーク制限つき台形として近似)
        std::vector<TrajectoryPoint<T>> traj(nPoints);
        for (int i = 0; i < nPoints; ++i) {
            const T t = tf * T(i) / T(nPoints - 1);
            T pos, vel, acc;

            if (t <= tj) {
                // セグメント1: ジャーク増加
                acc = jmax * t;
                vel = T(0.5) * jmax * t * t;
                pos = jmax * t * t * t / T(6);
            } else if (t <= ta - tj) {
                // セグメント2: 等加速
                const T dt = t - tj;
                acc = aActual;
                vel = aActual * tj / T(2) + aActual * dt;
                pos = aActual * tj * tj / T(6) + aActual * tj / T(2) * dt + T(0.5) * aActual * dt * dt;
            } else if (t <= ta) {
                // セグメント3: ジャーク減少
                const T dt = ta - t;
                const T velEnd = vAccel;
                acc = jmax * dt;
                vel = velEnd - T(0.5) * jmax * dt * dt;
                pos = T(0); // 積分で求めるべきだが、ここでは簡易計算
                // 簡易: 位置は台形近似
                pos = vAccel * t / T(2); // 近似
            } else if (t <= ta + tc) {
                // セグメント4: 等速
                const T dt = t - ta;
                acc = T(0);
                vel = vAccel;
                pos = vAccel * ta / T(2) + vAccel * dt;
            } else {
                // セグメント5-7: 減速 (加速の対称)
                const T tDecel = t - ta - tc;
                const T tRemain = ta - tDecel;
                vel = vAccel * tRemain / ta; // 線形近似
                acc = -aActual;
                pos = vAccel * ta / T(2) + vAccel * tc + vAccel * tDecel - vAccel * tDecel * tDecel / (T(2) * ta);
            }

            traj[i].time         = t;
            traj[i].position     = q0 + dir * pos;
            traj[i].velocity     = dir * vel;
            traj[i].acceleration = dir * acc;
        }
        return traj;
    }

} // namespace calx::robotics

#endif // CALX_ROBOTICS_TRAJECTORY_HPP
