"""Utilities for seeding VehicleManeuvering control maps with model initial values.

The helpers in this module inspect the OpenPLX signal interface (and, later on,
potentially the scene graph) to recover the starting state of each control. Those
values are then spliced into the control map so VehicleManeuvering pushes the same
state to the simulation queues before the user presses any keys.
"""

import math

from Control.vehicle_maneuvering import VehicleManeuvering

try:
    from agxPythonModules.openplx.control import PyControlInterface
except ImportError:  # pragma: no cover - optional dependency in some environments
    PyControlInterface = None


__all__ = ["prepare_control_map", "create_scalar_reader"]

def _candidate_output_names(input_name):
    """Return plausible output signal names corresponding to a control input."""
    names = [input_name]
    suffix = "_input"
    if input_name.endswith(suffix):
        stem = input_name[: -len(suffix)]
        names.append(stem)
        names.append(f"{stem}_output")
        names.append(f"{stem}_engaged")
    return names


def _ensure_initial_slot(spec, mode, initial_value):
    """Pad a ramp/ramp_reset spec so its trailing entry stores the initial value."""
    target_index = 3 if mode == "ramp" else 4
    spec_parts = list(spec)
    while len(spec_parts) <= target_index:
        spec_parts.append(None)
    spec_parts[target_index] = initial_value
    return tuple(spec_parts)


def _ensure_toggle_initial_state(spec, initial_state):
    """Pad a toggle spec with an initial_state flag."""
    spec_parts = list(spec)
    while len(spec_parts) <= 3:
        spec_parts.append(None)
    spec_parts[3] = bool(initial_state)
    return tuple(spec_parts)


def _ensure_step_initial_value(spec, initial_value):
    """Pad a step spec with an initial discrete value."""
    spec_parts = list(spec)
    while len(spec_parts) <= 4:
        spec_parts.append(None)
    spec_parts[4] = initial_value
    return tuple(spec_parts)


def _safe_read_signal(control_reader, signal_name):
    """Read a signal via PyControlInterface while suppressing empty-payload noise."""
    return control_reader.read(signal_name)

def create_scalar_reader(load_result, signal_interface):
    """Return a callable that reads scalar values for control attrs via PyControlInterface."""
    if PyControlInterface is None or signal_interface is None or load_result is None:
        return None

    try:
        outputs = signal_interface.getOutputs()
    except Exception as exc:  # pragma: no cover - defensive
        print(f"Unable to query signal outputs for runtime reads: {exc}")
        return None

    if not outputs:
        return None

    try:
        control_reader = PyControlInterface(load_result, signal_interface, True)
    except Exception as exc:  # pragma: no cover - defensive
        print(f"Unable to create control interface reader for runtime reads: {exc}")
        return None

    output_names = set(outputs.keys())

    def lookup_scalar(attr):
        for candidate in _candidate_output_names(attr):
            if candidate not in output_names:
                continue
            raw_value = _safe_read_signal(control_reader, candidate)
            if raw_value is None:
                continue

            value = raw_value
            if value is not None and math.isfinite(value):
                return value

        return None

    return lookup_scalar


def prepare_control_map(control_map, load_result, signal_interface):
    """Return a copy of the control map with initial values injected when available."""
    if PyControlInterface is None or signal_interface is None or load_result is None:
        return control_map

    try:
        outputs = signal_interface.getOutputs()
    except Exception as exc:  # pragma: no cover - defensive
        print(f"Unable to query signal outputs for initial values: {exc}")
        return control_map

    if not outputs:
        return control_map

    try:
        control_reader = PyControlInterface(load_result, signal_interface, True)
    except Exception as exc:  # pragma: no cover - defensive
        print(f"Unable to create control interface reader for initial values: {exc}")
        return control_map

    output_names = set(outputs.keys())
    scalar_cache = {}
    bool_cache = {}

    def lookup_scalar(attr):
        if attr in scalar_cache:
            return scalar_cache[attr]

        for candidate in _candidate_output_names(attr):
            if candidate not in output_names:
                continue
            raw_value = _safe_read_signal(control_reader, candidate)
            if raw_value is None:
                continue

            value = raw_value
            if value is not None and math.isfinite(value):
                scalar_cache[attr] = value
                return value

        scalar_cache[attr] = None
        return None

    def lookup_bool(attr):
        if attr in bool_cache:
            return bool_cache[attr]

        for candidate in _candidate_output_names(attr):
            if candidate not in output_names:
                continue
            raw_value = _safe_read_signal(control_reader, candidate)
            if raw_value is None:
                continue

            value = raw_value
            if value is not None:
                bool_cache[attr] = value
                return value

        bool_cache[attr] = None
        return None

    mutated_rows = []
    mutated = False
    control_map_is_tuple = isinstance(control_map, tuple)

    for row in control_map:
        row_kind = VehicleManeuvering._row_kind(row)
        if row_kind == "single":
            key, specs, mode = row
            new_specs = []
            specs_mutated = False

            if mode in ("ramp", "ramp_reset"):
                for spec in specs:
                    attr = spec[0]
                    initial_value = lookup_scalar(attr)
                    if initial_value is not None:
                        new_specs.append(_ensure_initial_slot(spec, mode, initial_value))
                        specs_mutated = True
                    else:
                        new_specs.append(spec)
            elif mode == "toggle":
                for spec in specs:
                    attr = spec[0]
                    initial_state = lookup_bool(attr)
                    if initial_state is not None:
                        new_specs.append(_ensure_toggle_initial_state(spec, initial_state))
                        specs_mutated = True
                    else:
                        new_specs.append(spec)
            else:
                mutated_rows.append(row)
                continue

            if specs_mutated:
                mutated = True
                specs_container = tuple(new_specs) if isinstance(specs, tuple) else new_specs
                mutated_rows.append((key, specs_container, mode))
            else:
                mutated_rows.append(row)
        elif row_kind == "pair_step":
            key_inc, key_dec, specs, tag = row
            new_specs = []
            specs_mutated = False
            for spec in specs:
                attr = spec[0]
                initial_value = lookup_scalar(attr)
                if initial_value is not None:
                    new_specs.append(_ensure_step_initial_value(spec, initial_value))
                    specs_mutated = True
                else:
                    new_specs.append(spec)

            if specs_mutated:
                mutated = True
                specs_container = tuple(new_specs) if isinstance(specs, tuple) else new_specs
                mutated_rows.append((key_inc, key_dec, specs_container, tag))
            else:
                mutated_rows.append(row)
        else:
            mutated_rows.append(row)

    if not mutated:
        return control_map

    return tuple(mutated_rows) if control_map_is_tuple else mutated_rows
