"""Comprehensive tests for Phase 2B — Minkowski operations.

Tests cover:
  - minksum_ea  (external approximation of geometric sum)
  - minksum_ia  (internal approximation of geometric sum)
  - minkdiff_ea (external approximation of geometric difference)
  - minkdiff_ia (internal approximation of geometric difference)
  - isbaddirection (direction validity check for geometric difference)

Each test has a verbose docstring explaining *what* is being verified
and *why* it matters.
"""

import math

import numpy as np
import pytest

from ellipsoidal_toolbox.core.ellipsoid import Ellipsoid
from ellipsoidal_toolbox.core.config import ABS_TOL
from ellipsoidal_toolbox.calculus.minksum import minksum_ea, minksum_ia
from ellipsoidal_toolbox.calculus.minkdiff import (
    minkdiff_ea,
    minkdiff_ia,
    isbaddirection,
)


# =====================================================================
# Fixtures — reusable ellipsoids
# =====================================================================


@pytest.fixture
def unit_circle():
    """Unit circle E(0, I) in R^2."""
    return Ellipsoid(np.zeros(2), np.eye(2))


@pytest.fixture
def big_circle():
    """E(0, 4I) in R^2 — radius 2 in every direction."""
    return Ellipsoid(np.zeros(2), 4 * np.eye(2))


@pytest.fixture
def shifted_circle():
    """E([1,2], I) in R^2."""
    return Ellipsoid([1.0, 2.0], np.eye(2))


@pytest.fixture
def axis_ellipse():
    """E(0, diag(4,1)) in R^2 — semi-axes 2 and 1."""
    return Ellipsoid(np.zeros(2), np.diag([4.0, 1.0]))


@pytest.fixture
def small_circle():
    """E(0, 0.25I) in R^2 — radius 0.5."""
    return Ellipsoid(np.zeros(2), 0.25 * np.eye(2))


@pytest.fixture
def unit_sphere_3d():
    """Unit sphere E(0, I) in R^3."""
    return Ellipsoid(np.zeros(3), np.eye(3))


# =====================================================================
# minksum_ea — external approximation of Minkowski sum
# =====================================================================


