cmvr-es/src/planner/joint_space_planner/include/toppra_bspline.h

#pragma once

#include <memory>
#include <vector>
#include <Eigen/Dense>

#include "planner/joint_space_planner/include/joint_space_planner.h"
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/parametrizer/const_accel.hpp>
#include <toppra/parametrizer/spline.hpp>

namespace cmvr {
    // 适配器：ConstAccel
    class ConstAccelTraj : public ITrajectory {
    public:
        explicit ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p);

        toppra::Bound timeInterval() const override;

        Eigen::VectorXd q(double t) const override;

        Eigen::VectorXd qd(double t) const override;

        Eigen::VectorXd qdd(double t) const override;

    private:
        std::shared_ptr<toppra::parametrizer::ConstAccel> impl_;
    };

    // 适配器：Spline
    class SplineTraj : public ITrajectory {
    public:
        SplineTraj(const std::shared_ptr<toppra::PiecewisePolyPath> &path,
                   const toppra::Vector &grid,
                   const toppra::Vector &vsq);

        toppra::Bound timeInterval() const override;

        Eigen::VectorXd q(double t) const override;

        Eigen::VectorXd qd(double t) const override;

        Eigen::VectorXd qdd(double t) const override;

    private:
        toppra::parametrizer::Spline impl_;
    };

    // 具体规划器：一次/三次/五次可切换；ConstAccel 校验失败自动回退 Spline
    class ToppraBSpline : public JointSpacePlanner {
    public:
        explicit ToppraBSpline(PathType type = PathType::Quintic);


        // 统一入口：两点/多点皆可
        bool plan(const std::vector<std::vector<double> > &waypoints, TrajPtr &traj_out) override;

        // 兼容旧 API（可选）：转发为两点的统一入口
        bool plan(const std::vector<double> &start_joints,
                  const std::vector<double> &goal_joints,
                  TrajPtr &traj_out) override;

        std::vector<TrajSample> sampleTrajectory(const TrajPtr &traj, double dt) override;

        bool writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) override;

    private:
        // —— 几何路径统一分发 ——
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildPathUnified(const std::vector<Eigen::VectorXd> &q,
                         const std::vector<toppra::value_type> &S);

        //  二点专用
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildTwoPointPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildLinearTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildCubicHermiteTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildQuinticRestToRestTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildNaturalTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);

        // 多点
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildLinearMulti(const std::vector<Eigen::VectorXd> &q,
                         const std::vector<toppra::value_type> &S);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildCubicHermiteMulti(const std::vector<Eigen::VectorXd> &q,
                               const std::vector<toppra::value_type> &S);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildQuinticC2Multi(const std::vector<Eigen::VectorXd> &q,
                            const std::vector<toppra::value_type> &S);

        std::shared_ptr<toppra::PiecewisePolyPath>
        buildNaturalMulti(const std::vector<Eigen::VectorXd> &q,
                          const std::vector<toppra::value_type> &S);

        // —— 工具：限幅/参数/估计 ——
        bool ensureLimitsSized(std::size_t DoF);

        static void sanitizeVsq(toppra::Vector &v);


        // centripetal 弦长（alpha=0.5），生成严格递增 S
        static std::vector<toppra::value_type>
        makeS_centripetal(const std::vector<Eigen::VectorXd> &q) {
            const size_t M = q.size();
            std::vector<toppra::value_type> S(M, 0.0);
            auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) {
                double d = (a - b).norm();
                return std::pow(std::max(d, 1e-16), 0.5);
            };
            for (size_t i = 1; i < M; ++i) {
                S[i] = S[i - 1] + chord(q[i], q[i - 1]);
                if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
            }
            return S;
        }

        // 等距参数（简单稳妥）
        static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
            std::vector<toppra::value_type> S(M);
            for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
            return S;
        }

        // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
        static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
            for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
            double L = S.back();
            if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
            for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
        }

        // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
        static std::vector<Eigen::VectorXd>
        estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
                            const std::vector<toppra::value_type> &S) {
            const size_t M = q.size();
            const int DoF = static_cast<int>(q[0].size());
            std::vector<Eigen::VectorXd> v(M, Eigen::VectorXd::Zero(DoF));
            if (M <= 2) return v;
            for (size_t i = 1; i + 1 < M; ++i) {
                double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
                double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
                v[i] = ((q[i + 1] - q[i]) / ds1 * ds0 + (q[i] - q[i - 1]) / ds0 * ds1) / (ds0 + ds1);
            }
            return v;
        }

        // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]）
        static void clampNodeVels(std::vector<Eigen::VectorXd> &v,
                                  const std::vector<Eigen::VectorXd> &q,
                                  double k = 1.0) {
            const size_t M = q.size();
            if (M <= 2) return;
            for (size_t i = 1; i + 1 < M; ++i) {
                double d0 = (q[i] - q[i - 1]).norm();
                double d1 = (q[i + 1] - q[i]).norm();
                double d = std::max(std::min(d0, d1), 1e-12);
                double vmax = k * d;
                double n = v[i].norm();
                if (n > vmax) v[i] *= (vmax / n);
            }
        }

        // 估计结点几何加速度 a（端点=0；中点二阶差分，按 s 尺度）
        static std::vector<Eigen::VectorXd>
        estimateAccelsSecondDiff(const std::vector<Eigen::VectorXd> &q,
                                 const std::vector<toppra::value_type> &S) {
            const size_t M = q.size();
            const int DoF = static_cast<int>(q[0].size());
            std::vector<Eigen::VectorXd> a(M, Eigen::VectorXd::Zero(DoF));
            if (M <= 2) return a;
            for (size_t i = 1; i + 1 < M; ++i) {
                double h0 = std::max<double>(S[i] - S[i - 1], 1e-12); // 左间距
                double h1 = std::max<double>(S[i + 1] - S[i], 1e-12); // 右间距
                double denom = 0.5 * (h0 + h1); // 局部尺度
                // 非均匀中心二阶差分（更精确）：
                // a ≈ 2 * [ (q_{i+1}-q_i)/h1 - (q_i - q_{i-1})/h0 ] / (h0 + h1)
                a[i] = 2.0 * ((q[i + 1] - q[i]) / h1 - (q[i] - q[i - 1]) / h0) / (h0 + h1);
            }
            return a;
        }


        // τ→s 变元：把局部 Quintic(τ) 的系数 c_tau[0..5]（τ^0..τ^5）
        // 变成全局 s 的系数 alpha[0..5]（s^0..s^5），其中 τ = (s - S_k) / ds
        static inline void localQuinticToGlobalCoeffs(
            const std::array<Eigen::VectorXd, 6> &c_tau, // c0..c5（DoF维向量）
            double Sk, double ds,
            std::array<Eigen::VectorXd, 6> &alpha // α0..α5（DoF维向量）
        ) {
            static const double C[6][6] = {
                // binomial(n,m)
                {1, 0, 0, 0, 0, 0},
                {1, 1, 0, 0, 0, 0},
                {1, 2, 1, 0, 0, 0},
                {1, 3, 3, 1, 0, 0},
                {1, 4, 6, 4, 1, 0},
                {1, 5, 10, 10, 5, 1}
            };
            const double eps = 1e-12;
            ds = std::max(ds, eps);

            for (int m = 0; m <= 5; ++m) alpha[m].setZero(c_tau[0].size());

            // α_m = Σ_{n=m..5} c_n * C(n,m) * (-S_k)^{n-m} / ds^{n}
            for (int n = 0; n <= 5; ++n) {
                double invdsn = std::pow(ds, -n);
                for (int m = 0; m <= n; ++m) {
                    double factor = C[n][m] * std::pow(-Sk, n - m) * invdsn;
                    alpha[m].noalias() += factor * c_tau[n];
                }
            }
        }
    };
} // namespace cmvr