radar_system/python_app/hardware_full/switched_matrix_radar_service.py

"""Matrix radar behind real GPIO switches on the stimulus and/or receiver path."""

from __future__ import annotations

from dataclasses import dataclass, field, replace
import logging
import time

from python_app.hardware_full.matrix_radar_service import MatrixRadarService
from python_app.hardware_full.switch_service import SwitchService
from python_app.models.dataset_model import ComboKey, SweepCollection, TraceData
from python_app.models.run_config_model import RadarSweepModel, SwitchModel

logger = logging.getLogger(__name__)


@dataclass(slots=True)
class SwitchedMatrixRadarService:
    """Widen a matrix radar's combo matrix with real switch positions.

    Implements the ``MatrixRadarService`` protocol, so the producer and the
    capture workflows treat it as an ordinary matrix radar that simply reports
    more positions. The hardware sweep is never stopped: switches are only ever
    driven BETWEEN ``acquire_collection`` calls, and the inner service's
    free-running collection discards any partially swept cycle.
    """

    inner: MatrixRadarService
    output_switch: SwitchService | None
    input_switch: SwitchService | None
    inner_output_positions: int
    inner_input_positions: int
    settling_ms: int = 0
    # Monotonic end of the previous inner collection, so the DEBUG timing trace can
    # report how long the gap between "sweep collected" and "switch driven" really is
    # — that gap is where a stale in-flight point 0 can still slip past the drain.
    _last_inner_end_ns: int = field(init=False, default=0, repr=False)

    def open(self) -> None:
        """Open the inner radar and both switches."""
        self.inner.open()
        if self.output_switch is not None:
            self.output_switch.open()
        if self.input_switch is not None:
            self.input_switch.open()

    def close(self) -> None:
        """Close switches first, then the inner radar; never raises."""
        for switch in (self.input_switch, self.output_switch):
            if switch is not None:
                try:
                    switch.close()
                except Exception as exc:  # noqa: BLE001 — shutdown path
                    logger.warning("Switch close ignored error: %s", exc)
        self.inner.close()

    def configure(self, sweep: RadarSweepModel) -> None:
        """Apply sweep settings to the inner radar."""
        self.inner.configure(sweep)

    def recover(self) -> None:
        """Reconnect the inner radar; switches are not on the USB transport."""
        self.inner.recover()

    def acquire_collection(self, collection_id: int = 1) -> SweepCollection:
        """Acquire the full widened matrix, one inner collection per switch step.

        A partial failure raises instead of returning a short collection: the
        preprocessor requires every runtime combo to be present, so half a matrix
        is worse than a dropped frame.
        """
        capture_start_ns = time.monotonic_ns()
        out_steps = self.output_switch.position_count() if self.output_switch is not None else 1
        in_steps = self.input_switch.position_count() if self.input_switch is not None else 1
        total_inputs = in_steps * self.inner_input_positions
        total_outputs = out_steps * self.inner_output_positions

        # Place each trace at its canonical index rather than appending. The GPR stage
        # rejects a collection whose trace order differs from run.combos, and run.combos
        # is built output-major (`build_full_combos`) while these loops run switch-major.
        # Appending happens to agree for an output switch and to disagree for an input one.
        slots: list[TraceData | None] = [None] * (total_inputs * total_outputs)

        for out_k in range(out_steps):
            for in_k in range(in_steps):
                for trace in self._acquire_step_traces(out_k, in_k, collection_id):
                    slots[trace.combo.output * total_inputs + trace.combo.input] = trace

        if any(trace is None for trace in slots):
            missing = sum(1 for trace in slots if trace is None)
            raise RuntimeError(
                f"Switched matrix collection is incomplete: {missing} of {len(slots)} combos missing"
            )

        return SweepCollection(
            collection_id=int(collection_id),
            monotonic_ns=time.monotonic_ns(),
            traces=[trace for trace in slots if trace is not None],
            capture_start_ns=capture_start_ns,
            capture_end_ns=time.monotonic_ns(),
        )

    def acquire_combo_collection(
        self,
        *,
        input_pos: int,
        output_pos: int,
        collection_id: int = 1,
    ) -> SweepCollection:
        """Acquire only the physical switch step that carries one widened combo.

        The per-combo capture workflows need a single trace at a time; sweeping
        every switch position for that (a full ``acquire_collection``) multiplies
        the capture time by the number of physical steps and freezes the caller
        for the whole sweep. One widened combo lives entirely inside one
        (out_k, in_k) step, so acquiring just that step is sufficient. The result
        contains that step's traces with widened combo keys, including the
        requested combo.
        """
        out_steps = self.output_switch.position_count() if self.output_switch is not None else 1
        in_steps = self.input_switch.position_count() if self.input_switch is not None else 1
        total_inputs = in_steps * self.inner_input_positions
        total_outputs = out_steps * self.inner_output_positions
        if not (0 <= int(input_pos) < total_inputs and 0 <= int(output_pos) < total_outputs):
            raise ValueError(
                f"Widened combo out of range: input={input_pos} (of {total_inputs}), "
                f"output={output_pos} (of {total_outputs})"
            )

        capture_start_ns = time.monotonic_ns()
        out_k = int(output_pos) // self.inner_output_positions
        in_k = int(input_pos) // self.inner_input_positions
        traces = self._acquire_step_traces(out_k, in_k, collection_id)
        return SweepCollection(
            collection_id=int(collection_id),
            monotonic_ns=time.monotonic_ns(),
            traces=traces,
            capture_start_ns=capture_start_ns,
            capture_end_ns=time.monotonic_ns(),
        )

    def _acquire_step_traces(self, out_k: int, in_k: int, collection_id: int) -> list[TraceData]:
        """Drive both switches to one step, settle, and collect its widened traces.

        Every returned trace carries the monotonic window of the inner collection
        that produced it, so a consumer can tell when each combo of a switched
        matrix was really measured instead of only when the whole cycle began and
        ended. The switch drive and settling are deliberately outside the window.
        """
        step_start_ns = time.monotonic_ns()
        if self.output_switch is not None:
            self.output_switch.switch_to(out_k)
        if self.input_switch is not None:
            self.input_switch.switch_to(in_k)
        switched_ns = time.monotonic_ns()
        # Settle AFTER the last switch change and BEFORE collecting, so the
        # cycle we anchor on starts with the RF path already stable.
        if self.settling_ms > 0:
            time.sleep(self.settling_ms / 1000.0)
        settled_ns = time.monotonic_ns()

        sub = self.inner.acquire_collection(collection_id)
        inner_end_ns = time.monotonic_ns()
        logger.debug(
            "timing: collection %d step out=%d in=%d | gap_prev_collect_to_switch=%s ms, "
            "switch=%.3f ms, settle=%.2f ms, inner_collect=%.2f ms",
            collection_id,
            out_k,
            in_k,
            (
                f"{(step_start_ns - self._last_inner_end_ns) / 1e6:.2f}"
                if self._last_inner_end_ns
                else "n/a"
            ),
            (switched_ns - step_start_ns) / 1e6,
            (settled_ns - switched_ns) / 1e6,
            (inner_end_ns - settled_ns) / 1e6,
        )
        self._last_inner_end_ns = inner_end_ns
        return [
            replace(
                trace,
                combo=ComboKey(
                    input=in_k * self.inner_input_positions + int(trace.combo.input),
                    output=out_k * self.inner_output_positions + int(trace.combo.output),
                ),
                # Keep the inner service's own per-trace window when it reports one
                # (it knows its internal port order better than this step does);
                # otherwise fall back to the window of this inner collection.
                capture_start_ns=int(trace.capture_start_ns) or settled_ns,
                capture_end_ns=int(trace.capture_end_ns) or inner_end_ns,
            )
            for trace in sub.traces
        ]


def build_physical_switch(
    model: SwitchModel,
    physical_positions: int,
    radar_driver_mode: str,
) -> SwitchService | None:
    """Build the driver for a real switch described by a virtual switch section.

    The config section carries the LOGICAL axis size and a forced "mock" mode so
    the C++ loader accepts it; the real driver needs the PHYSICAL position count
    and native mode. Mock radar runs keep mock switches so the whole path can be
    exercised without GPIO.
    """
    if physical_positions <= 1:
        return None
    driver_mode = "mock" if radar_driver_mode.strip().lower() == "mock" else "native"
    return SwitchService.from_model(
        replace(model, positions=physical_positions, driver_mode=driver_mode)
    )