class TestMinkSumEA:
    """Tests for the external approximation of the Minkowski sum."""

    def test_single_ellipsoid_passthrough(self, unit_circle):
        """A sum of a single ellipsoid must return that same ellipsoid
        regardless of the direction, since there's nothing to approximate."""
        L = np.array([[1.0], [0.0]])
        result = minksum_ea([unit_circle], L)
        assert len(result) == 1
        assert result[0] == unit_circle

    def test_two_unit_circles_axis_direction(self, unit_circle):
        """Summing two identical unit circles in an axis direction.
        For E(0,I) + E(0,I) with l = [1,0]:
          p1 = p2 = sqrt(<l, I l>) = 1
          Q = (1+1)*((1/1)*I + (1/1)*I) = 2*(2I) = 4I
        The result should be E(0, 4I)."""
        L = np.array([[1.0], [0.0]])
        result = minksum_ea([unit_circle, unit_circle], L)
        assert len(result) == 1
        E = result[0]
        np.testing.assert_allclose(E.center, [0.0, 0.0], atol=1e-12)
        np.testing.assert_allclose(E.shape, 4.0 * np.eye(2), atol=1e-10)

    def test_sum_centers_add(self, unit_circle, shifted_circle):
        """The center of the sum should equal q1 + q2."""
        L = np.array([[1.0], [0.0]])
        result = minksum_ea([unit_circle, shifted_circle], L)
        assert len(result) == 1
        np.testing.assert_allclose(result[0].center, [1.0, 2.0], atol=1e-12)

    def test_multiple_directions(self, unit_circle, shifted_circle):
        """When given k direction columns, must return k ellipsoids."""
        L = np.eye(2)
        result = minksum_ea([unit_circle, shifted_circle], L)
        assert len(result) == 2

    def test_single_direction_vector(self, unit_circle):
        """Passing a 1-D direction vector (not a column matrix) must work."""
        l = np.array([1.0, 0.0])
        result = minksum_ea([unit_circle, unit_circle], l)
        assert len(result) == 1
        np.testing.assert_allclose(result[0].shape, 4.0 * np.eye(2), atol=1e-10)

    def test_support_function_tight(self, unit_circle, axis_ellipse):
        """The external approximation must be *tight* in the given direction:
        rho(l | EA) == rho(l | E1) + rho(l | E2).
        We verify this for l = [1, 0]."""
        l = np.array([1.0, 0.0])
        rho1, _ = unit_circle.rho(l)
        rho2, _ = axis_ellipse.rho(l)
        result = minksum_ea([unit_circle, axis_ellipse], l)
        rho_ea, _ = result[0].rho(l)
        np.testing.assert_allclose(rho_ea, rho1 + rho2, atol=1e-10)

    def test_support_function_tight_diagonal(self, unit_circle, axis_ellipse):
        """Tightness check in a diagonal direction l = [1, 1]/sqrt(2)."""
        l = np.array([1.0, 1.0]) / np.sqrt(2)
        rho1, _ = unit_circle.rho(l)
        rho2, _ = axis_ellipse.rho(l)
        result = minksum_ea([unit_circle, axis_ellipse], l)
        rho_ea, _ = result[0].rho(l)
        np.testing.assert_allclose(rho_ea, rho1 + rho2, atol=1e-10)

    def test_three_ellipsoids(self, unit_circle, shifted_circle, axis_ellipse):
        """Sum of three ellipsoids: centers must add up."""
        L = np.array([[1.0], [0.0]])
        result = minksum_ea([unit_circle, shifted_circle, axis_ellipse], L)
        expected_center = np.array([0.0, 0.0]) + np.array([1.0, 2.0]) + np.array([0.0, 0.0])
        np.testing.assert_allclose(result[0].center, expected_center, atol=1e-12)

    def test_ea_contains_true_sum(self, unit_circle, axis_ellipse):
        """The external approximation must *contain* the true Minkowski sum.
        We verify by sampling random directions: rho(l|EA) >= rho(l|E1)+rho(l|E2)."""
        rng = np.random.default_rng(42)
        L = rng.standard_normal((2, 50))
        for j in range(L.shape[1]):
            l = L[:, j]
            rho1, _ = unit_circle.rho(l)
            rho2, _ = axis_ellipse.rho(l)
            ea = minksum_ea([unit_circle, axis_ellipse], l)[0]
            rho_ea, _ = ea.rho(l)
            assert rho_ea >= rho1 + rho2 - 1e-8, (
                f"EA support {rho_ea} < sum of supports {rho1 + rho2} "
                f"for direction {l}"
            )

    def test_dimension_mismatch_raises(self, unit_circle, unit_sphere_3d):
        """Mixing 2D and 3D ellipsoids must raise."""
        with pytest.raises(ValueError, match="same dimension"):
            minksum_ea([unit_circle, unit_sphere_3d], np.array([1, 0]))

    def test_empty_list_raises(self):
        """An empty list of ellipsoids must raise."""
        with pytest.raises(ValueError, match="at least one"):
            minksum_ea([], np.array([1, 0]))

    def test_empty_ellipsoid_raises(self, unit_circle):
        """Including an empty ellipsoid must raise."""
        with pytest.raises(ValueError, match="non-empty"):
            minksum_ea([unit_circle, Ellipsoid()], np.array([[1], [0]]))

    def test_direction_dim_mismatch(self, unit_circle):
        """3-D directions with 2-D ellipsoids must raise."""
        with pytest.raises(ValueError, match="R\\^2"):
            minksum_ea([unit_circle], np.array([1, 0, 0]))


# =====================================================================
# minksum_ia — internal approximation of Minkowski sum
# =====================================================================


