"""Flight kinematics — waypoint patrol and step integration."""

from __future__ import annotations

import random

from virtual_drone.config import Config
from virtual_drone.flight import initial_pose, make_waypoints, step


def test_make_waypoints_returns_closed_perimeter() -> None:
    cfg = Config()
    rng = random.Random(7)
    wps = make_waypoints(cfg, rng)
    assert len(wps) == 4
    assert all(len(p) == 2 for p in wps)


def test_step_moves_toward_target() -> None:
    cfg = Config()
    rng = random.Random(3)
    pose = initial_pose(cfg, rng, 1_000_000_000)
    target = (pose.x + 50.0, pose.y + 50.0)
    next_pose, _ = step(pose, target, cfg, rng, dt=0.1, ts_ns=1_100_000_000)
    dist_before = ((target[0] - pose.x) ** 2 + (target[1] - pose.y) ** 2) ** 0.5
    dist_after = ((target[0] - next_pose.x) ** 2 + (target[1] - next_pose.y) ** 2) ** 0.5
    assert dist_after < dist_before


def test_reached_when_close_to_waypoint() -> None:
    cfg = Config()
    rng = random.Random(1)
    pose = initial_pose(cfg, rng, 2_000_000_000)
    _, reached = step(pose, (pose.x, pose.y), cfg, rng, dt=1.0, ts_ns=2_100_000_000)
    assert reached is True