#ifndef PATH_TRACKER_H
#define PATH_TRACKER_H

#include "agv_model.h"
#include "path_curve.h"
#include "control_generator.h"
#include <string>
#include <fstream>

/**
 * @brief 路径跟踪器
 * 整合模型、路径和控制生成，提供完整的路径跟踪功能
 */
class PathTracker {
public:
    /**
     * @brief 构造函数
     * @param model AGV模型
     */
    explicit PathTracker(const AGVModel& model);

    /**
     * @brief 设置参考路径
     */
    void setReferencePath(const PathCurve& path);

    /**
     * @brief 设置初始状态
     */
    void setInitialState(const AGVModel::State& state);

    /**
     * @brief 生成控制序列
     * @param algorithm 算法类型 (pure_pursuit 或 stanley)
     * @param dt 时间步长
     * @param horizon 时域
     * @param desired_velocity 期望速度 (m/s)
     * @return 是否成功
     */
    bool generateControlSequence(const std::string& algorithm = "pure_pursuit",
                                 double dt = 0.1,
                                 double horizon = 10.0,
                                 double desired_velocity = 1.0);

    /**
     * @brief 获取控制序列
     */
    const ControlSequence& getControlSequence() const { return control_sequence_; }

    /**
     * @brief 打印控制序列到控制台
     */
    void printControlSequence() const;

    /**
     * @brief 保存控制序列到文件
     * @param filename 文件名
     */
    bool saveControlSequence(const std::string& filename) const;

    /**
     * @brief 保存轨迹到文件（用于可视化）
     * @param filename 文件名
     */
    bool saveTrajectory(const std::string& filename) const;

private:
    AGVModel model_;
    PathCurve reference_path_;
    AGVModel::State initial_state_;
    ControlSequence control_sequence_;
    ControlGenerator control_generator_;
};

#endif // PATH_TRACKER_H