"""Phase 2D tests — distance_to_ellipsoid, intersect, isinside.

These functions use CVXPY to solve QCQPs and are therefore somewhat
slower than the closed-form routines.  We keep tolerances generous
(1e-3) because SCS is an iterative solver.
"""

import numpy as np
import pytest

from ellipsoidal_toolbox.core.ellipsoid import Ellipsoid
from ellipsoidal_toolbox.core.hyperplane import Hyperplane
from ellipsoidal_toolbox.calculus.distance import (
    distance_to_ellipsoid,
)
from ellipsoidal_toolbox.calculus.intersection import (
    intersect,
    isinside,
)


# =====================================================================
# distance_to_ellipsoid
# =====================================================================

class TestDistanceToEllipsoid:
    """Tests for the CVXPY-based ellipsoid-to-ellipsoid distance."""

    def test_identical_ellipsoids_distance_zero(self):
        """Two identical ellipsoids overlap; distance = 0."""
        E = Ellipsoid([0, 0], [[4, 0], [0, 1]])
        assert distance_to_ellipsoid(E, E) == pytest.approx(0.0, abs=1e-3)

    def test_concentric_overlapping(self):
        """One inside the other, same center → distance 0."""
        E1 = Ellipsoid([0, 0], [[9, 0], [0, 9]])
        E2 = Ellipsoid([0, 0], [[1, 0], [0, 1]])
        assert distance_to_ellipsoid(E1, E2) == pytest.approx(0.0, abs=1e-3)

    def test_touching_unit_circles(self):
        """Two unit circles touching at one point → distance ≈ 0."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([2, 0], np.eye(2))
        d = distance_to_ellipsoid(E1, E2)
        assert d == pytest.approx(0.0, abs=1e-2)

    def test_separated_circles(self):
        """Two unit circles far apart → positive distance."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([5, 0], np.eye(2))
        d = distance_to_ellipsoid(E1, E2)
        # centers 5 apart, radii 1 each → gap = 3
        assert d == pytest.approx(3.0, abs=0.1)

    def test_overlapping_offset(self):
        """Two overlapping circles → distance 0."""
        E1 = Ellipsoid([0, 0], 4 * np.eye(2))
        E2 = Ellipsoid([1, 0], 4 * np.eye(2))
        assert distance_to_ellipsoid(E1, E2) == pytest.approx(0.0, abs=1e-3)

    def test_3d_separated(self):
        """3D ellipsoids separated along x-axis."""
        E1 = Ellipsoid([0, 0, 0], np.eye(3))
        E2 = Ellipsoid([4, 0, 0], np.eye(3))
        d = distance_to_ellipsoid(E1, E2)
        assert d == pytest.approx(2.0, abs=0.1)

    def test_dimension_mismatch_raises(self):
        """Different dimensions should raise."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([0, 0, 0], np.eye(3))
        with pytest.raises(ValueError, match="dimension mismatch"):
            distance_to_ellipsoid(E1, E2)

    def test_empty_ellipsoid_raises(self):
        """Empty ellipsoids should raise."""
        E1 = Ellipsoid()
        E2 = Ellipsoid([0, 0], np.eye(2))
        with pytest.raises(ValueError, match="non-empty"):
            distance_to_ellipsoid(E1, E2)

    def test_ellipsoidal_metric(self):
        """Ellipsoidal metric returns a value (just smoke test)."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([5, 0], np.eye(2))
        d = distance_to_ellipsoid(E1, E2, metric="ellipsoidal")
        assert d > 0.0

    def test_1d_separated(self):
        """1D intervals (degenerate to line segments) with gap."""
        E1 = Ellipsoid([0], [[1.0]])
        E2 = Ellipsoid([5], [[1.0]])
        d = distance_to_ellipsoid(E1, E2)
        assert d == pytest.approx(3.0, abs=0.1)


# =====================================================================
# intersect — union mode
# =====================================================================