class TestMinkSumIA:
    """Tests for the internal approximation of the Minkowski sum."""

    def test_single_ellipsoid_passthrough(self, unit_circle):
        """Internal approximation of a single ellipsoid is itself."""
        L = np.array([[0.0], [1.0]])
        result = minksum_ia([unit_circle], L)
        assert len(result) == 1
        assert result[0] == unit_circle

    def test_two_unit_circles(self, unit_circle):
        """For two identical unit circles and any direction, the internal
        approximation should equal the external approximation (both are tight
        for identical ellipsoids). For l=[1,0]: Q_ia = M^T M with
        M = I + I = 2I, so Q = 4I."""
        l = np.array([1.0, 0.0])
        result = minksum_ia([unit_circle, unit_circle], l)
        assert len(result) == 1
        np.testing.assert_allclose(result[0].shape, 4.0 * np.eye(2), atol=1e-10)

    def test_centers_add(self, unit_circle, shifted_circle):
        """Center of internal approximation must equal sum of centers."""
        L = np.array([[1.0], [0.0]])
        result = minksum_ia([unit_circle, shifted_circle], L)
        np.testing.assert_allclose(result[0].center, [1.0, 2.0], atol=1e-12)

    def test_support_function_tight(self, unit_circle, axis_ellipse):
        """Internal approximation must also be tight in the given direction:
        rho(l | IA) == rho(l | E1) + rho(l | E2)."""
        l = np.array([1.0, 0.0])
        rho1, _ = unit_circle.rho(l)
        rho2, _ = axis_ellipse.rho(l)
        result = minksum_ia([unit_circle, axis_ellipse], l)
        rho_ia, _ = result[0].rho(l)
        np.testing.assert_allclose(rho_ia, rho1 + rho2, atol=1e-8)

    def test_ia_inside_true_sum(self, unit_circle, axis_ellipse):
        """The internal approximation must be *inside* the true Minkowski sum.
        Verify: rho(l|IA) <= rho(l|E1)+rho(l|E2) for random directions."""
        rng = np.random.default_rng(123)
        for _ in range(50):
            l = rng.standard_normal(2)
            rho1, _ = unit_circle.rho(l)
            rho2, _ = axis_ellipse.rho(l)
            ia = minksum_ia([unit_circle, axis_ellipse], l)[0]
            rho_ia, _ = ia.rho(l)
            assert rho_ia <= rho1 + rho2 + 1e-8, (
                f"IA support {rho_ia} > sum {rho1 + rho2} for dir {l}"
            )

    def test_ia_vs_ea_volume(self, unit_circle, axis_ellipse):
        """Internal approximation volume should be <= external approximation
        volume for the same direction."""
        l = np.array([1.0, 1.0]) / np.sqrt(2)
        ea = minksum_ea([unit_circle, axis_ellipse], l)[0]
        ia = minksum_ia([unit_circle, axis_ellipse], l)[0]
        assert ia.volume() <= ea.volume() + 1e-8

    def test_multiple_directions(self, unit_circle, shifted_circle):
        """k direction columns => k output ellipsoids."""
        L = np.eye(2)
        result = minksum_ia([unit_circle, shifted_circle], L)
        assert len(result) == 2

    def test_three_ellipsoids_tight(self):
        """For three identical unit circles, internal and external should
        coincide (both tight) for any direction, giving Q = 9I."""
        E = Ellipsoid(np.zeros(2), np.eye(2))
        l = np.array([0.0, 1.0])
        ea = minksum_ea([E, E, E], l)[0]
        ia = minksum_ia([E, E, E], l)[0]
        np.testing.assert_allclose(ea.shape, 9.0 * np.eye(2), atol=1e-8)
        np.testing.assert_allclose(ia.shape, 9.0 * np.eye(2), atol=1e-8)

    def test_3d_ellipsoids(self, unit_sphere_3d):
        """Works in 3-D."""
        l = np.array([1.0, 0.0, 0.0])
        result = minksum_ia([unit_sphere_3d, unit_sphere_3d], l)
        assert len(result) == 1
        np.testing.assert_allclose(result[0].shape, 4.0 * np.eye(3), atol=1e-8)


# =====================================================================
# isbaddirection
# =====================================================================


