#ifndef INTERPOLATION_BASE_H
#define INTERPOLATION_BASE_H

#include "utils/math/geometry.h"
#include <vector>
#include <stdexcept>

namespace cmvr::math {

    /**
     * 插值基类，用于机械臂位姿插值
     * 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值
     */
    class AbstractInterpolation {
    public:
        // 构造函数
        AbstractInterpolation(const std::vector<Pose3d>& waypoints);
        AbstractInterpolation(const std::vector<Pose2d>& waypoints);

        // 析构函数
        virtual ~AbstractInterpolation() = default;

        /**
         * 插值计算接口
         * @param t 插值参数，范围[0, 1]，0表示起点，1表示终点
         * @return 插值得到的位姿
         */
        virtual Pose3d interpolate_3d(double t) const = 0;
        virtual Pose2d interpolate_2d(double t) const = 0;

        /**
         * 生成轨迹点序列
         * @param num_points 生成的轨迹点数量，包括起点和终点
         * @return 轨迹点序列
         */
        std::vector<Pose3d> generate_trajectory_3d(size_t num_points) const;
        std::vector<Pose2d> generate_trajectory_2d(size_t num_points) const;

    protected:
        // 路径点存储
        std::vector<Pose3d> waypoints_3d_;
        std::vector<Pose2d> waypoints_2d_;

        /**
         * 辅助函数：线性插值单个数值
         * @param a 起始值
         * @param b 目标值
         * @param t 插值参数[0,1]
         * @return 插值结果
         */
        static double linear_interpolate(double a, double b, double t);

        /**
         * 辅助函数：四元数球面线性插值(slerp)
         * @param q1 起始四元数
         * @param q2 目标四元数
         * @param t 插值参数[0,1]
         * @return 插值结果
         */
        static Quat slerp(const Quat& q1, const Quat& q2, double t);

        /**
         * 检查t的范围并进行约束
         * @param t 原始参数
         * @return 约束在[0,1]范围内的参数
         */
        static double clamp_t(double t);
    };

} // namespace cmvr::math

#endif // INTERPOLATION_BASE_H