#!/usr/bin/env python3
"""Independent sampled convex-hull intersection oracle; no SAT/checker import.

Run after verify-checker.mjs. SciPy is a research dependency only, not required
by the deployed tool or portable checker. Agreement is counterexample evidence,
not a substitute for the continuous clearance proof.
"""
import json
import math
from pathlib import Path

import numpy as np
from scipy.optimize import linprog

HERE = Path(__file__).resolve().parent


def rotated(point, pose):
    # Construct each vertex by applying Rz, then Rx, then Ry in separate steps.
    # This avoids reusing the checker's closed-form rotation columns.
    x, y, z = point
    rx, ry, rz = pose[3:]
    x, y = x * math.cos(rz) - y * math.sin(rz), x * math.sin(rz) + y * math.cos(rz)
    y, z = y * math.cos(rx) - z * math.sin(rx), y * math.sin(rx) + z * math.cos(rx)
    x, z = x * math.cos(ry) + z * math.sin(ry), -x * math.sin(ry) + z * math.cos(ry)
    return [x + pose[0], y + pose[1], z + pose[2]]


def corners(box, pose=None):
    result = []
    for dx in (-1, 1):
        for dy in (-1, 1):
            for dz in (-1, 1):
                p = [box['center'][i] + sign * box['size'][i] / 2 for i, sign in enumerate((dx, dy, dz))]
                result.append(rotated(p, pose) if pose is not None else p)
    return np.array(result).T


def intersect(a, b):
    # Convex combinations of the two sets of vertices coincide iff the convex
    # boxes intersect. Five equalities: three spatial, two coefficient sums.
    eq = np.zeros((5, 16))
    eq[:3, :8] = a
    eq[:3, 8:] = -b
    eq[3, :8] = 1
    eq[4, 8:] = 1
    solution = linprog(np.zeros(16), A_eq=eq, b_eq=[0, 0, 0, 1, 1], bounds=(0, None), method='highs')
    if solution.status not in (0, 2):
        raise RuntimeError(f'Unexpected LP status {solution.status}')
    return solution.status == 0


def delta(a, b):
    value = (b - a) % (2 * math.pi)
    return value - 2 * math.pi if value > math.pi else value


def sample(a, b, t):
    return [a[i] + ((b[i] - a[i]) if i < 3 else delta(a[i], b[i])) * t for i in range(6)]


def collision(project, pose):
    obstacles = [corners(value) for value in project['world']['obstacles']]
    for box in project['furniture']['boxes']:
        vertices = corners(box, pose)
        # Outside the allowed rectangular world is a boundary violation.
        for axis, size in enumerate(project['world']['size']):
            if np.min(vertices[axis]) < -1e-8 or np.max(vertices[axis]) > size + 1e-8:
                return True
        for obstacle in obstacles:
            if intersect(vertices, obstacle):
                return True
    return False


cases = json.loads((HERE / 'checker-cases.json').read_text())
checked_poses = 0
accepted_cases = 0
rejected_cases_with_collision = 0
for case in cases:
    project = case['project']
    path = project['path']
    found = False
    edges = list(zip(path[:-1], path[1:])) or [(path[0], path[0])]
    for a, b in edges:
        for i in range(101):
            checked_poses += 1
            found |= collision(project, sample(a, b, i / 100))
    if case['expected'] == 'verified':
        assert not found, f"Independently detected collision in verified case: {case['name']}"
        accepted_cases += 1
    elif found:
        rejected_cases_with_collision += 1

# Three rejection fixtures concern margins rather than geometric intersection;
# the LP oracle should not mislabel them as actual physical overlap.
summary = {
    'method': 'Convex-combination feasibility using SciPy HiGHS, sequential vertex rotations',
    'acceptedCasesWithoutSampledIntersection': accepted_cases,
    'rejectedCasesWithSampledIntersection': rejected_cases_with_collision,
    'sampledPoses': checked_poses,
    'counterexamplesToAcceptedCertificates': 0,
    'scope': 'Independent sampled intersection check; continuous proof remains the displacement enclosure',
}
(HERE / 'lp-results.json').write_text(json.dumps(summary, indent=2) + '\n')
print(json.dumps(summary, indent=2))