gcode_modifier.py

"""
Módulo de parsing y modificación de G-code.
Lee un archivo .nc, aplica compensación de altura usando el HeightMap,
y genera un nuevo archivo nivelado.

v3.0 (auditoría 2026-05):
- Validación de Z post-compensación contra machine limits.
- Cuenta warnings de "Z fuera de rango" para que el usuario los vea.
"""

import re
import math

try:
    from logging_config import get_logger
    _logger = get_logger(__name__)
    def _log_info(msg): _logger.info(msg)
    def _log_warn(msg): _logger.warning(msg)
except ImportError:
    def _log_info(msg): print(f"[GcodeModifier] {msg}")
    def _log_warn(msg): print(f"[GcodeModifier WARNING] {msg}")

# Hard limits de la máquina TwoTrees 3018 (mecánicos, no se exceden nunca con seguridad).
# Coinciden con MACHINE_LIMITS en gcode_sender.py.
MACHINE_Z_MIN = -10.0
MACHINE_Z_MAX = 50.0


class GcodeModifier:
    def __init__(self, heightmap, max_segment=1.0, z_min=MACHINE_Z_MIN, z_max=MACHINE_Z_MAX):
        self.hmap = heightmap
        # FIX auditoría 2026-05-28: max_segment<=0 provocaba ZeroDivisionError en
        # _segmented_move; negativos producían leveling silenciosamente malo (sin
        # segmentar). validate_probe_params no cubría este campo. Guard defensivo aquí.
        if not isinstance(max_segment, (int, float)) or max_segment <= 0:
            _log_warn(f"max_segment inválido ({max_segment!r}); usando 1.0mm por defecto.")
            max_segment = 1.0
        self.max_segment = float(max_segment)
        self.z_min = z_min
        self.z_max = z_max
        self.z_out_of_bounds_count = 0  # estadística para warning al usuario
        self.arc_passthrough_count = 0  # arcos G02/G03 de corte sin compensar (auditoría 2026-05-28)
        self.current_x = 0.0
        self.current_y = 0.0
        self.current_z = 0.0
        self.current_f = 0.0
        self.absolute = True
        self.current_mode = None

    def _safe_z(self, z, x=None, y=None):
        """Clampea Z a [z_min, z_max] y cuenta cuántas veces hubo que ajustar."""
        if z < self.z_min:
            self.z_out_of_bounds_count += 1
            _log_warn(
                f"Z={z:.4f} bajo z_min={self.z_min} en ({x},{y}) — clampeado. "
                f"Posible problema en el heightmap o G-code muy profundo."
            )
            return self.z_min
        if z > self.z_max:
            self.z_out_of_bounds_count += 1
            _log_warn(f"Z={z:.4f} sobre z_max={self.z_max} en ({x},{y}) — clampeado.")
            return self.z_max
        return z

    def process_file(self, input_path, output_path):
        with open(input_path, "r", encoding="utf-8", errors="replace") as f:
            lines = f.readlines()

        output_lines = []
        modified_count = 0
        total_moves = 0

        output_lines.append("(Autolevel aplicado - Mecha Leveling v3.0)\n")
        output_lines.append(f"(Grilla: {len(self.hmap.points_x)}x{len(self.hmap.points_y)} puntos)\n")
        output_lines.append(f"(Segmento max: {self.max_segment}mm)\n")
        output_lines.append("\n")

        for line_num, line in enumerate(lines):
            stripped = line.strip()

            if not stripped or stripped.startswith("(") or stripped.startswith(";") or stripped.startswith("%"):
                output_lines.append(line)
                continue

            if "G90" in stripped.upper():
                self.absolute = True
                output_lines.append(line)
                continue
            if "G91" in stripped.upper():
                self.absolute = False
                output_lines.append(line)
                continue

            parsed = self._parse_line(stripped)

            if parsed is None:
                output_lines.append(line)
                continue

            gcode, params = parsed
            total_moves += 1

            if gcode in ("G00", "G0"):
                new_lines = self._process_rapid(params)
                output_lines.extend(new_lines)
                modified_count += 1
            elif gcode in ("G01", "G1"):
                new_lines = self._process_linear(params)
                output_lines.extend(new_lines)
                modified_count += 1
            elif gcode in ("G02", "G2", "G03", "G3"):
                # Arcos: pasan SIN compensación de altura (no se segmentan).
                # En G-code de PCB linealizado (FlatCAM/MechaPCB) no aparecen.
                # Si aparecen y son corte (Z<0), avisamos: sobre placa alabeada
                # cortarían a profundidad incorrecta. (auditoría 2026-05-28)
                arc_z = params.get("Z", self.current_z)
                if arc_z < 0:
                    self.arc_passthrough_count += 1
                output_lines.append(line)
                self._update_state(params)
            else:
                output_lines.append(line)

        # Ensure safe return at end of file
        last_real = [l.strip().upper() for l in output_lines[-10:] if l.strip() and not l.strip().startswith("(")]
        has_return = any("X0" in l and "Y0" in l for l in last_real)
        if not has_return:
            output_lines.append("\n")
            output_lines.append("(Safe return - Mecha Leveling)\n")
            if not any("M05" in l or "M5" in l for l in last_real[-3:]):
                output_lines.append("M05\n")
            output_lines.append("G00 Z15.000\n")
            output_lines.append("G00 X0 Y0\n")

        with open(output_path, "w", encoding="utf-8") as f:
            f.writelines(output_lines)

        _log_info(f"G-code nivelado guardado en: {output_path}")
        _log_info(f"  Movimientos procesados: {modified_count}/{total_moves}")
        _log_info(f"  Lineas originales: {len(lines)} → resultado: {len(output_lines)}")
        if self.z_out_of_bounds_count > 0:
            _log_warn(
                f"⚠ {self.z_out_of_bounds_count} valores Z fueron clampeados "
                f"a [{self.z_min}, {self.z_max}]. Revisar heightmap o profundidad del G-code."
            )
        if self.arc_passthrough_count > 0:
            _log_warn(
                f"⚠ {self.arc_passthrough_count} arcos G02/G03 de corte (Z<0) pasaron SIN "
                f"compensación de altura. En una placa alabeada pueden cortar muy profundo o muy "
                f"superficial. Si tu G-code tiene arcos, re-exportá linealizado (solo G00/G01)."
            )
        return {
            "input": input_path,
            "output": output_path,
            "total_moves": total_moves,
            "modified": modified_count,
            "z_clamped": self.z_out_of_bounds_count,
            "arcs_uncompensated": self.arc_passthrough_count,
        }

    def _parse_line(self, line):
        # FIX auditoria 2026-06-25: quitar comentarios inline ANTES de parsear.
        # pcb2gcode emite comentarios entre parentesis al final de lineas con
        # codigo, ej "M5 ( Spindle off. )". La F de "off." se capturaba como un
        # eje F con valor '.' y reventaba float('.') al nivelar. Ademas el
        # fallback de current_mode trataba ese M5 como un movimiento G01. Los
        # comentarios nunca deben parsearse como G-code: se eliminan aqui (los
        # parentesis se reemplazan por espacio; ';' corta hasta el fin de linea).
        line = re.sub(r"\(.*?\)", " ", line)
        line = line.split(";", 1)[0]
        line_upper = line.upper()

        g_match = re.match(r"(G\d+\.?\d*)", line_upper)
        if g_match:
            gcode = g_match.group(1)
            if gcode in ("G0", "G00", "G1", "G01", "G2", "G02", "G3", "G03"):
                self.current_mode = gcode
            else:
                return None
        else:
            if self.current_mode and re.search(r"[XYZF]", line_upper):
                gcode = self.current_mode
            else:
                return None

        params = {}
        for axis in ["X", "Y", "Z", "F", "I", "J"]:
            match = re.search(rf"{axis}([-\d.]+)", line_upper)
            if match:
                params[axis] = float(match.group(1))

        if not params:
            return None

        return (gcode, params)

    def _process_rapid(self, params):
        target_x = params.get("X", self.current_x)
        target_y = params.get("Y", self.current_y)
        target_z = params.get("Z", self.current_z)

        if "Z" in params and target_z >= 0:
            self._update_state(params)
            return [self._format_line("G00", params) + "\n"]

        if "Z" in params and target_z < 0:
            z_offset = self.hmap.get_z_offset(target_x, target_y)
            new_params = dict(params)
            new_params["Z"] = self._safe_z(target_z + z_offset, target_x, target_y)
            self._update_state(params)
            return [self._format_line("G00", new_params) + "\n"]

        self._update_state(params)
        return [self._format_line("G00", params) + "\n"]

    def _process_linear(self, params):
        target_x = params.get("X", self.current_x)
        target_y = params.get("Y", self.current_y)
        target_z = params.get("Z", self.current_z)
        feed = params.get("F", self.current_f)

        if len(params) == 1 and "F" in params:
            self.current_f = feed
            return [self._format_line("G01", params) + "\n"]

        if "Z" in params and target_z >= 0 and "X" not in params and "Y" not in params:
            self._update_state(params)
            return [self._format_line("G01", params) + "\n"]

        if "Z" in params and target_z < 0 and "X" not in params and "Y" not in params:
            z_offset = self.hmap.get_z_offset(self.current_x, self.current_y)
            new_params = dict(params)
            new_params["Z"] = self._safe_z(target_z + z_offset, self.current_x, self.current_y)
            self._update_state(params)
            return [self._format_line("G01", new_params) + "\n"]

        if target_z < 0 or self.current_z < 0:
            lines = self._segmented_move(
                self.current_x, self.current_y, self.current_z,
                target_x, target_y, target_z, feed)
            self.current_x = target_x
            self.current_y = target_y
            self.current_z = target_z
            if feed:
                self.current_f = feed
            return lines
        else:
            self._update_state(params)
            return [self._format_line("G01", params) + "\n"]

    def _segmented_move(self, x0, y0, z0, x1, y1, z1, feed):
        dx = x1 - x0
        dy = y1 - y0
        distance = math.sqrt(dx * dx + dy * dy)

        if distance <= self.max_segment:
            z_offset = self.hmap.get_z_offset(x1, y1)
            params = {"X": x1, "Y": y1, "Z": self._safe_z(z1 + z_offset, x1, y1)}
            if feed:
                params["F"] = feed
            return [self._format_line("G01", params) + "\n"]

        num_segments = max(2, int(math.ceil(distance / self.max_segment)))
        lines = []

        for i in range(1, num_segments + 1):
            t = i / num_segments
            xi = x0 + dx * t
            yi = y0 + dy * t
            zi = z0 + (z1 - z0) * t

            z_offset = self.hmap.get_z_offset(xi, yi)

            params = {"X": xi, "Y": yi, "Z": self._safe_z(zi + z_offset, xi, yi)}
            if feed and i == 1:
                params["F"] = feed
            lines.append(self._format_line("G01", params) + "\n")

        return lines

    def _update_state(self, params):
        if "X" in params:
            self.current_x = params["X"]
        if "Y" in params:
            self.current_y = params["Y"]
        if "Z" in params:
            self.current_z = params["Z"]
        if "F" in params:
            self.current_f = params["F"]

    def _format_line(self, gcode, params):
        parts = [gcode]
        if "X" in params:
            parts.append(f"X{params['X']:.4f}")
        if "Y" in params:
            parts.append(f"Y{params['Y']:.4f}")
        if "Z" in params:
            parts.append(f"Z{params['Z']:.4f}")
        if "F" in params:
            parts.append(f"F{params['F']:.2f}")
        if "I" in params:
            parts.append(f"I{params['I']:.4f}")
        if "J" in params:
            parts.append(f"J{params['J']:.4f}")
        return " ".join(parts)