"""Unit test della logica pura del move-toward (no I/O)."""
from __future__ import annotations

import numpy as np
import pytest

from presa_mano.right_arm.move import (
    DeltaOversize,
    apply_z_floor,
    compute_delta_wrist,
)


def test_zero_offset_returns_centroid_with_signs():
    delta = compute_delta_wrist(
        centroid_camera_m=np.array([0.0, 0.0, 1.0]),
        offset_camera_m=np.array([0.0, 0.0, 0.0]),
        axis_signs=(1, 1, 1),
    )
    assert np.allclose(delta, [0.0, 0.0, 1.0])


def test_offset_subtracted_before_signs():
    # centroid=(0,0,1) offset=(0,0,0.1) sign=(1,-1,-1) → (0, 0, -0.9)
    delta = compute_delta_wrist(
        centroid_camera_m=np.array([0.0, 0.0, 1.0]),
        offset_camera_m=np.array([0.0, 0.0, 0.1]),
        axis_signs=(1, -1, -1),
    )
    assert np.allclose(delta, [0.0, 0.0, -0.9])


def test_signs_applied_componentwise():
    delta = compute_delta_wrist(
        centroid_camera_m=np.array([0.05, 0.10, 0.20]),
        offset_camera_m=np.array([0.00, 0.00, 0.00]),
        axis_signs=(1, -1, -1),
    )
    assert np.allclose(delta, [0.05, -0.10, -0.20])


def test_oversize_raises_when_not_forced():
    with pytest.raises(DeltaOversize):
        compute_delta_wrist(
            centroid_camera_m=np.array([0.0, 0.0, 0.5]),
            offset_camera_m=np.array([0.0, 0.0, 0.0]),
            axis_signs=(1, 1, 1),
            max_step_m=0.15,
            force=False,
        )


def test_oversize_passes_when_forced():
    delta = compute_delta_wrist(
        centroid_camera_m=np.array([0.0, 0.0, 0.5]),
        offset_camera_m=np.array([0.0, 0.0, 0.0]),
        axis_signs=(1, 1, 1),
        max_step_m=0.15,
        force=True,
    )
    assert np.allclose(delta, [0.0, 0.0, 0.5])


def test_z_floor_clips_when_target_below():
    # wrist_before.z=0.10, delta.z=-0.30, target=-0.20, floor=0.05 → clip a 0.05
    # → delta.z = 0.05 - 0.10 = -0.05
    new_delta, clipped = apply_z_floor(
        wrist_z_m=0.10,
        delta=np.array([0.01, -0.02, -0.30]),
        z_floor_m=0.05,
    )
    assert clipped is True
    assert new_delta[2] == pytest.approx(-0.05)
    # X/Y invariati
    assert new_delta[0] == pytest.approx(0.01)
    assert new_delta[1] == pytest.approx(-0.02)


def test_z_floor_noop_when_target_above():
    new_delta, clipped = apply_z_floor(
        wrist_z_m=0.30,
        delta=np.array([0.0, 0.0, -0.10]),
        z_floor_m=0.05,
    )
    assert clipped is False
    assert np.allclose(new_delta, [0.0, 0.0, -0.10])


def test_z_floor_none_disables_clip():
    new_delta, clipped = apply_z_floor(
        wrist_z_m=0.10,
        delta=np.array([0.0, 0.0, -0.50]),
        z_floor_m=None,
    )
    assert clipped is False
    assert np.allclose(new_delta, [0.0, 0.0, -0.50])