houseplan-card/custom_components/houseplan/radar_geometry.py

"""Pure geometry shared by radar setup, live projection and analysis (#485)."""
from __future__ import annotations

import math
from dataclasses import dataclass
from typing import Any, Iterable

UNIT_TO_CM = {"mm": 0.1, "cm": 1.0, "m": 100.0, "in": 2.54, "ft": 30.48}
MAX_LOCAL_CM = 10_000.0


@dataclass(frozen=True)
class RadarFit:
    heading_deg: float
    mirror: bool
    rms_cm: float
    errors_cm: tuple[float, float]


def length_cm(value: Any, unit: str) -> float:
    """Convert one finite length without guessing or clamping its unit."""
    number = float(value)
    if not math.isfinite(number) or unit not in UNIT_TO_CM:
        raise ValueError("invalid_radar")
    result = number * UNIT_TO_CM[unit]
    if abs(result) > MAX_LOCAL_CM:
        raise ValueError("invalid_radar")
    return result


def polar_to_cartesian(distance: Any, bearing: Any, *, unit: str,
                       angle_unit: str, zero: str, clockwise: bool) -> tuple[float, float]:
    distance_cm = length_cm(distance, unit)
    if distance_cm < 0:
        raise ValueError("invalid_radar")
    angle = float(bearing)
    if not math.isfinite(angle) or angle_unit not in {"degrees", "radians"}:
        raise ValueError("invalid_radar")
    angle = math.degrees(angle) if angle_unit == "radians" else angle
    if not clockwise:
        angle = -angle
    if zero == "right":
        angle += 90.0
    elif zero != "forward":
        raise ValueError("invalid_radar")
    radians = math.radians(angle)
    return distance_cm * math.sin(radians), distance_cm * math.cos(radians)


def project_local(mount: dict[str, Any], calibration: dict[str, Any],
                  x_cm: float, y_cm: float) -> tuple[float, float]:
    """Project canonical local centimetres into the square normalized plan."""
    x_cm = float(x_cm)
    y_cm = float(y_cm)
    if not all(map(math.isfinite, (x_cm, y_cm))) or math.hypot(x_cm, y_cm) > MAX_LOCAL_CM:
        raise ValueError("invalid_radar")
    mx, my = float(mount["x"]), float(mount["y"])
    theta = math.radians(float(mount["heading_deg"]))
    cell_cm = float(calibration["cell_cm"])
    if not all(map(math.isfinite, (mx, my, theta, cell_cm))) or cell_cm <= 0:
        raise ValueError("invalid_radar")
    mirror = -1.0 if calibration.get("mirror") else 1.0
    scale = 240.0 * cell_cm
    return (
        mx + (mirror * x_cm * math.cos(theta) + y_cm * math.sin(theta)) / scale,
        my + (mirror * x_cm * math.sin(theta) - y_cm * math.cos(theta)) / scale,
    )


def inverse_project(mount: dict[str, Any], calibration: dict[str, Any],
                    x: float, y: float) -> tuple[float, float]:
    theta = math.radians(float(mount["heading_deg"]))
    dx = (float(x) - float(mount["x"])) * 240.0 * float(calibration["cell_cm"])
    dy = (float(y) - float(mount["y"])) * 240.0 * float(calibration["cell_cm"])
    mirror = -1.0 if calibration.get("mirror") else 1.0
    return (
        mirror * (dx * math.cos(theta) + dy * math.sin(theta)),
        dx * math.sin(theta) - dy * math.cos(theta),
    )


def point_in_polygon(point: tuple[float, float], polygon: Iterable[Any],
                     tolerance: float = 1e-9) -> bool:
    """Boundary-inclusive even/odd test; no bounding-box fallback."""
    points = [(float(p[0] if isinstance(p, (list, tuple)) else p["x"]),
               float(p[1] if isinstance(p, (list, tuple)) else p["y"])) for p in polygon]
    if len(points) < 3:
        return True
    px, py = point
    inside = False
    for index, (ax, ay) in enumerate(points):
        bx, by = points[(index + 1) % len(points)]
        cross = (px - ax) * (by - ay) - (py - ay) * (bx - ax)
        if abs(cross) <= tolerance and min(ax, bx) - tolerance <= px <= max(ax, bx) + tolerance \
                and min(ay, by) - tolerance <= py <= max(ay, by) + tolerance:
            return True
        if (ay > py) != (by > py):
            at_x = ax + (py - ay) * (bx - ax) / (by - ay)
            if px < at_x:
                inside = not inside
    return inside


