"""Comprehensive tests for Phase 2C — Intersection & Distance operations.

Tests cover:
  2C-i:   is_internal, distance_to_point, distance_to_hyperplane, hpintersection
  2C-ii:  intersection_ea, intersection_ia (ellipsoid-halfspace)
  2C-iii: ellintersection_ia, ellunion_ea (SDP, skipped if CVXPY unavailable)
"""

import numpy as np
import pytest

from ellipsoidal_toolbox.core.ellipsoid import Ellipsoid
from ellipsoidal_toolbox.core.hyperplane import Hyperplane
from ellipsoidal_toolbox.core.config import ABS_TOL
from ellipsoidal_toolbox.calculus.distance import (
    distance_to_point,
    distance_to_hyperplane,
)
from ellipsoidal_toolbox.calculus.intersection import (
    hpintersection,
    intersection_ea,
    intersection_ia,
)

# Conditionally import SDP functions
try:
    import cvxpy  # noqa: F401
    HAS_CVXPY = True
except ImportError:
    HAS_CVXPY = False


# =====================================================================
# Fixtures
# =====================================================================

@pytest.fixture
def unit_circle():
    return Ellipsoid(np.zeros(2), np.eye(2))


@pytest.fixture
def big_circle():
    return Ellipsoid(np.zeros(2), 4.0 * np.eye(2))


@pytest.fixture
def shifted_circle():
    return Ellipsoid([1.0, 0.0], np.eye(2))


@pytest.fixture
def axis_ellipse():
    return Ellipsoid(np.zeros(2), np.diag([4.0, 1.0]))


@pytest.fixture
def unit_sphere_3d():
    return Ellipsoid(np.zeros(3), np.eye(3))


# =====================================================================
# is_internal — point membership
# =====================================================================

class TestIsInternal:
    """Tests for the Ellipsoid.is_internal() method."""

    def test_center_is_internal(self, unit_circle):
        """The center of any ellipsoid should be internal."""
        result = unit_circle.is_internal(np.array([0.0, 0.0]))
        assert result[0]

    def test_boundary_point_is_internal(self, unit_circle):
        """Points on the boundary should be considered internal (<=1)."""
        result = unit_circle.is_internal(np.array([1.0, 0.0]))
        assert result[0]

    def test_exterior_point_not_internal(self, unit_circle):
        """A point outside the ellipsoid should not be internal."""
        result = unit_circle.is_internal(np.array([2.0, 0.0]))
        assert not result[0]

    def test_multiple_points(self, unit_circle):
        """Multiple points as columns of a matrix."""
        X = np.array([[0.0, 0.5, 2.0], [0.0, 0.0, 0.0]])
        result = unit_circle.is_internal(X)
        assert result.shape == (3,)
        assert result[0] and result[1] and not result[2]

    def test_shifted_ellipsoid(self, shifted_circle):
        """Points relative to a non-origin center."""
        assert shifted_circle.is_internal(np.array([1.0, 0.0]))[0]
        assert not shifted_circle.is_internal(np.array([-1.0, 0.0]))[0]

    def test_3d(self, unit_sphere_3d):
        """3D point membership."""
        assert unit_sphere_3d.is_internal(np.array([0.5, 0.5, 0.5]))[0]
        assert not unit_sphere_3d.is_internal(np.array([1.0, 1.0, 1.0]))[0]

    def test_degenerate_ellipsoid_regularized(self):
        """Degenerate ellipsoids are regularized before the check."""
        E = Ellipsoid(np.zeros(2), np.diag([4.0, 0.0]))
        result = E.is_internal(np.array([1.0, 0.0]))
        assert result[0]

    def test_empty_raises(self):
        """Empty ellipsoid raises."""
        with pytest.raises(ValueError, match="empty"):
            Ellipsoid().is_internal(np.array([0.0]))

    def test_dim_mismatch_raises(self, unit_circle):
        """Wrong dimension raises."""
        with pytest.raises(ValueError, match="dimension"):
            unit_circle.is_internal(np.array([1.0, 0.0, 0.0]))


# =====================================================================
# distance_to_point
# =====================================================================