class TestIntersectUnion:
    """Tests for intersect() with mode='u' (union)."""

    def test_single_ellipsoid_overlaps(self):
        """Single ellipsoid overlapping with X → 1."""
        E = Ellipsoid([0, 0], 4 * np.eye(2))
        X = Ellipsoid([1, 0], np.eye(2))
        assert intersect([E], X, mode="u") == 1

    def test_single_ellipsoid_no_overlap(self):
        """Single ellipsoid far from X → 0."""
        E = Ellipsoid([0, 0], np.eye(2))
        X = Ellipsoid([10, 0], np.eye(2))
        assert intersect([E], X, mode="u") == 0

    def test_union_one_hits(self):
        """Two ellipsoids, one overlaps X → union intersects (1)."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([10, 0], np.eye(2))
        X = Ellipsoid([11, 0], np.eye(2))
        assert intersect([E1, E2], X, mode="u") == 1

    def test_union_none_hits(self):
        """Two ellipsoids, neither overlaps X → 0."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([3, 0], np.eye(2))
        X = Ellipsoid([20, 0], np.eye(2))
        assert intersect([E1, E2], X, mode="u") == 0

    def test_union_with_hyperplane(self):
        """Union intersects a hyperplane through the center."""
        E = Ellipsoid([0, 0], 4 * np.eye(2))
        H = Hyperplane([1, 0], 0.0)  # x1 = 0 passes through center
        assert intersect([E], H, mode="u") == 1

    def test_union_hyperplane_no_intersect(self):
        """Union does not reach the hyperplane."""
        E = Ellipsoid([0, 0], np.eye(2))
        H = Hyperplane([1, 0], 5.0)  # x1 = 5, far away
        assert intersect([E], H, mode="u") == 0


# =====================================================================
# intersect — intersection mode (QCQP)
# =====================================================================

class TestIntersectIntersection:
    """Tests for intersect() with mode='i' (intersection via QCQP)."""

    def test_two_overlapping_ellipsoids_intersect_x(self):
        """Intersection of two overlapping ellipsoids contains origin; X at origin → 1."""
        E1 = Ellipsoid([0, 0], 4 * np.eye(2))
        E2 = Ellipsoid([1, 0], 4 * np.eye(2))
        X = Ellipsoid([0.5, 0], 0.01 * np.eye(2))
        assert intersect([E1, E2], X, mode="i") == 1

    def test_two_separated_ellipsoids_infeasible(self):
        """Intersection of two non-overlapping ellipsoids is empty → -1."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([10, 0], np.eye(2))
        X = Ellipsoid([5, 0], np.eye(2))
        assert intersect([E1, E2], X, mode="i") == -1

    def test_intersection_does_not_reach_x(self):
        """Intersection is non-empty but doesn't reach X → 0."""
        E1 = Ellipsoid([0, 0], 4 * np.eye(2))
        E2 = Ellipsoid([1, 0], 4 * np.eye(2))
        X = Ellipsoid([20, 0], np.eye(2))
        assert intersect([E1, E2], X, mode="i") == 0

    def test_intersection_with_hyperplane(self):
        """Intersection of two ellipsoids meets a hyperplane → 1."""
        E1 = Ellipsoid([0, 0], 4 * np.eye(2))
        E2 = Ellipsoid([1, 0], 4 * np.eye(2))
        H = Hyperplane([1, 0], 0.5)  # x1 = 0.5, through the overlap region
        assert intersect([E1, E2], H, mode="i") == 1

    def test_intersection_hyperplane_infeasible(self):
        """Intersection empty, hyperplane test → -1."""
        E1 = Ellipsoid([0, 0], np.eye(2))
        E2 = Ellipsoid([10, 0], np.eye(2))
        H = Hyperplane([1, 0], 0.0)
        assert intersect([E1, E2], H, mode="i") == -1


# =====================================================================
# isinside — union mode
# =====================================================================

