#!/usr/bin/env python3
"""Compute exact coordinates for the convex-decomposition schematic.

Follows supplement.tex S-IV verbatim:

  eq:sup_ellipsoid_quadratic  E = { p | (p-d)^T (C C^T)^{-1} (p-d) <= 1 },
                              o_i not in int(E)
  eq:sup_halfspace_normal     atil_i = (C C^T)^{-1} (o_i - d),  a_i = atil/|atil|,
                              b_i = a_i^T o_i

Construction: d is the segment midpoint, the major axis is the segment
direction with semi-axis equal to the segment half-length, and the minor
semi-axis is shrunk until the boundary first touches an obstacle point.
Redundant obstacles are pruned greedily: after each half-space is emitted,
every obstacle it already excludes is dropped.

Prints TikZ-ready numbers; nothing is drawn here.
"""
import numpy as np

# --- segment (figure units = cm in the TikZ picture) ------------------------
P0 = np.array([0.0, 0.0])
P1 = np.array([5.2, 0.0])
D = 0.5 * (P0 + P1)
A_AX = 0.5 * np.linalg.norm(P1 - P0)      # semi-major, along the segment

# --- obstacle points (site-boundary / ore-pile samples) --------------------
OBST = np.array([
    [0.30, 1.42], [1.35, 1.05], [2.55, 1.28], [3.75, 1.62], [4.85, 1.35],
    [0.65, -1.30], [1.90, -1.62], [3.10, -1.12], [4.35, -1.45], [5.35, -1.02],
    [-0.55, 0.85], [5.75, 0.62],
    # points beyond the segment ends: they do not constrain the minor axis
    # (|u| > 1) but they do cap the polytope longitudinally
    [-1.05, 0.12], [6.35, -0.18],
])


