//
// Created by lgv on 11/7/25.
//

#pragma once

#include <vector>
#include "common/consts/constant.h"
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/parametrizer/const_accel.hpp>
#include <toppra/parametrizer/spline.hpp>

namespace cmvr {
    // 统一轨迹接口
    struct ITrajectory {
        virtual ~ITrajectory() = default;
        virtual toppra::Bound timeInterval() const = 0;
        virtual Eigen::VectorXd q(double t)   const = 0;
        virtual Eigen::VectorXd qd(double t)  const = 0;
        virtual Eigen::VectorXd qdd(double t) const = 0;
    };
    using TrajPtr = std::shared_ptr<ITrajectory>;

    struct TrajSample {
        double t{};
        Eigen::VectorXd q;    // DoF x 1
        Eigen::VectorXd qd;   // DoF x 1
        Eigen::VectorXd qdd;  // DoF x 1
    };

    // 三种几何路径
    enum class PathType { Linear, CubicHermite, Quintic };
    class JointSpacePlanner {
    public:

        explicit JointSpacePlanner(PathType p):path_type_(p){};
        JointSpacePlanner()=default;
        virtual ~JointSpacePlanner() = default;

        virtual bool plan(const std::vector<double>& start_joints,
                         const std::vector<double>& goal_joints,
                         TrajPtr& traj) {
            UNUSED_VARIABLE(start_joints, goal_joints,traj);
            return false;
        }

        // 采样函数：从 ITrajectory 生成采样序列 dt(s)
        virtual std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) {
            UNUSED_VARIABLE(traj,dt);
            return {};
        }

        virtual bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) {
            UNUSED_VARIABLE(filename,samples);
            return false;
        }

        // 对称限：[-v_max, v_max]、[-a_max, a_max]
        virtual void setSymmetricLimits(const std::vector<double> &v_max,
                                const std::vector<double> &a_max);

        // 可调网格密度（默认 150/300）
        virtual void setGridSizes(int N, int N_high);

        // 切换几何路径类型
        virtual void setPathType(PathType p);
    protected:
        PathType path_type_{PathType::Quintic};
        int N_grid_ = 150;
        int N_grid_high_ = 300;
        std::vector<double> v_max_, a_max_;

    };
}