class TestDistanceToPoint:
    """Tests for distance_to_point()."""

    def test_center_negative_distance(self, unit_circle):
        """Distance from ellipsoid to its own center should be negative."""
        d = distance_to_point(unit_circle, np.array([0.0, 0.0]))
        assert d < 0

    def test_boundary_zero_distance(self, unit_circle):
        """Distance to a boundary point should be ≈ 0."""
        d = distance_to_point(unit_circle, np.array([1.0, 0.0]))
        assert abs(d) < 1e-4

    def test_exterior_positive_distance(self, unit_circle):
        """Distance to a point outside should be positive."""
        d = distance_to_point(unit_circle, np.array([3.0, 0.0]))
        assert d > 0
        np.testing.assert_allclose(d, 2.0, atol=1e-4)

    def test_interior_negative_distance(self, big_circle):
        """A point inside E(0,4I) at (1,0): distance to boundary = 1 (radius=2, d=1)."""
        d = distance_to_point(big_circle, np.array([1.0, 0.0]))
        assert d < 0
        np.testing.assert_allclose(d, -1.0, atol=1e-4)

    def test_shifted(self, shifted_circle):
        """E([1,0],I): point (3,0) is 1 unit outside the boundary."""
        d = distance_to_point(shifted_circle, np.array([3.0, 0.0]))
        assert d > 0
        np.testing.assert_allclose(d, 1.0, atol=1e-4)

    def test_3d(self, unit_sphere_3d):
        """Distance from unit sphere to (3,0,0) should be 2."""
        d = distance_to_point(unit_sphere_3d, np.array([3.0, 0.0, 0.0]))
        np.testing.assert_allclose(d, 2.0, atol=1e-4)

    def test_empty_raises(self):
        """Empty ellipsoid raises."""
        with pytest.raises(ValueError, match="non-empty"):
            distance_to_point(Ellipsoid(), np.array([0.0]))

    def test_dim_mismatch_raises(self, unit_circle):
        """Wrong dimension raises."""
        with pytest.raises(ValueError, match="dimension"):
            distance_to_point(unit_circle, np.array([1.0, 0.0, 0.0]))


# =====================================================================
# distance_to_hyperplane
# =====================================================================

class TestDistanceToHyperplane:
    """Tests for distance_to_hyperplane()."""

    def test_tangent_hyperplane_zero(self, unit_circle):
        """Hyperplane x=1 is tangent to the unit circle → distance 0."""
        H = Hyperplane([1.0, 0.0], 1.0)
        d = distance_to_hyperplane(unit_circle, H)
        np.testing.assert_allclose(d, 0.0, atol=1e-6)

    def test_separating_hyperplane_positive(self, unit_circle):
        """Hyperplane x=3 is separated from the unit circle by distance 2."""
        H = Hyperplane([1.0, 0.0], 3.0)
        d = distance_to_hyperplane(unit_circle, H)
        np.testing.assert_allclose(d, 2.0, atol=1e-6)

    def test_intersecting_hyperplane_negative(self, unit_circle):
        """Hyperplane x=0.5 cuts through the unit circle → negative distance."""
        H = Hyperplane([1.0, 0.0], 0.5)
        d = distance_to_hyperplane(unit_circle, H)
        assert d < 0

    def test_hyperplane_through_center(self, unit_circle):
        """Hyperplane through center → distance = -1 (fully intersecting)."""
        H = Hyperplane([1.0, 0.0], 0.0)
        d = distance_to_hyperplane(unit_circle, H)
        assert d < 0

    def test_negative_shift_normalised(self, unit_circle):
        """Negative shift is handled by flipping sign internally."""
        H = Hyperplane([-1.0, 0.0], -3.0)
        d = distance_to_hyperplane(unit_circle, H)
        np.testing.assert_allclose(d, 2.0, atol=1e-6)

    def test_3d(self, unit_sphere_3d):
        """Distance from unit sphere to plane z=5 should be 4."""
        H = Hyperplane([0.0, 0.0, 1.0], 5.0)
        d = distance_to_hyperplane(unit_sphere_3d, H)
        np.testing.assert_allclose(d, 4.0, atol=1e-6)

    def test_empty_ellipsoid_raises(self):
        """Empty ellipsoid raises."""
        with pytest.raises(ValueError, match="non-empty"):
            distance_to_hyperplane(Ellipsoid(), Hyperplane([1, 0], 0))

    def test_empty_hyperplane_raises(self, unit_circle):
        """Empty hyperplane raises."""
        with pytest.raises(ValueError, match="non-empty"):
            distance_to_hyperplane(unit_circle, Hyperplane())


# =====================================================================
# hpintersection — ellipsoid–hyperplane slice
# =====================================================================

