from pathlib import Path
import sys

import numpy as np


PROJECT_ROOT = Path(__file__).resolve().parents[1]
if str(PROJECT_ROOT) not in sys.path:
    sys.path.insert(0, str(PROJECT_ROOT))

from guidance import ScreenGuidanceController


def test_guidance_reset_reinitializes_smoothed_point():
    ctrl = ScreenGuidanceController()
    ctrl.smoothed_point = np.array([100.0, 200.0], dtype=np.float32)
    ctrl.smoothed_cmd = np.array([0.7, -0.4], dtype=np.float32)

    ctrl.reset_for_target_switch(np.array([500.0, 300.0], dtype=np.float32))

    assert np.allclose(ctrl.smoothed_point, np.array([500.0, 300.0], dtype=np.float32))
    assert np.allclose(ctrl.smoothed_cmd, np.zeros(2, dtype=np.float32))


def test_guidance_reset_can_damp_instead_of_zero_when_requested():
    ctrl = ScreenGuidanceController()
    ctrl.smoothed_cmd = np.array([0.8, -0.6], dtype=np.float32)

    ctrl.reset_for_target_switch(np.array([10.0, 20.0], dtype=np.float32), cmd_damp=0.25)

    assert np.allclose(ctrl.smoothed_cmd, np.array([0.2, -0.15], dtype=np.float32))


def test_guidance_override_box_takes_priority_over_stale_locked_box():
    ctrl = ScreenGuidanceController()

    state = ctrl.update(
        frame_id=1,
        frame_w=1000,
        frame_h=600,
        confirmed=True,
        locked_box=np.array([50.0, 50.0, 150.0, 150.0], dtype=np.float32),
        pred_box=np.array([60.0, 60.0, 160.0, 160.0], dtype=np.float32),
        override_box=np.array([800.0, 100.0, 900.0, 200.0], dtype=np.float32),
        target_id=1,
        target_track_score=0.55,
        klt_valid=True,
        klt_quality=0.9,
        miss_streak=5,
        vx=0.0,
        vy=0.0,
    )

    assert state["status"] == "REACQ"
    assert 845.0 <= state["aim_x"] <= 855.0
    assert 145.0 <= state["aim_y"] <= 155.0
    assert state["box_w"] == 100.0
    assert state["box_h"] == 100.0