"""
Binaural spatial hearing — triangulation, tracking, gaze.

Combines DoA angles from two XVF3800 arrays into a sound source position,
smooths the tracking, and pushes gaze coordinates to the eye service.
"""

import logging
import math
import time
from typing import Optional

logger = logging.getLogger("headmic.spatial")

# Array geometry (measured on skull, can be overridden from config)
DEFAULT_ARRAY_SEPARATION_MM = 175.0  # center-to-center distance between arrays

# Gaze mapping
GAZE_CENTER = 127           # neutral gaze (0-255 range)
GAZE_X_RANGE = 80           # max horizontal deflection from center
GAZE_Y_RANGE = 30           # max vertical deflection from center
GAZE_MAX_DISTANCE_MM = 3000 # beyond this, gaze is "far" (no convergence)

# Smoothing
SMOOTHING_ALPHA = 0.4       # exponential smoothing (0=sluggish, 1=instant) — slightly snappy
IDLE_RETURN_SPEED = 0.03    # how fast gaze drifts to center when no VAD — gentle drift
IDLE_TIMEOUT_S = 1.5        # seconds of no VAD before drifting to center

# Distance estimation (ILD-based)
# ILD = 20 * log10(louder_energy / quieter_energy) in dB
# Empirical mapping: ILD varies with angle and distance.
# At 175mm separation, a source at 45° off-center produces:
#   ~0.5m: ILD ≈ 6-10 dB
#   ~1.5m: ILD ≈ 3-5 dB
#   ~3.0m: ILD ≈ 1-2 dB
# These are rough — calibrate on real hardware.
PROXIMITY_ZONES = [
    ("intimate",       0,    500),   # < 0.5m — whispering distance
    ("conversational", 500,  2000),  # 0.5-2m — normal talking
    ("across_room",    2000, 5000),  # 2-5m — raised voice
    ("far",            5000, 99999), # > 5m — shouting distance
]