class TestHpIntersection:
    """Tests for the exact ellipsoid–hyperplane intersection."""

    def test_unit_circle_x_axis(self, unit_circle):
        """Slicing unit circle with y=0 should give a degenerate ellipsoid
        (line segment from -1 to 1 on the x-axis)."""
        H = Hyperplane([0.0, 1.0], 0.0)
        result = hpintersection(unit_circle, H)
        assert not result.is_empty()
        # Result should be centered near (0,0)
        np.testing.assert_allclose(result.center, [0.0, 0.0], atol=1e-4)

    def test_no_intersection_returns_empty(self, unit_circle):
        """Hyperplane far away → empty result."""
        H = Hyperplane([1.0, 0.0], 5.0)
        result = hpintersection(unit_circle, H)
        assert result.is_empty()

    def test_tangent_returns_point(self, unit_circle):
        """Tangent hyperplane → intersection is a single point (degenerate)."""
        H = Hyperplane([1.0, 0.0], 1.0)
        result = hpintersection(unit_circle, H)
        assert not result.is_empty()
        np.testing.assert_allclose(result.center[0], 1.0, atol=1e-3)

    def test_3d_sphere_slice(self, unit_sphere_3d):
        """Slicing unit sphere with z=0 should give a unit circle in the xy-plane."""
        H = Hyperplane([0.0, 0.0, 1.0], 0.0)
        result = hpintersection(unit_sphere_3d, H)
        assert not result.is_empty()
        np.testing.assert_allclose(result.center[2], 0.0, atol=1e-3)

    def test_result_is_on_hyperplane(self, big_circle):
        """Center of the intersection should satisfy <v, q> ≈ c."""
        H = Hyperplane([1.0, 0.0], 1.0)
        result = hpintersection(big_circle, H)
        if not result.is_empty():
            v, c = H.parameters()
            np.testing.assert_allclose(float(v @ result.center), c, atol=1e-3)

    def test_result_inside_original(self, big_circle):
        """All boundary points of the slice should be inside the original ellipsoid."""
        H = Hyperplane([0.0, 1.0], 0.5)
        result = hpintersection(big_circle, H)
        if not result.is_empty():
            rng = np.random.default_rng(42)
            for _ in range(20):
                l = rng.standard_normal(2)
                _, bp = result.rho(l)
                internal = big_circle.is_internal(bp)
                assert internal[0], f"Boundary point {bp} not inside original"

    def test_empty_inputs_raise(self):
        """Empty inputs raise."""
        with pytest.raises(ValueError, match="non-empty"):
            hpintersection(Ellipsoid(), Hyperplane([1, 0], 0))


# =====================================================================
# intersection_ea — external approximation of intersection
# =====================================================================

class TestIntersectionEA:
    """Tests for the fusion-parameter external intersection approximation."""

    def test_identical_ellipsoids(self, unit_circle):
        """EA of E ∩ E should be E itself."""
        result = intersection_ea(unit_circle, unit_circle)
        assert not result.is_empty()
        np.testing.assert_allclose(result.center, unit_circle.center, atol=1e-4)

    def test_ea_contains_intersection(self, unit_circle, shifted_circle):
        """The EA must contain the true intersection: any point inside both
        E1 and E2 must be inside EA."""
        result = intersection_ea(unit_circle, shifted_circle)
        if not result.is_empty():
            # Sample points from the overlap region
            rng = np.random.default_rng(42)
            for _ in range(100):
                x = rng.uniform(-1, 2, size=2)
                in_E1 = unit_circle.is_internal(x)[0]
                in_E2 = shifted_circle.is_internal(x)[0]
                if in_E1 and in_E2:
                    in_EA = result.is_internal(x)[0]
                    assert in_EA, f"Point {x} in intersection but not in EA"

    def test_ea_with_halfspace(self, unit_circle):
        """EA of ellipsoid intersected with a halfspace."""
        H = Hyperplane([1.0, 0.0], 0.5)
        result = intersection_ea(unit_circle, H)
        assert not result.is_empty()

    def test_nonempty_for_overlapping(self, unit_circle, shifted_circle):
        """Two overlapping circles should give non-empty EA."""
        result = intersection_ea(unit_circle, shifted_circle)
        assert not result.is_empty()

    def test_result_is_psd(self, unit_circle, shifted_circle):
        """Shape matrix of EA must be PSD."""
        result = intersection_ea(unit_circle, shifted_circle)
        if not result.is_empty():
            eigvals = np.linalg.eigvalsh(result.shape)
            assert np.all(eigvals >= -ABS_TOL)

    def test_empty_raises(self):
        """Empty inputs raise."""
        with pytest.raises(ValueError, match="non-empty"):
            intersection_ea(Ellipsoid(), Ellipsoid(np.zeros(2), np.eye(2)))

    def test_3d(self, unit_sphere_3d):
        """3D EA intersection should work."""
        E2 = Ellipsoid([0.5, 0.0, 0.0], np.eye(3))
        result = intersection_ea(unit_sphere_3d, E2)
        assert not result.is_empty()