class TestIsBadDirection:
    """Tests for the bad-direction checker used in Minkowski difference."""

    def test_all_good_when_much_bigger(self, big_circle, small_circle):
        """E(0,4I) >> E(0,0.25I), so no direction should be bad."""
        L = np.eye(2)
        bad = isbaddirection(big_circle, small_circle, L)
        assert not np.any(bad)

    def test_all_bad_when_smaller(self, small_circle, big_circle):
        """E(0,0.25I) < E(0,4I), so the difference is empty → all bad."""
        L = np.eye(2)
        bad = isbaddirection(small_circle, big_circle, L)
        assert np.all(bad)

    def test_equal_ellipsoids_all_bad(self, unit_circle):
        """E - E: equal ellipsoids have generalised eigenvalue = 1 everywhere,
        so the strict inequality a > p never holds → all directions are bad."""
        L = np.eye(2)
        bad = isbaddirection(unit_circle, unit_circle, L)
        assert np.all(bad)

    def test_axis_ellipse_vs_small(self, axis_ellipse, small_circle):
        """E(0, diag(4,1)) vs E(0, 0.25I): should have some good directions
        (at least along the major axis)."""
        L = np.array([[1.0, 0.0], [0.0, 1.0]])
        bad = isbaddirection(axis_ellipse, small_circle, L)
        assert not np.all(bad), "Expected at least one good direction."

    def test_single_direction_vector(self, big_circle, small_circle):
        """Passing a 1-D vector instead of column matrix."""
        l = np.array([1.0, 0.0])
        bad = isbaddirection(big_circle, small_circle, l)
        assert bad.shape == (1,)
        assert not bad[0]

    def test_empty_ellipsoid_all_bad(self, unit_circle):
        """If either operand is empty, all directions are bad."""
        L = np.array([[1.0], [0.0]])
        bad = isbaddirection(unit_circle, Ellipsoid(), L)
        assert np.all(bad)

    def test_dimension_mismatch_raises(self, big_circle, small_circle):
        """3-D directions with 2-D ellipsoids must raise."""
        with pytest.raises(ValueError, match="dimension"):
            isbaddirection(big_circle, small_circle, np.array([[1], [0], [0]]))


# =====================================================================
# minkdiff_ea — external approximation of Minkowski difference
# =====================================================================


class TestMinkDiffEA:
    """Tests for the external approximation of the geometric difference."""

    def test_empty_when_not_bigger(self, unit_circle, big_circle):
        """If E1 is not bigger than E2, result must be empty list."""
        L = np.eye(2)
        result = minkdiff_ea(unit_circle, big_circle, L)
        assert result == []

    def test_diff_big_minus_small(self, big_circle, small_circle):
        """E(0,4I) - E(0,0.25I) should produce non-empty results."""
        L = np.eye(2)
        result = minkdiff_ea(big_circle, small_circle, L)
        assert len(result) > 0
        for ea in result:
            assert not ea.is_empty()

    def test_center_is_difference(self):
        """Center of the result must be q1 - q2."""
        E1 = Ellipsoid([3.0, 4.0], 4.0 * np.eye(2))
        E2 = Ellipsoid([1.0, 1.0], 0.25 * np.eye(2))
        L = np.array([[1.0], [0.0]])
        result = minkdiff_ea(E1, E2, L)
        if len(result) > 0:
            np.testing.assert_allclose(result[0].center, [2.0, 3.0], atol=1e-12)

    def test_support_tight_in_direction(self, big_circle, small_circle):
        """rho(l | EA) must equal rho(l | E1) - rho(l | E2) in direction l.
        For E(0,4I) and E(0,0.25I) with l=[1,0]:
          rho(l|E1) = sqrt(4) = 2, rho(l|E2) = sqrt(0.25) = 0.5
          Expected: rho(l|EA) = 1.5."""
        l = np.array([1.0, 0.0])
        result = minkdiff_ea(big_circle, small_circle, l)
        if len(result) > 0:
            rho1, _ = big_circle.rho(l)
            rho2, _ = small_circle.rho(l)
            rho_ea, _ = result[0].rho(l)
            np.testing.assert_allclose(rho_ea, rho1 - rho2, atol=1e-8)

    def test_ea_contains_true_diff(self, big_circle, small_circle):
        """EA must contain the true difference in all directions:
        rho(l|EA) >= rho(l|E1) - rho(l|E2) for random l."""
        rng = np.random.default_rng(77)
        for _ in range(50):
            l = rng.standard_normal(2)
            rho1, _ = big_circle.rho(l)
            rho2, _ = small_circle.rho(l)
            ea_list = minkdiff_ea(big_circle, small_circle, l)
            if len(ea_list) > 0:
                rho_ea, _ = ea_list[0].rho(l)
                assert rho_ea >= rho1 - rho2 - 1e-8

    def test_multiple_directions_filters_bad(self, axis_ellipse, small_circle):
        """Some directions may be filtered out as bad. The number of results
        should be <= number of input directions."""
        rng = np.random.default_rng(99)
        L = rng.standard_normal((2, 20))
        result = minkdiff_ea(axis_ellipse, small_circle, L)
        assert len(result) <= 20

    def test_empty_operand_raises(self, unit_circle):
        """Empty operands must raise."""
        with pytest.raises(ValueError, match="non-empty"):
            minkdiff_ea(unit_circle, Ellipsoid(), np.eye(2))

    def test_dimension_mismatch_raises(self, unit_circle, unit_sphere_3d):
        """Mismatched dimensions must raise."""
        with pytest.raises(ValueError, match="same dimension"):
            minkdiff_ea(unit_circle, unit_sphere_3d, np.eye(2))

    def test_result_is_psd(self, big_circle, small_circle):
        """Shape matrices of all returned ellipsoids must be PSD."""
        L = np.eye(2)
        for ea in minkdiff_ea(big_circle, small_circle, L):
            eigvals = np.linalg.eigvalsh(ea.shape)
            assert np.all(eigvals >= -ABS_TOL), f"Negative eigenvalue: {eigvals}"


