#!/usr/bin/env python3
"""Test UAV control logic."""

import pytest
import numpy as np


def test_waypoint_distance():
    current = np.array([0, 0, 5])
    target = np.array([10, 0, 5])
    distance = np.linalg.norm(target - current)
    assert np.isclose(distance, 10.0)


def test_velocity_command():
    current = np.array([0, 0, 5])
    target = np.array([10, 0, 5])
    
    direction = target - current
    distance = np.linalg.norm(direction)
    direction = direction / distance
    
    max_vel = 2.0
    speed = min(max_vel, distance * 0.5)
    velocity = direction * speed
    
    assert np.isclose(velocity[0], max_vel)
    assert np.isclose(velocity[1], 0.0)


def test_position_hold():
    current = np.array([5.0, 3.0, 10.0])
    target = current.copy()
    
    error = np.linalg.norm(target - current)
    assert np.isclose(error, 0.0)


if __name__ == '__main__':
    pytest.main([__file__, '-v'])