# =====================================================================
# intersection_ia — internal approximation of intersection
# =====================================================================

class TestIntersectionIA:
    """Tests for the internal intersection approximation."""

    def test_ia_with_halfspace(self, unit_circle):
        """IA with a halfspace that cuts through."""
        H = Hyperplane([1.0, 0.0], 0.0)
        result = intersection_ia(unit_circle, H)
        assert not result.is_empty()

    def test_ia_inside_original(self, big_circle):
        """IA must be inside the original ellipsoid."""
        H = Hyperplane([1.0, 0.0], 0.5)
        result = intersection_ia(big_circle, H)
        if not result.is_empty():
            rng = np.random.default_rng(55)
            for _ in range(20):
                l = rng.standard_normal(2)
                _, bp = result.rho(l)
                assert big_circle.is_internal(bp)[0], \
                    f"IA boundary point {bp} outside original"

    def test_identical_ellipsoids_ia(self, unit_circle):
        """IA of E ∩ E should be E."""
        result = intersection_ia(unit_circle, unit_circle)
        assert not result.is_empty()

    def test_empty_raises(self):
        """Empty inputs raise."""
        with pytest.raises(ValueError, match="non-empty"):
            intersection_ia(Ellipsoid(), Hyperplane([1, 0], 0))


# =====================================================================
# SDP-based tests (require CVXPY)
# =====================================================================

@pytest.mark.skipif(not HAS_CVXPY, reason="CVXPY not installed")
class TestEllIntersectionIA:
    """Tests for ellintersection_ia (SDP: max-volume inscribed ellipsoid)."""

    def test_two_overlapping_circles(self):
        """Max-volume ellipsoid inside the intersection of two overlapping circles."""
        from ellipsoidal_toolbox.calculus.intersection import ellintersection_ia
        E1 = Ellipsoid(np.zeros(2), np.eye(2))
        E2 = Ellipsoid([0.5, 0.0], np.eye(2))
        result = ellintersection_ia([E1, E2])
        assert not result.is_empty()
        # The result must be inside both
        rng = np.random.default_rng(42)
        for _ in range(20):
            l = rng.standard_normal(2)
            _, bp = result.rho(l)
            assert E1.is_internal(bp)[0], f"IA point {bp} outside E1"
            assert E2.is_internal(bp)[0], f"IA point {bp} outside E2"

    def test_concentric_circles(self):
        """Intersection of E(0,4I) and E(0,I) is E(0,I) — IA should ≈ I."""
        from ellipsoidal_toolbox.calculus.intersection import ellintersection_ia
        E1 = Ellipsoid(np.zeros(2), 4.0 * np.eye(2))
        E2 = Ellipsoid(np.zeros(2), np.eye(2))
        result = ellintersection_ia([E1, E2])
        assert not result.is_empty()
        assert result.volume() <= E2.volume() + 0.1

    def test_three_ellipsoids(self):
        """Three-way intersection."""
        from ellipsoidal_toolbox.calculus.intersection import ellintersection_ia
        E1 = Ellipsoid(np.zeros(2), 4.0 * np.eye(2))
        E2 = Ellipsoid([0.5, 0.0], np.eye(2))
        E3 = Ellipsoid([0.0, 0.5], np.eye(2))
        result = ellintersection_ia([E1, E2, E3])
        assert not result.is_empty()

    def test_less_than_two_raises(self):
        """Need at least two ellipsoids."""
        from ellipsoidal_toolbox.calculus.intersection import ellintersection_ia
        with pytest.raises(ValueError, match="at least two"):
            ellintersection_ia([Ellipsoid(np.zeros(2), np.eye(2))])


