#ifndef CONTROL_GENERATOR_H
#define CONTROL_GENERATOR_H

#include "agv_model.h"
#include "path_curve.h"
#include <vector>

/**
 * @brief 控制序列结构体
 */
struct ControlSequence {
    std::vector<AGVModel::Control> controls;    // 控制量数组
    std::vector<double> timestamps;              // 时间戳数组
    std::vector<AGVModel::State> predicted_states; // 预测状态轨迹

    void clear() {
        controls.clear();
        timestamps.clear();
        predicted_states.clear();
    }

    size_t size() const {
        return controls.size();
    }
};

/**
 * @brief 控制序列生成器
 * 根据给定的路径曲线生成控制序列
 */
class ControlGenerator {
public:
    /**
     * @brief 构造函数
     * @param model AGV运动学模型
     */
    explicit ControlGenerator(const AGVModel& model);

    /**
     * @brief 生成控制序列（基本跟踪方法）
     * @param path 参考路径
     * @param initial_state 初始状态
     * @param dt 时间步长 (s)
     * @param horizon 预测时域
     * @return 控制序列
     */
    ControlSequence generate(const PathCurve& path,
                            const AGVModel::State& initial_state,
                            double dt = 0.1,
                            double horizon = 10.0);

    /**
     * @brief 使用Pure Pursuit算法生成控制序列
     * @param path 参考路径
     * @param initial_state 初始状态
     * @param dt 时间步长 (s)
     * @param lookahead_distance 前视距离 (m)
     * @param desired_velocity 期望速度 (m/s)
     * @param horizon 预测时域
     * @return 控制序列
     */
    ControlSequence generatePurePursuit(const PathCurve& path,
                                       const AGVModel::State& initial_state,
                                       double dt = 0.1,
                                       double lookahead_distance = 1.5,
                                       double desired_velocity = 1.0,
                                       double horizon = 10.0);

    /**
     * @brief 使用Stanley算法生成控制序列
     * @param path 参考路径
     * @param initial_state 初始状态
     * @param dt 时间步长 (s)
     * @param k_gain 增益系数
     * @param desired_velocity 期望速度 (m/s)
     * @param horizon 预测时域
     * @return 控制序列
     */
    ControlSequence generateStanley(const PathCurve& path,
                                   const AGVModel::State& initial_state,
                                   double dt = 0.1,
                                   double k_gain = 1.0,
                                   double desired_velocity = 1.0,
                                   double horizon = 10.0);

private:
    AGVModel model_;

    /**
     * @brief Pure Pursuit算法计算单步控制
     */
    AGVModel::Control computePurePursuitControl(const AGVModel::State& state,
                                               const PathPoint& target_point,
                                               double desired_velocity);

    /**
     * @brief Stanley算法计算单步控制
     */
    AGVModel::Control computeStanleyControl(const AGVModel::State& state,
                                           const PathPoint& nearest_point,
                                           double k_gain,
                                           double desired_velocity);

    /**
     * @brief 找到前视点
     */
    PathPoint findLookaheadPoint(const PathCurve& path,
                                const AGVModel::State& state,
                                double lookahead_distance) const;

    /**
     * @brief 计算角度差（归一化到[-π, π]）
     */
    double normalizeAngle(double angle) const;
};

#endif // CONTROL_GENERATOR_H