# =====================================================================
# minkdiff_ia — internal approximation of Minkowski difference
# =====================================================================


class TestMinkDiffIA:
    """Tests for the internal approximation of the geometric difference."""

    def test_empty_when_not_bigger(self, unit_circle, big_circle):
        """If E1 is not bigger than E2, result must be empty list."""
        L = np.eye(2)
        result = minkdiff_ia(unit_circle, big_circle, L)
        assert result == []

    def test_diff_big_minus_small(self, big_circle, small_circle):
        """E(0,4I) - E(0,0.25I) should produce non-empty results."""
        L = np.eye(2)
        result = minkdiff_ia(big_circle, small_circle, L)
        assert len(result) > 0

    def test_center_is_difference(self):
        """Center of IA must also be q1 - q2."""
        E1 = Ellipsoid([5.0, 6.0], 4.0 * np.eye(2))
        E2 = Ellipsoid([1.0, 2.0], 0.25 * np.eye(2))
        L = np.array([[1.0], [0.0]])
        result = minkdiff_ia(E1, E2, L)
        if len(result) > 0:
            np.testing.assert_allclose(result[0].center, [4.0, 4.0], atol=1e-12)

    def test_support_tight_in_direction(self, big_circle, small_circle):
        """rho(l | IA) must equal rho(l | E1) - rho(l | E2) in direction l.
        Both EA and IA are tight in the approximation direction."""
        l = np.array([0.0, 1.0])
        result = minkdiff_ia(big_circle, small_circle, l)
        if len(result) > 0:
            rho1, _ = big_circle.rho(l)
            rho2, _ = small_circle.rho(l)
            rho_ia, _ = result[0].rho(l)
            np.testing.assert_allclose(rho_ia, rho1 - rho2, atol=1e-8)

    def test_ia_inside_true_diff(self, big_circle, small_circle):
        """IA must be inside the true difference:
        rho(l|IA) <= rho(l|E1) - rho(l|E2) for random l."""
        rng = np.random.default_rng(55)
        for _ in range(50):
            l = rng.standard_normal(2)
            rho1, _ = big_circle.rho(l)
            rho2, _ = small_circle.rho(l)
            ia_list = minkdiff_ia(big_circle, small_circle, l)
            if len(ia_list) > 0:
                rho_ia, _ = ia_list[0].rho(l)
                assert rho_ia <= rho1 - rho2 + 1e-8

    def test_ia_volume_le_ea_volume(self, big_circle, small_circle):
        """For the same direction, IA volume should be <= EA volume."""
        l = np.array([1.0, 0.0])
        ea = minkdiff_ea(big_circle, small_circle, l)
        ia = minkdiff_ia(big_circle, small_circle, l)
        if ea and ia:
            assert ia[0].volume() <= ea[0].volume() + 1e-8

    def test_empty_operand_raises(self, unit_circle):
        """Empty operands must raise."""
        with pytest.raises(ValueError, match="non-empty"):
            minkdiff_ia(Ellipsoid(), unit_circle, np.eye(2))

    def test_result_is_psd(self, big_circle, small_circle):
        """Shape matrices of all returned ellipsoids must be PSD."""
        L = np.eye(2)
        for ia in minkdiff_ia(big_circle, small_circle, L):
            eigvals = np.linalg.eigvalsh(ia.shape)
            assert np.all(eigvals >= -ABS_TOL), f"Negative eigenvalue: {eigvals}"