@pytest.mark.skipif(not HAS_CVXPY, reason="CVXPY not installed")
class TestEllUnionEA:
    """Tests for ellunion_ea (SDP: min-volume bounding ellipsoid)."""

    def test_two_circles(self):
        """Min-volume ellipsoid containing two unit circles."""
        from ellipsoidal_toolbox.calculus.intersection import ellunion_ea
        E1 = Ellipsoid([-1.0, 0.0], np.eye(2))
        E2 = Ellipsoid([1.0, 0.0], np.eye(2))
        result = ellunion_ea([E1, E2])
        assert not result.is_empty()
        # Must contain both
        rng = np.random.default_rng(99)
        for _ in range(30):
            l = rng.standard_normal(2)
            _, bp1 = E1.rho(l)
            _, bp2 = E2.rho(l)
            assert result.is_internal(bp1)[0], f"Union EA doesn't contain E1 boundary {bp1}"
            assert result.is_internal(bp2)[0], f"Union EA doesn't contain E2 boundary {bp2}"

    def test_concentric_returns_outer(self):
        """Union of E(0,I) and E(0,4I) is E(0,4I) — EA should ≈ 4I."""
        from ellipsoidal_toolbox.calculus.intersection import ellunion_ea
        E1 = Ellipsoid(np.zeros(2), np.eye(2))
        E2 = Ellipsoid(np.zeros(2), 4.0 * np.eye(2))
        result = ellunion_ea([E1, E2])
        assert not result.is_empty()
        # Volume should be close to E2's volume
        assert result.volume() >= E2.volume() - 0.5

    def test_less_than_two_raises(self):
        """Need at least two ellipsoids."""
        from ellipsoidal_toolbox.calculus.intersection import ellunion_ea
        with pytest.raises(ValueError, match="at least two"):
            ellunion_ea([Ellipsoid(np.zeros(2), np.eye(2))])


# =====================================================================
# Integration tests
# =====================================================================

class TestIntegration2C:
    """Cross-functional integration tests for Phase 2C."""

    def test_distance_point_consistent_with_is_internal(self, unit_circle):
        """distance < 0 iff is_internal is True."""
        rng = np.random.default_rng(77)
        for _ in range(50):
            pt = rng.uniform(-3, 3, size=2)
            d = distance_to_point(unit_circle, pt)
            internal = unit_circle.is_internal(pt)[0]
            if d < -ABS_TOL:
                assert internal, f"d={d} but not internal: {pt}"
            elif d > ABS_TOL:
                assert not internal, f"d={d} but internal: {pt}"

    def test_hp_distance_consistent_with_hpintersection(self, big_circle):
        """If distance > 0, hpintersection should be empty.
        If distance <= 0, hpintersection should be non-empty."""
        for shift in [0.0, 1.0, 1.9, 2.0, 2.5, 5.0]:
            H = Hyperplane([1.0, 0.0], shift)
            d = distance_to_hyperplane(big_circle, H)
            result = hpintersection(big_circle, H)
            if d > ABS_TOL:
                assert result.is_empty(), f"d={d} but intersection non-empty at shift={shift}"
            elif d < -ABS_TOL:
                assert not result.is_empty(), f"d={d} but intersection empty at shift={shift}"

    def test_ea_ia_volume_ordering(self, unit_circle, shifted_circle):
        """For the same pair, IA volume <= EA volume (when both exist)."""
        ea = intersection_ea(unit_circle, shifted_circle)
        if ea.is_empty():
            return
        # IA via halfspace as a proxy (IA for ell-ell needs CVXPY)
        H = Hyperplane([1.0, 0.0], 0.5)
        ia = intersection_ia(unit_circle, H)
        if not ia.is_empty() and not ea.is_empty():
            # Can't directly compare since they approximate different intersections
            # Just check both are valid ellipsoids
            assert ea.volume() > 0
            assert ia.volume() > 0

    def test_ea_halfspace_contains_hpintersection(self, big_circle):
        """EA of E ∩ halfspace must contain the exact hpintersection slice."""
        H = Hyperplane([1.0, 0.0], 1.0)
        ea = intersection_ea(big_circle, H)
        hp_slice = hpintersection(big_circle, H)
        if not ea.is_empty() and not hp_slice.is_empty():
            rng = np.random.default_rng(33)
            for _ in range(20):
                l = rng.standard_normal(2)
                _, bp = hp_slice.rho(l)
                assert ea.is_internal(bp)[0], \
                    f"HP slice point {bp} not in EA"