def main():
    # ---- minor semi-axis: shrink until the boundary touches an obstacle ----
    u = (OBST[:, 0] - D[0]) / A_AX
    live = np.abs(u) < 1.0 - 1e-9
    b_req = np.abs(OBST[live, 1]) / np.sqrt(1.0 - u[live] ** 2)
    B_AX = float(b_req.min())
    touch = int(np.nonzero(live)[0][int(np.argmin(b_req))])
    print("semi-axes: a = %.4f, b = %.4f   center d = (%.3f, %.3f)"
          % (A_AX, B_AX, D[0], D[1]))
    print("touching obstacle index %d at (%.3f, %.3f)"
          % (touch, OBST[touch, 0], OBST[touch, 1]))

    C = np.diag([A_AX, B_AX])
    Minv = np.linalg.inv(C @ C.T)

    def qform(P):
        dv = P - D
        return np.einsum("ij,jk,ik->i", dv, Minv, dv)

    q = qform(OBST)
    print("min obstacle quadratic form = %.6f (must be >= 1)" % q.min())
    assert q.min() >= 1.0 - 1e-9, "eq:sup_ellipsoid_quadratic violated"
    # the segment must lie inside the ellipsoid
    seg = P0 + np.linspace(0, 1, 201)[:, None] * (P1 - P0)
    print("max segment quadratic form = %.6f (must be <= 1)" % qform(seg).max())
    assert qform(seg).max() <= 1.0 + 1e-9

    # ---- greedy half-space generation with redundancy pruning --------------
    remaining = list(range(len(OBST)))
    planes = []
    while remaining:
        # process the obstacle closest in the ellipsoid metric first
        qi = qform(OBST[remaining])
        k = remaining[int(np.argmin(qi))]
        o = OBST[k]
        at = Minv @ (o - D)
        a = at / np.linalg.norm(at)
        b = float(a @ o)
        planes.append((k, a, b))
        # the ellipsoid (hence the segment) must be on the safe side
        sup = float(a @ D + np.sqrt((a @ (C @ C.T)) @ a))
        assert sup <= b + 1e-9, "ellipsoid crosses the half-space"
        remaining = [j for j in remaining if float(a @ OBST[j]) < b - 1e-9]

    print("\n%d half-spaces retained out of %d obstacles"
          % (len(planes), len(OBST)))
    for k, a, b in planes:
        print("  o_%-2d (%6.3f,%6.3f)  a = (%7.4f,%7.4f)  b = %7.4f"
              % (k, OBST[k, 0], OBST[k, 1], a[0], a[1], b))

    # ---- polytope vertices, for drawing ------------------------------------
    A = np.array([p[1] for p in planes])
    bb = np.array([p[2] for p in planes])
    verts = []
    for i in range(len(planes)):
        for j in range(i + 1, len(planes)):
            M = np.array([A[i], A[j]])
            if abs(np.linalg.det(M)) < 1e-9:
                continue
            v = np.linalg.solve(M, [bb[i], bb[j]])
            if np.all(A @ v <= bb + 1e-7):
                verts.append(v)
    V = np.array(verts)
    ctr = V.mean(axis=0)
    V = V[np.argsort(np.arctan2(V[:, 1] - ctr[1], V[:, 0] - ctr[0]))]
    print("\npolytope P: %d vertices" % len(V))
    print("  tikz path: " + " -- ".join("(%.3f,%.3f)" % (x, y) for x, y in V)
          + " -- cycle")

    # ---- ellipse as a TikZ primitive ---------------------------------------
    print("\n  tikz ellipse: (%.3f,%.3f) ellipse [x radius=%.4f, "
          "y radius=%.4f]" % (D[0], D[1], A_AX, B_AX))
    print("  tikz obstacles: " + " ".join("(%.2f,%.2f)" % (x, y)
                                          for x, y in OBST))
    kept = " ".join("o%d" % p[0] for p in planes)
    print("  retained: " + kept)

    # ---- support points: where each half-space touches the ellipsoid -------
    # The boundary point whose outward normal is parallel to a_i is
    #   p = d + (C C^T) a / sqrt(a^T (C C^T) a).
    M = C @ C.T
    print("\n  ellipsoid support points (tangency anchors of a_i):")
    for k, a, b in planes:
        p = D + (M @ a) / np.sqrt(a @ M @ a)
        assert abs(qform(p[None, :])[0] - 1.0) < 1e-9, "anchor off the boundary"
        print("    o_%-2d: anchor (%.3f,%.3f)  gap to o_i = %.3f"
              % (k, p[0], p[1], float(a @ OBST[k]) - float(a @ p)))

    # ---- pruned (redundant) obstacles, drawn hollow in the figure ----------
    kept_idx = {p[0] for p in planes}
    pruned = [i for i in range(len(OBST)) if i not in kept_idx]
    print("\n  pruned as redundant: " +
          " ".join("(%.2f,%.2f)" % (OBST[i, 0], OBST[i, 1]) for i in pruned))
    print("  retained points:     " +
          " ".join("(%.2f,%.2f)" % (OBST[p[0], 0], OBST[p[0], 1])
                   for p in planes))

    # ---- initial sphere before the minor-axis shrink ------------------------
    inside_sphere = [i for i in range(len(OBST))
                     if np.linalg.norm(OBST[i] - D) < A_AX]
    print("\n  initial sphere radius %.3f encloses %d obstacle(s)"
          % (A_AX, len(inside_sphere)))

    # ---- ready-to-paste TikZ for the half-space boundaries -----------------
    # Each line a^T p = b is drawn through a*b along t = (-a_y, a_x) and left
    # for the picture's own \clip to trim.
    print("\n  TikZ half-space boundaries (draw long, clip in the picture):")
    for k, a, b in planes:
        p_on = a * b
        t = np.array([-a[1], a[0]])
        q0, q1 = p_on - 9.0 * t, p_on + 9.0 * t
        print("    \\draw[hsp] (%.3f,%.3f) -- (%.3f,%.3f); %% o_%d"
              % (q0[0], q0[1], q1[0], q1[1], k))

    # ---- half-space boundary segments clipped to the drawing box ----------
    print("\n  half-space boundary lines (clipped to x in [-1.2, 6.4]):")
    for k, a, b in planes:
        # parameterize the line a^T p = b
        t = np.array([-a[1], a[0]])
        p_on = a * b            # closest point of the line to the origin
        ts = []
        for xlim in (-1.2, 6.4):
            if abs(t[0]) > 1e-9:
                ts.append((xlim - p_on[0]) / t[0])
        for ylim in (-2.3, 2.3):
            if abs(t[1]) > 1e-9:
                ts.append((ylim - p_on[1]) / t[1])
        pts = [p_on + tv * t for tv in ts]
        pts = [p for p in pts
               if -1.2 - 1e-6 <= p[0] <= 6.4 + 1e-6
               and -2.3 - 1e-6 <= p[1] <= 2.3 + 1e-6]
        if len(pts) >= 2:
            pts = sorted(pts, key=lambda p: (p[0], p[1]))
            print("    o_%-2d: (%.3f,%.3f) -- (%.3f,%.3f)"
                  % (k, pts[0][0], pts[0][1], pts[-1][0], pts[-1][1]))


if __name__ == "__main__":
    main()