def polygon_is_convex(polygon: Iterable[Any]) -> bool:
    """Whether a straight visual transition stays inside this room.

    A chord between valid points can leave a concave room. Smoothing is only
    cosmetic, so disabling it for such rooms is safer than drawing presence
    through a wall.
    """
    points = [(float(p[0] if isinstance(p, (list, tuple)) else p["x"]),
               float(p[1] if isinstance(p, (list, tuple)) else p["y"])) for p in polygon]
    if len(points) < 3:
        return False
    sign = 0
    for index, (ax, ay) in enumerate(points):
        bx, by = points[(index + 1) % len(points)]
        cx, cy = points[(index + 2) % len(points)]
        cross = (bx - ax) * (cy - by) - (by - ay) * (cx - bx)
        if abs(cross) <= 1e-12:
            continue
        current = 1 if cross > 0 else -1
        if sign and current != sign:
            return False
        sign = current
    return sign != 0


def clipped_arc_segments(
    center: tuple[float, float], radius: float, heading_deg: float,
    fov_deg: float | None, polygon: Iterable[Any] | None,
) -> list[list[tuple[float, float]]]:
    """Approximate a range arc and split it at a room boundary.

    Range input has no bearing, so the arc is the honest geometry.  Returning
    only contiguous inside runs prevents the browser from drawing the arc
    through walls or across a concave cut-out.  The resolution (<=4 degrees)
    is considerably finer than the two-CSS-pixel display stroke.
    """
    if not all(math.isfinite(value) for value in (*center, radius, heading_deg)) \
            or radius <= 0 or fov_deg is None or not math.isfinite(fov_deg) \
            or not 0 < fov_deg <= 360:
        return []
    points = list(polygon) if polygon is not None else None
    steps = max(8, min(180, math.ceil(fov_deg / 4)))
    start = heading_deg - 90 - fov_deg / 2
    samples = [
        (
            center[0] + math.cos(math.radians(start + fov_deg * index / steps)) * radius,
            center[1] + math.sin(math.radians(start + fov_deg * index / steps)) * radius,
        )
        for index in range(steps + 1)
    ]
    if points is None:
        return [samples]
    segments: list[list[tuple[float, float]]] = []
    current: list[tuple[float, float]] = []
    for sample in samples:
        if point_in_polygon(sample, points, 1e-6):
            current.append(sample)
        else:
            if len(current) >= 2:
                segments.append(current)
            current = []
    if len(current) >= 2:
        segments.append(current)
    return segments


def solve_two_point(mount: tuple[float, float], local_points: list[tuple[float, float]],
                    plan_points: list[tuple[float, float]], cell_cm: float) -> RadarFit:
    """Fit one rigid heading and mirror with the physical scale fixed."""
    if len(local_points) != 2 or len(plan_points) != 2 or cell_cm <= 0:
        raise ValueError("invalid_selection")
    scale = 240.0 * cell_cm
    targets = [((x - mount[0]) * scale, -(y - mount[1]) * scale) for x, y in plan_points]
    if any(math.hypot(*local) < 50 for local in local_points):
        raise ValueError("invalid_selection")
    denominator = math.hypot(*local_points[0]) * math.hypot(*local_points[1])
    angle = math.degrees(math.acos(max(-1.0, min(1.0,
        sum(a * b for a, b in zip(local_points[0], local_points[1], strict=True)) / denominator))))
    if angle < 20 or angle > 160:
        raise ValueError("invalid_selection")

    candidates: list[RadarFit] = []
    for mirrored in (False, True):
        transformed = [((-x if mirrored else x), y) for x, y in local_points]
        dot = sum(sx * tx + sy * ty for (sx, sy), (tx, ty) in zip(transformed, targets, strict=True))
        cross = sum(sx * ty - sy * tx for (sx, sy), (tx, ty) in zip(transformed, targets, strict=True))
        rotation = math.atan2(cross, dot)
        errors = []
        for (sx, sy), (tx, ty) in zip(transformed, targets, strict=True):
            rx = sx * math.cos(rotation) - sy * math.sin(rotation)
            ry = sx * math.sin(rotation) + sy * math.cos(rotation)
            errors.append(math.hypot(rx - tx, ry - ty))
        radial_ok = all(abs(math.hypot(*local) - math.hypot(*target)) <= max(20, .15 * math.hypot(*target))
                        for local, target in zip(local_points, targets, strict=True))
        rms = math.sqrt(sum(error * error for error in errors) / 2)
        if radial_ok and rms <= 20 and max(errors) <= 30:
            candidates.append(RadarFit(
                (-math.degrees(rotation)) % 360,
                mirrored,
                rms,
                (errors[0], errors[1]),
            ))
    if not candidates:
        raise ValueError("invalid_selection")
    candidates.sort(key=lambda fit: fit.rms_cm)
    if len(candidates) > 1 and abs(candidates[0].rms_cm - candidates[1].rms_cm) < 10:
        raise ValueError("ambiguous_sources")
    return candidates[0]