# =====================================================================
# Integration tests — combining sum and difference
# =====================================================================


class TestIntegration:
    """Cross-functional tests verifying consistency between operations."""

    def test_sum_then_diff_recovers_shape_isotropic(self):
        """For isotropic ellipsoids (balls), E + F - F should recover E's
        shape (up to numerical tolerance) if the result exists.
        E(0,4I) + E(0,I) gives a sum; then subtracting E(0,I) from it
        (using the external approximation of the sum as E1) should
        recover something close to E(0,4I)."""
        E = Ellipsoid(np.zeros(2), 4.0 * np.eye(2))
        F = Ellipsoid(np.zeros(2), np.eye(2))
        l = np.array([1.0, 0.0])

        # E + F (external approx for isotropic = exact)
        ea_sum = minksum_ea([E, F], l)[0]

        # (E + F) - F
        diff_list = minkdiff_ea(ea_sum, F, l)
        if diff_list:
            rho_diff, _ = diff_list[0].rho(l)
            rho_E, _ = E.rho(l)
            np.testing.assert_allclose(rho_diff, rho_E, atol=1e-6)

    def test_ea_always_outer_ia_always_inner_sum(self):
        """For the same direction, EA support >= true sum >= IA support."""
        E1 = Ellipsoid([1.0, 0.0], np.diag([4.0, 1.0]))
        E2 = Ellipsoid([0.0, 1.0], np.diag([1.0, 9.0]))
        rng = np.random.default_rng(999)
        for _ in range(30):
            l = rng.standard_normal(2)
            ea = minksum_ea([E1, E2], l)[0]
            ia = minksum_ia([E1, E2], l)[0]
            rho_ea, _ = ea.rho(l)
            rho_ia, _ = ia.rho(l)
            rho1, _ = E1.rho(l)
            rho2, _ = E2.rho(l)
            true_sum = rho1 + rho2
            assert rho_ea >= true_sum - 1e-8, "EA not outer"
            assert rho_ia <= true_sum + 1e-8, "IA not inner"

    def test_3d_minksum_and_diff(self):
        """Smoke test: sum and difference in 3-D."""
        E1 = Ellipsoid(np.zeros(3), 9.0 * np.eye(3))
        E2 = Ellipsoid(np.zeros(3), np.eye(3))
        l = np.array([1.0, 0.0, 0.0])

        ea_sum = minksum_ea([E1, E2], l)
        ia_sum = minksum_ia([E1, E2], l)
        assert len(ea_sum) == 1
        assert len(ia_sum) == 1

        ea_diff = minkdiff_ea(E1, E2, l)
        ia_diff = minkdiff_ia(E1, E2, l)
        assert len(ea_diff) > 0
        assert len(ia_diff) > 0

    def test_symmetric_sum_directions(self):
        """For symmetric (isotropic) ellipsoids, the sum approximation
        should not depend on the direction. All results should be identical."""
        E = Ellipsoid(np.zeros(2), np.eye(2))
        F = Ellipsoid(np.zeros(2), 4.0 * np.eye(2))
        L = np.array([[1.0, 0.0, 1.0], [0.0, 1.0, 1.0]])
        # Normalise directions for comparability
        for j in range(L.shape[1]):
            L[:, j] /= np.linalg.norm(L[:, j])
        ea_list = minksum_ea([E, F], L)
        shapes = [e.shape for e in ea_list]
        for s in shapes[1:]:
            np.testing.assert_allclose(s, shapes[0], atol=1e-10)

    def test_degenerate_ellipsoid_sum(self):
        """Summing involving a degenerate (rank-deficient) ellipsoid.
        The regularisation should prevent crashes."""
        Q_degen = np.diag([4.0, 0.0])
        E_degen = Ellipsoid(np.zeros(2), Q_degen)
        E_normal = Ellipsoid(np.zeros(2), np.eye(2))
        l = np.array([1.0, 0.0])

        ea = minksum_ea([E_degen, E_normal], l)
        assert len(ea) == 1
        assert not ea[0].is_empty()

        ia = minksum_ia([E_degen, E_normal], l)
        assert len(ia) == 1
        assert not ia[0].is_empty()
