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

import pytest
import numpy as np


def test_differential_drive():
    target_angle = np.pi / 4
    current_yaw = 0
    
    angle_error = target_angle - current_yaw
    kp_angular = 1.5
    max_angular = 1.0
    
    angular_cmd = np.clip(kp_angular * angle_error, -max_angular, max_angular)
    assert np.isclose(angular_cmd, 1.0)


def test_angle_normalization():
    def normalize(angle):
        while angle > np.pi:
            angle -= 2 * np.pi
        while angle < -np.pi:
            angle += 2 * np.pi
        return angle
    
    assert np.isclose(normalize(3 * np.pi), np.pi)
    assert np.isclose(normalize(-3 * np.pi), np.pi)


def test_goal_distance_2d():
    current = np.array([0, 0])
    target = np.array([3, 4])
    
    distance = np.linalg.norm(target - current)
    assert np.isclose(distance, 5.0)


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