import math

import pytest

from hitl.dynamics import DifferentialDriveModel


def test_straight_motion_updates_position():
    model = DifferentialDriveModel()
    model.reset()
    state = model.step(target_linear=1.0, target_angular=0.0, dt=1.0)
    assert state.east == pytest.approx(1.0, rel=0.05)
    assert state.north == pytest.approx(0.0, abs=0.05)
    assert state.heading == pytest.approx(0.0, abs=1e-3)


def test_in_place_rotation_changes_heading():
    model = DifferentialDriveModel()
    model.reset()
    state = model.step(target_linear=0.0, target_angular=math.radians(90.0), dt=1.0)
    assert math.degrees(state.heading) == pytest.approx(90.0, abs=1.0)
    assert abs(state.linear_velocity) < 1e-6


def test_velocity_limits_respected():
    model = DifferentialDriveModel(max_linear_speed=0.5, max_linear_accel=0.5)
    model.reset()
    state = model.step(target_linear=2.0, target_angular=0.0, dt=0.2)
    assert state.linear_velocity <= model.max_linear_speed + 1e-6


def test_body_acceleration_contains_gravity():
    model = DifferentialDriveModel()
    model.reset()
    state = model.step(target_linear=0.0, target_angular=0.0, dt=0.1)
    ax, ay, az = state.body_accel_g
    assert -1.05 < az < -0.95  # 重力方向
    assert abs(ax) < 0.05
    assert abs(ay) < 0.05