#include "agv_model.h"
#include <algorithm>

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

AGVModel::AGVModel(double wheelbase, double max_velocity, double max_steering_angle)
    : wheelbase_(wheelbase)
    , max_velocity_(max_velocity)
    , max_steering_angle_(max_steering_angle) {
}

AGVModel::State AGVModel::derivative(const State& state, const Control& control) const {
    State dstate;

    // 单舵轮AGV运动学方程
    // dx/dt = v * cos(theta)
    // dy/dt = v * sin(theta)
    // dtheta/dt = (v / L) * tan(delta)
    // 其中 L 是轮距，delta 是舵轮转向角

    dstate.x = control.v * std::cos(state.theta);
    dstate.y = control.v * std::sin(state.theta);
    dstate.theta = (control.v / wheelbase_) * std::tan(control.delta);

    return dstate;
}

AGVModel::State AGVModel::update(const State& state, const Control& control, double dt) const {
    // 限制控制输入
    Control clamped_control = clampControl(control);

    // 使用欧拉法更新状态
    State dstate = derivative(state, clamped_control);

    State new_state;
    new_state.x = state.x + dstate.x * dt;
    new_state.y = state.y + dstate.y * dt;
    new_state.theta = state.theta + dstate.theta * dt;

    // 归一化角度到 [-π, π]
    while (new_state.theta > M_PI) new_state.theta -= 2.0 * M_PI;
    while (new_state.theta < -M_PI) new_state.theta += 2.0 * M_PI;

    return new_state;
}

AGVModel::Control AGVModel::clampControl(const Control& control) const {
    Control clamped;

    // 限制速度
    clamped.v = std::max(-max_velocity_, std::min(max_velocity_, control.v));

    // 限制转向角
    clamped.delta = std::max(-max_steering_angle_,
                            std::min(max_steering_angle_, control.delta));

    return clamped;
}