class TestIsinsideUnion:
    """Tests for isinside() with mode='u' (union of E2 inside intersection of E1)."""

    def test_small_inside_big(self):
        """Small ellipsoid inside a big one → 1."""
        E_big = Ellipsoid([0, 0], 100 * np.eye(2))
        E_small = Ellipsoid([0, 0], np.eye(2))
        assert isinside([E_big], [E_small], mode="u") == 1

    def test_big_not_inside_small(self):
        """Big ellipsoid does NOT fit inside a small one → 0."""
        E_big = Ellipsoid([0, 0], 100 * np.eye(2))
        E_small = Ellipsoid([0, 0], np.eye(2))
        assert isinside([E_small], [E_big], mode="u") == 0

    def test_offset_not_contained(self):
        """Offset ellipsoid partially outside → 0."""
        E_outer = Ellipsoid([0, 0], 4 * np.eye(2))
        E_inner = Ellipsoid([1.5, 0], np.eye(2))
        assert isinside([E_outer], [E_inner], mode="u") == 0

    def test_concentric_contained(self):
        """Concentric, inner radius smaller → 1."""
        E_outer = Ellipsoid([0, 0], 9 * np.eye(2))
        E_inner = Ellipsoid([0, 0], 4 * np.eye(2))
        assert isinside([E_outer], [E_inner], mode="u") == 1

    def test_multiple_containers_all_must_contain(self):
        """Union of E2 must be inside intersection of E1."""
        E1a = Ellipsoid([0, 0], 100 * np.eye(2))
        E1b = Ellipsoid([0, 0], 50 * np.eye(2))
        E2 = Ellipsoid([0, 0], np.eye(2))
        assert isinside([E1a, E1b], [E2], mode="u") == 1

    def test_multiple_e2_union(self):
        """Both E2 ellipsoids must be inside."""
        E1 = Ellipsoid([0, 0], 100 * np.eye(2))
        E2a = Ellipsoid([0, 0], np.eye(2))
        E2b = Ellipsoid([50, 0], np.eye(2))  # outside E1
        assert isinside([E1], [E2a, E2b], mode="u") == 0


# =====================================================================
# isinside — intersection mode (QCQP)
# =====================================================================

class TestIsinsideIntersection:
    """Tests for isinside() with mode='i' (intersection of E2 inside intersection of E1)."""

    def test_intersection_inside_single_big(self):
        """Intersection of two small overlapping ellipsoids inside a big one → 1."""
        E_big = Ellipsoid([0, 0], 100 * np.eye(2))
        E2a = Ellipsoid([0, 0], 4 * np.eye(2))
        E2b = Ellipsoid([1, 0], 4 * np.eye(2))
        assert isinside([E_big], [E2a, E2b], mode="i") == 1

    def test_intersection_not_inside_small(self):
        """Intersection of two big ellipsoids does not fit inside a tiny one → 0."""
        E_small = Ellipsoid([0, 0], 0.01 * np.eye(2))
        E2a = Ellipsoid([0, 0], 4 * np.eye(2))
        E2b = Ellipsoid([1, 0], 4 * np.eye(2))
        assert isinside([E_small], [E2a, E2b], mode="i") == 0

    def test_single_e2_intersection_mode(self):
        """Single E2 in intersection mode → same as union mode."""
        E_big = Ellipsoid([0, 0], 100 * np.eye(2))
        E_small = Ellipsoid([0, 0], np.eye(2))
        assert isinside([E_big], [E_small], mode="i") == 1


# =====================================================================
# Edge cases
# =====================================================================

class TestEdgeCases:
    """Edge cases and error handling."""

    def test_intersect_bad_mode(self):
        E = Ellipsoid([0, 0], np.eye(2))
        with pytest.raises(ValueError, match="mode must be"):
            intersect([E], E, mode="x")

    def test_isinside_bad_mode(self):
        E = Ellipsoid([0, 0], np.eye(2))
        with pytest.raises(ValueError, match="mode must be"):
            isinside([E], [E], mode="x")

    def test_intersect_empty_raises(self):
        E = Ellipsoid()
        X = Ellipsoid([0, 0], np.eye(2))
        with pytest.raises(ValueError, match="non-empty"):
            intersect([E], X)

    def test_isinside_empty_raises(self):
        E = Ellipsoid()
        X = Ellipsoid([0, 0], np.eye(2))
        with pytest.raises(ValueError, match="non-empty"):
            isinside([E], [X])

    def test_intersect_3d(self):
        """3D smoke test for intersection mode."""
        E1 = Ellipsoid([0, 0, 0], 4 * np.eye(3))
        E2 = Ellipsoid([1, 0, 0], 4 * np.eye(3))
        X = Ellipsoid([0.5, 0, 0], 0.1 * np.eye(3))
        assert intersect([E1, E2], X, mode="i") == 1

    def test_isinside_3d(self):
        """3D smoke test for isinside intersection mode."""
        E_big = Ellipsoid([0, 0, 0], 100 * np.eye(3))
        E2a = Ellipsoid([0, 0, 0], 4 * np.eye(3))
        E2b = Ellipsoid([1, 0, 0], 4 * np.eye(3))
        assert isinside([E_big], [E2a, E2b], mode="i") == 1