class SpatialTracker:
    """Triangulates sound source from two DoA angles and produces smooth gaze."""

    def __init__(self, array_separation_mm: float = DEFAULT_ARRAY_SEPARATION_MM):
        self.separation = array_separation_mm
        self.half_sep = array_separation_mm / 2.0

        # Smoothed state
        self._smooth_x: float = 0.0    # mm, relative to skull center
        self._smooth_y: float = 0.0    # mm, forward from skull
        self._smooth_gaze_x: float = float(GAZE_CENTER)
        self._smooth_gaze_y: float = float(GAZE_CENTER)
        self._smooth_distance: float = GAZE_MAX_DISTANCE_MM
        self._smooth_ild: float = 0.0  # dB

        # VAD tracking
        self._last_vad_time: float = 0.0
        self._any_vad: bool = False

        # Last raw result for API
        self.last_position: Optional[dict] = None

    def update(self, doa: dict, left_energy: float = 0.0, right_energy: float = 0.0) -> Optional[dict]:
        """
        Process DoA readings + audio energy from both arrays.

        Args:
            doa: {"left": {"angle": 0-359, "vad": bool}, "right": {"angle": 0-359, "vad": bool}}
            left_energy: RMS energy from left mic stream (0.0-1.0)
            right_energy: RMS energy from right mic stream (0.0-1.0)

        Returns:
            {"x_mm", "y_mm", "distance_mm", "ild_db", "proximity",
             "gaze_x", "gaze_y", "vad", "side"}
            or None if insufficient data.
        """
        left = doa.get("left")
        right = doa.get("right")

        if not left or not right:
            return self._idle_drift()

        left_vad = left.get("vad", False)
        right_vad = right.get("vad", False)
        any_vad = left_vad or right_vad

        if any_vad:
            self._last_vad_time = time.monotonic()
            self._any_vad = True

        left_angle = left["angle"]
        right_angle = right["angle"]

        # Triangulate position
        pos = self._triangulate(left_angle, right_angle)

        # Compute ILD (Interaural Level Difference)
        ild_db = self._compute_ild(left_energy, right_energy)

        if pos and any_vad:
            # Smooth the position
            self._smooth_x += SMOOTHING_ALPHA * (pos["x_mm"] - self._smooth_x)
            self._smooth_y += SMOOTHING_ALPHA * (pos["y_mm"] - self._smooth_y)
            self._smooth_ild += SMOOTHING_ALPHA * (ild_db - self._smooth_ild)

            # Fuse triangulated distance with ILD
            tri_dist = math.sqrt(self._smooth_x**2 + self._smooth_y**2)
            ild_dist = self._ild_to_distance(self._smooth_ild)
            # Weighted average: trust triangulation more (0.7) but let ILD correct it (0.3)
            fused_dist = 0.7 * tri_dist + 0.3 * ild_dist
            self._smooth_distance += SMOOTHING_ALPHA * (fused_dist - self._smooth_distance)
        elif not any_vad:
            return self._idle_drift()

        # Convert to gaze
        gaze_x, gaze_y = self._position_to_gaze(self._smooth_x, self._smooth_y)

        # Smooth gaze
        self._smooth_gaze_x += SMOOTHING_ALPHA * (gaze_x - self._smooth_gaze_x)
        self._smooth_gaze_y += SMOOTHING_ALPHA * (gaze_y - self._smooth_gaze_y)

        # Classify proximity zone
        proximity = self._classify_proximity(self._smooth_distance)

        result = {
            "x_mm": round(self._smooth_x, 1),
            "y_mm": round(self._smooth_y, 1),
            "distance_mm": round(self._smooth_distance, 1),
            "ild_db": round(self._smooth_ild, 1),
            "proximity": proximity,
            "gaze_x": int(round(self._smooth_gaze_x)),
            "gaze_y": int(round(self._smooth_gaze_y)),
            "vad": any_vad,
            "side": "left" if self._smooth_x < 0 else "right",
        }
        self.last_position = result
        return result

    def _idle_drift(self) -> Optional[dict]:
        """When no VAD, smoothly return gaze to center."""
        elapsed = time.monotonic() - self._last_vad_time

        if elapsed < IDLE_TIMEOUT_S:
            # Hold last position briefly
            return self.last_position

        # Drift toward center
        self._smooth_gaze_x += IDLE_RETURN_SPEED * (GAZE_CENTER - self._smooth_gaze_x)
        self._smooth_gaze_y += IDLE_RETURN_SPEED * (GAZE_CENTER - self._smooth_gaze_y)

        result = {
            "x_mm": round(self._smooth_x, 1),
            "y_mm": round(self._smooth_y, 1),
            "distance_mm": round(self._smooth_distance, 1),
            "ild_db": round(self._smooth_ild, 1),
            "proximity": self._classify_proximity(self._smooth_distance),
            "gaze_x": int(round(self._smooth_gaze_x)),
            "gaze_y": int(round(self._smooth_gaze_y)),
            "vad": False,
            "side": "center",
        }
        self.last_position = result
        return result

    @staticmethod
    def _compute_ild(left_energy: float, right_energy: float) -> float:
        """Compute Interaural Level Difference in dB.
        Positive = louder on left, negative = louder on right."""
        # Clamp to avoid log(0)
        left_e = max(left_energy, 1e-10)
        right_e = max(right_energy, 1e-10)
        return 20.0 * math.log10(left_e / right_e)

    @staticmethod
    def _ild_to_distance(ild_db: float) -> float:
        """Estimate distance from ILD magnitude.
        Higher ILD = closer source (head shadow effect is stronger up close).
        This is a rough empirical mapping — should be calibrated per-installation."""
        ild_abs = abs(ild_db)
        if ild_abs > 8.0:
            return 300.0    # very close, ~30cm
        elif ild_abs > 5.0:
            return 700.0    # close, ~70cm
        elif ild_abs > 3.0:
            return 1500.0   # conversational, ~1.5m
        elif ild_abs > 1.5:
            return 2500.0   # across room, ~2.5m
        else:
            return 4000.0   # far or directly ahead (no ILD)

    @staticmethod
    def _classify_proximity(distance_mm: float) -> str:
        """Classify distance into a proximity zone."""
        for name, lo, hi in PROXIMITY_ZONES:
            if lo <= distance_mm < hi:
                return name
        return "far"

    def _triangulate(self, left_deg: float, right_deg: float) -> Optional[dict]:
        """
        Triangulate sound source position from two DoA angles.

        Array coordinate system:
        - Origin: center of skull
        - X axis: positive = right (toward right ear)
        - Y axis: positive = forward (in front of skull)

        Each array's DoA is 0° = front, 90° = right, 180° = back, 270° = left.
        The arrays are positioned at (-half_sep, 0) and (+half_sep, 0).
        """
        # Convert DoA angles to bearing vectors
        # DoA 0° = forward (+Y), 90° = right (+X) for each array
        left_rad = math.radians(left_deg)
        right_rad = math.radians(right_deg)

        # Direction vectors from each array position
        # Left array at (-half_sep, 0), right array at (+half_sep, 0)
        left_dx = math.sin(left_rad)
        left_dy = math.cos(left_rad)
        right_dx = math.sin(right_rad)
        right_dy = math.cos(right_rad)

        # Solve intersection of two rays:
        # P_left + t * D_left = P_right + s * D_right
        # (-half_sep + t*left_dx, t*left_dy) = (half_sep + s*right_dx, s*right_dy)
        #
        # t*left_dx - s*right_dx = separation
        # t*left_dy - s*right_dy = 0

        denom = left_dx * right_dy - left_dy * right_dx

        if abs(denom) < 0.001:
            # Parallel rays — can't triangulate, source is very far away or directly ahead
            # Fall back to bearing midpoint at a default distance
            avg_rad = (left_rad + right_rad) / 2
            return {
                "x_mm": GAZE_MAX_DISTANCE_MM * math.sin(avg_rad),
                "y_mm": GAZE_MAX_DISTANCE_MM * math.cos(avg_rad),
            }

        t = (self.separation * right_dy) / denom

        if t < 0:
            # Intersection is behind the arrays — likely noise or rear source
            # Use the bearing with positive t scaled to max distance
            avg_rad = (left_rad + right_rad) / 2
            return {
                "x_mm": GAZE_MAX_DISTANCE_MM * math.sin(avg_rad) * 0.5,
                "y_mm": GAZE_MAX_DISTANCE_MM * math.cos(avg_rad) * 0.5,
            }

        # Compute intersection point relative to left array, then shift to skull center
        x = -self.half_sep + t * left_dx
        y = t * left_dy

        return {"x_mm": x, "y_mm": y}

    def _position_to_gaze(self, x_mm: float, y_mm: float) -> tuple[float, float]:
        """
        Convert position (mm) to gaze coordinates (0-255).

        Horizontal: source on the right → eyes look right (gaze_x > 127)
        Vertical: source closer → eyes look slightly down, farther → straight ahead
        """
        distance = math.sqrt(x_mm**2 + y_mm**2)
        if distance < 1.0:
            return float(GAZE_CENTER), float(GAZE_CENTER)

        # Horizontal: angle from center
        angle = math.atan2(x_mm, max(y_mm, 100.0))  # clamp y to avoid extreme angles
        # Map angle (roughly -pi/2 to pi/2) to gaze range
        gaze_x = GAZE_CENTER + GAZE_X_RANGE * (angle / (math.pi / 2))
        gaze_x = max(GAZE_CENTER - GAZE_X_RANGE, min(GAZE_CENTER + GAZE_X_RANGE, gaze_x))

        # Vertical: closer = slightly down, far = center
        # This simulates looking down at someone close vs straight ahead at someone far
        proximity = max(0.0, 1.0 - distance / GAZE_MAX_DISTANCE_MM)
        gaze_y = GAZE_CENTER + GAZE_Y_RANGE * proximity * 0.3  # subtle effect

        return gaze_x, gaze_y