heightmap.py

"""
Modulo de mapa de alturas con modelo de superficie.
v3.0 - Interpolacion RBF (Radial Basis Function) para cobertura
completa del area de fresado, incluyendo extrapolacion natural.

Dado un punto (X, Y), calcula el Z de compensacion usando un modelo
matematico que genera una superficie suave a partir de los puntos medidos.

NORMALIZACION:
Se usa como referencia el punto mas cercano a (0,0), que es donde
el usuario setea Z=0 antes de fresar.

v3.0 (auditoría 2026-05):
- Validación de inputs (no NaN/inf en heights, min 3×3, dimensiones consistentes)
- Manejo defensivo de RBF singular matrix (puntos colineales o duplicados)
- Mensajes de error descriptivos
"""

import numpy as np
from scipy.interpolate import RBFInterpolator

try:
    from logging_config import get_logger
    _logger = get_logger(__name__)
    def _log(msg): _logger.info(msg)
except ImportError:
    def _log(msg): print(f"[HeightMap] {msg}")


class HeightMapError(ValueError):
    """Inputs inválidos o construcción de modelo fallida."""


class HeightMap:
    def __init__(self, points_x, points_y, heights):
        self.points_x = np.array(points_x)
        self.points_y = np.array(points_y)
        self.heights_raw = np.array(heights, dtype=float)

        # ─────── Validación de inputs (defensiva) ───────
        nx = len(self.points_x)
        ny = len(self.points_y)
        if nx < 3 or ny < 3:
            raise HeightMapError(
                f"HeightMap requiere grid mínimo 3×3 para RBF. Recibido {nx}×{ny}."
            )
        if self.heights_raw.shape != (ny, nx):
            raise HeightMapError(
                f"Dimensiones inconsistentes: heights={self.heights_raw.shape}, "
                f"esperado ({ny}, {nx})."
            )
        # Detectar inf (NaN se trata aparte abajo: se permite que probe falle en algunos puntos)
        if np.any(np.isinf(self.heights_raw)):
            raise HeightMapError(
                "heights contiene valores infinitos. Re-escanear puntos defectuosos."
            )
        # Validar que TODA la secuencia sea estrictamente creciente
        # (no solo extremos: [0, 2, 1, 3] tiene extremos OK pero interior decreciente)
        if not np.all(np.diff(self.points_x) > 0):
            raise HeightMapError(
                f"points_x no es estrictamente creciente: {self.points_x.tolist()}"
            )
        if not np.all(np.diff(self.points_y) > 0):
            raise HeightMapError(
                f"points_y no es estrictamente creciente: {self.points_y.tolist()}"
            )

        # NORMALIZAR: usar el punto (0,0) como referencia
        ref = self.heights_raw[0][0]
        if np.isnan(ref):
            # Si el punto de referencia falló en el probe, usar el primer punto válido
            valid_idx = np.argwhere(~np.isnan(self.heights_raw))
            if len(valid_idx) == 0:
                raise HeightMapError("Ningún punto del mapa tiene altura válida (todos NaN).")
            r0, c0 = valid_idx[0]
            ref = self.heights_raw[r0][c0]
            _log(f"⚠ heights[0][0] es NaN, usando heights[{r0}][{c0}] = {ref:.4f} como referencia")
        self.reference_z = float(ref)
        self.heights = self.heights_raw - self.reference_z

        self.x_min = float(self.points_x[0])
        self.x_max = float(self.points_x[-1])
        self.y_min = float(self.points_y[0])
        self.y_max = float(self.points_y[-1])

        # Construir modelo de superficie RBF
        # Convertir grilla a pares (x,y) y valores z (omitiendo NaN del probe)
        coords = []
        values = []
        for r in range(len(self.points_y)):
            for c in range(len(self.points_x)):
                z = self.heights[r][c]
                if not np.isnan(z):
                    coords.append([self.points_x[c], self.points_y[r]])
                    values.append(z)

        if len(values) < 3:
            raise HeightMapError(
                f"Solo {len(values)} puntos válidos en el mapa (después de filtrar NaN). "
                f"RBF requiere mínimo 3."
            )

        coords = np.array(coords)
        values = np.array(values)

        # RBF con kernel thin_plate_spline: genera superficie suave
        # que interpola exactamente en los puntos medidos y extrapola
        # naturalmente fuera del area con continuidad C1
        try:
            self.rbf = RBFInterpolator(coords, values, kernel='thin_plate_spline', smoothing=0.0)
        except np.linalg.LinAlgError as e:
            raise HeightMapError(
                f"RBF singular matrix — puntos colineales o muy cercanos. "
                f"Re-escanear con más densidad. Detalle: {e}"
            ) from e

        # Estadisticas
        valid = self.heights[~np.isnan(self.heights)]
        if len(valid) > 0:
            self.z_min = float(valid.min())
            self.z_max = float(valid.max())
            self.z_range = self.z_max - self.z_min
        else:
            self.z_min = self.z_max = self.z_range = 0.0

        self.zero_ref_x = float(self.points_x[0])
        self.zero_ref_y = float(self.points_y[0])

        _log(f"Modelo RBF con {len(values)} puntos válidos de {nx*ny} totales")
        _log(f"Referencia Z en ({self.zero_ref_x:.0f},{self.zero_ref_y:.0f}): {self.reference_z:.4f}mm")
        _log(f"Offsets: {self.z_min:+.4f} a {self.z_max:+.4f}mm (var: {self.z_range:.4f}mm)")

    def get_z_offset(self, x, y):
        """
        Retorna el offset Z relativo para cualquier punto (x,y).
        Clampea coordenadas al area del grid para evitar extrapolacion
        RBF que puede diverger fuera del area medida.
        """
        x_c = max(float(self.x_min), min(float(self.x_max), x))
        y_c = max(float(self.y_min), min(float(self.y_max), y))
        point = np.array([[x_c, y_c]])
        result = self.rbf(point)
        return float(result[0])

    def get_z_offsets_batch(self, xy_pairs):
        """
        Calcula offsets para multiples puntos de una vez (mas eficiente).
        Clampea al area del grid.
        """
        points = np.array(xy_pairs, dtype=float)
        if points.ndim == 1:
            points = points.reshape(1, -1)
        points[:, 0] = np.clip(points[:, 0], float(self.x_min), float(self.x_max))
        points[:, 1] = np.clip(points[:, 1], float(self.y_min), float(self.y_max))
        results = self.rbf(points)
        return results.tolist()

    def get_zero_recommendation(self):
        return (f"Setea Z=0 en la posicion ({self.zero_ref_x:.0f}, {self.zero_ref_y:.0f}) "
                f"de la placa (esquina inferior izquierda del area escaneada). "
                f"Ahi el mapa marco {self.reference_z:.4f}mm como referencia.")

    def print_map(self):
        """Imprime resumen del mapa al logger (antes: print() directo)."""
        _log(f"Mapa de alturas ({len(self.points_y)} x {len(self.points_x)}):")
        _log(f"X: {self.x_min:.1f} -> {self.x_max:.1f}mm")
        _log(f"Y: {self.y_min:.1f} -> {self.y_max:.1f}mm")
        _log(f"Z variacion: {self.z_range:.4f}mm")
        _log("Modelo: RBF thin_plate_spline")