fhd_scan_web_app/backend/camera_params.py

"""Camera exposure/gain parameter handling.

Ported from scan_gui_triple.py's per-camera tab logic:
- load defaults by reading the base params YAML (ros__parameters block)
- "apply": live `ros2 param set <node> <name> <value>` per param, checked
  (subprocess.run with captured output), reporting structured per-param
  success/failure — the triple GUI's more robust approach, preferred here
  over the single/dual GUIs' fire-and-forget version.
- "save": regex text-patch of the base YAML, NOT yaml.dump — the source
  YAML files carry human-authored comments (e.g. timestamp-calibration
  notes) that a full re-dump would destroy.
"""
from __future__ import annotations

import re
import subprocess
from dataclasses import dataclass, asdict
from typing import Optional

import yaml

from config import ROS_SETUP, CAMERA2_WS_SETUP, CameraSpec


@dataclass
class CameraParamValues:
    exposure_auto: bool
    exposure_auto_target_brightness: int
    exposure_auto_min: float
    exposure_auto_max: float
    exposure_time: int
    gain: float

    def as_dict(self) -> dict:
        return asdict(self)


def load_defaults(cam: CameraSpec) -> CameraParamValues:
    p = {}
    try:
        with open(cam.base_params_path, encoding="utf-8") as f:
            cfg = yaml.safe_load(f)
        p = cfg[f"/{cam.node_name}"]["ros__parameters"]
    except Exception:
        pass
    return CameraParamValues(
        exposure_auto=bool(p.get("exposure_auto", False)),
        exposure_auto_target_brightness=int(p.get("exposure_auto_target_brightness", 128)),
        exposure_auto_min=float(p.get("exposure_auto_min", 100.0)),
        exposure_auto_max=float(p.get("exposure_auto_max", 10000.0)),
        exposure_time=int(p.get("exposure_time", 5000)),
        gain=float(p.get("gain", 8.0)),
    )


def write_launch_params_yaml(cam: CameraSpec, values: CameraParamValues, tmp_prefix: str = "/tmp") -> str:
    """Write a temp YAML (base + overridden values) for passing as a launch arg."""
    import tempfile

    try:
        with open(cam.base_params_path, encoding="utf-8") as f:
            cfg = yaml.safe_load(f)
    except Exception:
        cfg = {}
    node_key = f"/{cam.node_name}"
    cfg.setdefault(node_key, {}).setdefault("ros__parameters", {})
    cfg[node_key]["ros__parameters"].update(values.as_dict())

    tf = tempfile.NamedTemporaryFile(
        mode="w", suffix=".yaml", prefix=f"{tmp_prefix}/{cam.id}_params_", delete=False
    )
    yaml.dump(cfg, tf, default_flow_style=False, allow_unicode=True)
    tf.close()
    return tf.name


def _format_param_value(value) -> str:
    if isinstance(value, bool):
        return "true" if value else "false"
    if isinstance(value, float):
        return f"{value:.4f}"
    return str(value)


def apply_params(cam: CameraSpec, values: CameraParamValues) -> list[dict]:
    """Run `ros2 param set` per param, checked. Returns [{name, ok, message}, ...]."""
    node = f"/{cam.node_name}"
    src = f"source {ROS_SETUP} && source {CAMERA2_WS_SETUP}"

    order = ["exposure_auto"]
    if values.exposure_auto:
        order += ["exposure_auto_target_brightness", "exposure_auto_min", "exposure_auto_max"]
    else:
        order += ["exposure_time"]
    order += ["gain"]

    vals = values.as_dict()
    results = []
    for name in order:
        val_str = _format_param_value(vals[name])
        cmd = f"{src} && ros2 param set {node} {name} {val_str}"
        try:
            proc = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=10)
        except subprocess.TimeoutExpired:
            results.append({"name": name, "ok": False, "message": "timeout"})
            continue
        if proc.returncode != 0:
            reason = (proc.stdout.strip() or proc.stderr.strip() or "unknown error")
            results.append({"name": name, "ok": False, "message": reason})
        else:
            results.append({"name": name, "ok": True, "message": "ok"})
    return results


def _patch_yaml_value(text: str, key: str, new_value: str) -> tuple[str, bool]:
    pattern = re.compile(rf"^([ \t]*){re.escape(key)}:([ \t]*)\S+", re.MULTILINE)
    new_text, n = pattern.subn(
        lambda m: f"{m.group(1)}{key}:{m.group(2)}{new_value}", text, count=1,
    )
    return new_text, n > 0


def _insert_missing_yaml_keys(text: str, missing: dict) -> str:
    indent = "    "
    anchor_pos: Optional[int] = None
    for anchor in ("exposure_time", "gain"):
        m = re.search(rf"^([ \t]*){re.escape(anchor)}:", text, re.MULTILINE)
        if m:
            indent = m.group(1)
            anchor_pos = m.start()
            break
    block = "".join(f"{indent}{k}: {v}\n" for k, v in missing.items())
    if anchor_pos is None:
        return text.rstrip("\n") + "\n" + block
    return text[:anchor_pos] + block + text[anchor_pos:]


def save_params(cam: CameraSpec, values: CameraParamValues) -> None:
    """Persist values into the base YAML via comment-preserving text patch."""
    formatted = {
        "exposure_auto": "true" if values.exposure_auto else "false",
        "exposure_auto_target_brightness": str(values.exposure_auto_target_brightness),
        "exposure_auto_min": f"{values.exposure_auto_min:.1f}",
        "exposure_auto_max": f"{values.exposure_auto_max:.1f}",
        "exposure_time": str(values.exposure_time),
        "gain": f"{values.gain:.1f}",
    }
    with open(cam.base_params_path, encoding="utf-8") as f:
        text = f.read()
    missing = {}
    for key, val in formatted.items():
        text, found = _patch_yaml_value(text, key, val)
        if not found:
            missing[key] = val
    if missing:
        text = _insert_missing_yaml_keys(text, missing)
    with open(cam.base_params_path, "w", encoding="utf-8") as f:
        f.write(text)