#ifndef AGV_MODEL_H
#define AGV_MODEL_H

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/**
 * @brief 单舵轮AGV运动学模型
 *
 * 模型描述：前面一个主舵轮，后面两个从动轮
 * 状态量：x, y, theta (位置和航向角)
 * 控制量：v (速度), delta (主舵轮转向角)
 */
class AGVModel {
public:
    /**
     * @brief AGV状态结构体
     */
    struct State {
        double x;       // x坐标 (m)
        double y;       // y坐标 (m)
        double theta;   // 航向角 (rad)

        State(double x_ = 0.0, double y_ = 0.0, double theta_ = 0.0)
            : x(x_), y(y_), theta(theta_) {}
    };

    /**
     * @brief 控制输入结构体
     */
    struct Control {
        double v;       // 速度 (m/s)
        double delta;   // 主舵轮转向角 (rad)

        Control(double v_ = 0.0, double delta_ = 0.0)
            : v(v_), delta(delta_) {}
    };

    /**
     * @brief 构造函数
     * @param wheelbase 轮距（主舵轮到后轴中心的距离）(m)
     * @param max_velocity 最大速度 (m/s)
     * @param max_steering_angle 最大转向角 (rad)
     */
    AGVModel(double wheelbase = 1.0,
             double max_velocity = 2.0,
             double max_steering_angle = M_PI / 4);

    /**
     * @brief 运动学方程，计算状态导数
     * @param state 当前状态
     * @param control 控制输入
     * @return 状态导数
     */
    State derivative(const State& state, const Control& control) const;

    /**
     * @brief 使用欧拉法更新状态
     * @param state 当前状态
     * @param control 控制输入
     * @param dt 时间步长 (s)
     * @return 更新后的状态
     */
    State update(const State& state, const Control& control, double dt) const;

    /**
     * @brief 限制控制输入在允许范围内
     * @param control 控制输入
     * @return 限制后的控制输入
     */
    Control clampControl(const Control& control) const;

    // 获取参数
    double getWheelbase() const { return wheelbase_; }
    double getMaxVelocity() const { return max_velocity_; }
    double getMaxSteeringAngle() const { return max_steering_angle_; }

private:
    double wheelbase_;              // 轮距 (m)
    double max_velocity_;           // 最大速度 (m/s)
    double max_steering_angle_;     // 最大转向角 (rad)
};

#endif // AGV_MODEL_H