#!/usr/bin/env python3
# DIAMETRAL AC250K1D serial protocol summary:
# - Serial link: 9600 baud, 8 data bits, no parity, 1 stop bit, no HW/SW flow control.
# - Command frame: @ + AA + PPP + PARAMETERS + BCC + CR.
# - Response frame: # + AA + DATA + optional CR.
# - AA is the device address in two uppercase hexadecimal ASCII digits, usually 0A.
# - PPP is a three-character ASCII command code.
# - PARAMETERS depend on the command.
# - BCC is the modulo-256 sum of ASCII bytes from AA through the last parameter byte.
# - BCC does not include @, the BCC field itself, or the trailing CR.
# - Formula: sum(payload.encode("ascii")) & 0xFF, formatted as two uppercase hex digits.
# - Supported commands in this script:
#   - NAPxxx: set voltage in volts, zero-padded to three digits.
#   - NAP???: read measured REAL voltage.
#   - OUT1: enable output.
#   - OUT0: disable output.
#   - OUT?: read output state.
#   - ID?: read model and serial string.
"""Relay-aware demonstration controller for the DIAMETRAL AC250K1D."""

from __future__ import annotations

import argparse
import os
import statistics
import sys
import time
from collections import deque
from dataclasses import dataclass
from typing import Deque

try:
    from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE, Serial, SerialException
except ImportError as exc:
    raise SystemExit("PySerial is required. Install it with: python3 -m pip install pyserial") from exc


DEFAULT_PORT = "COM21" if os.name == "nt" else "/dev/ttyUSB0"
DEFAULT_ADDRESS = 10
DEFAULT_BAUD_RATE = 9600
MIN_VOLTAGE_V = 0
MAX_VOLTAGE_V = 250
MAX_RESPONSE_BYTES = 256


class ProtocolError(RuntimeError):
    """Report an invalid or unexpected device response."""


class ControlError(RuntimeError):
    """Report a control-loop or safety failure."""


@dataclass(frozen=True)
class SerialConfig:
    """Serial-port configuration."""

    port: str = DEFAULT_PORT
    address: int = DEFAULT_ADDRESS
    baud_rate: int = DEFAULT_BAUD_RATE
    timeout_s: float = 0.8
    inter_byte_timeout_s: float = 0.25
    minimum_command_interval_s: float = 0.25
    tail_read_timeout_s: float = 0.3
    output_settle_s: float = 0.5


@dataclass(frozen=True)
class RelayPolicy:
    """Relay-wear and voltage-control policy."""

    tolerance_v: int = 2
    stable_cycles: int = 2
    measurement_samples: int = 3
    measurement_spacing_s: float = 0.25
    settle_after_set_s: float = 2.0
    post_enable_settle_s: float = 1.0
    max_initial_adjustments: int = 3
    tuning_max_step_v: int = 25
    regulation_max_step_v: int = 5
    max_set_offset_v: int = 30
    reversal_guard_v: int = 2
    regulation_sample_interval_s: float = 5.0
    regulation_min_adjustment_interval_s: float = 60.0
    regulation_required_bad_cycles: int = 3
    max_adjustments_per_hour: int = 6
    emergency_deviation_v: int = 20
    model_min_span_v: int = 4
    model_gain_min: float = 0.5
    model_gain_max: float = 1.5
    model_history_size: int = 8
    regulation_hysteresis_v: int = 2
    regulation_large_error_threshold_v: int = 10
    regulation_large_error_max_step_v: int = 10


@dataclass(frozen=True)
class VoltageState:
    """Last commanded SET value and measured REAL value."""

    device_set_voltage_v: int
    target_real_voltage_v: int
    real_voltage_v: int

    @property
    def error_v(self) -> int:
        """Return target minus measured voltage."""
        return self.target_real_voltage_v - self.real_voltage_v


def calculate_bcc(payload: str) -> str:
    """Return the two-character ASCII BCC value."""
    return f"{sum(payload.encode('ascii')) & 0xFF:02X}"


def build_frame(address: int, command: str, parameters: str) -> bytes:
    """Build one DIAMETRAL command frame."""
    if not 0 <= address <= 0x1F:
        raise ValueError("Address must be between 0 and 31.")
    if len(command) != 3:
        raise ValueError("Command must contain exactly three characters.")
    if "@" in command or "#" in command or "@" in parameters or "#" in parameters:
        raise ValueError("Command and parameters must not contain '@' or '#'.")

    payload = f"{address:02X}{command}{parameters}"
    return f"@{payload}{calculate_bcc(payload)}\r".encode("ascii")


def format_ascii(data: bytes) -> str:
    """Format control characters without changing terminal layout."""
    return (
        data.decode("ascii", errors="backslashreplace")
        .replace("\\", "\\\\")
        .replace("\r", "\\r")
        .replace("\n", "\\n")
    )


def clamp(value: int, minimum: int, maximum: int) -> int:
    """Clamp an integer to an inclusive range."""
    return max(minimum, min(value, maximum))


def direction(value: int) -> int:
    """Return the sign of an integer."""
    return (value > 0) - (value < 0)


class DiametralAc250K1D:
    """Minimal AC250K1D protocol client."""

    def __init__(self, config: SerialConfig, verbose: bool = False) -> None:
        self._config = config
        self._verbose = verbose
        self._serial: Serial | None = None
        self._last_set_voltage_v: int | None = None
        self._last_command_completed_at = 0.0

    @property
    def last_set_voltage_v(self) -> int | None:
        """Return the last SET value sent by this process."""
        return self._last_set_voltage_v

    def __enter__(self) -> DiametralAc250K1D:
        self.open()
        return self

    def __exit__(self, _exc_type: object, _exc_value: object, _traceback: object) -> None:
        self.close()

    def open(self) -> None:
        """Open the configured serial port."""
        if self._serial is not None:
            return

        serial_kwargs = dict(
            port=self._config.port,
            baudrate=self._config.baud_rate,
            bytesize=EIGHTBITS,
            parity=PARITY_NONE,
            stopbits=STOPBITS_ONE,
            timeout=0,
            write_timeout=self._config.timeout_s,
            xonxoff=False,
            rtscts=False,
            dsrdtr=False,
        )

        try:
            self._serial = Serial(exclusive=True, **serial_kwargs)
        except TypeError:
            self._serial = Serial(**serial_kwargs)

    def close(self) -> None:
        """Close the serial port."""
        if self._serial is not None:
            self._serial.close()
            self._serial = None

    def set_voltage(self, voltage_v: int) -> None:
        """Set the device SET value."""
        if not MIN_VOLTAGE_V <= voltage_v <= MAX_VOLTAGE_V:
            raise ValueError(f"Voltage must be between {MIN_VOLTAGE_V} and {MAX_VOLTAGE_V} V.")

        response = self._exchange("NAP", f"{voltage_v:03d}")
        self._expect_ok(response)
        self._last_set_voltage_v = voltage_v

    def read_real_voltage(self) -> int:
        """Read the measured REAL voltage."""
        response = self._exchange("NAP", "???")
        prefix = f"#{self._config.address:02X}NAP".encode("ascii")
        normalized = response.rstrip(b"\r")

        if not normalized.startswith(prefix):
            raise ProtocolError(f"Unexpected REAL voltage response: {format_ascii(response)}")

        value = normalized[len(prefix):]
        if len(value) != 3 or not value.isdigit():
            raise ProtocolError(f"Invalid REAL voltage value: {format_ascii(response)}")

        return int(value.decode("ascii"))

    def output_on(self) -> None:
        """Enable the output."""
        response = self._exchange("OUT", "1")
        self._expect_ok(response)
        time.sleep(self._config.output_settle_s)

    def output_off(self) -> None:
        """Disable the output."""
        response = self._exchange("OUT", "0")
        self._expect_ok(response)
        time.sleep(self._config.output_settle_s)

    def read_output_enabled(self) -> bool:
        """Read the output state."""
        response = self._exchange("OUT", "?")
        expected_prefix = f"#{self._config.address:02X}OUT".encode("ascii")
        normalized = response.rstrip(b"\r")

        if not normalized.startswith(expected_prefix):
            raise ProtocolError(f"Unexpected output response: {format_ascii(response)}")

        value = normalized[len(expected_prefix):]
        if value not in (b"0", b"1"):
            raise ProtocolError(f"Invalid output state: {format_ascii(response)}")

        return value == b"1"

    def _expect_ok(self, response: bytes) -> None:
        expected = f"#{self._config.address:02X}OK".encode("ascii")
        normalized = response.rstrip(b"\r")

        if normalized == expected:
            return
        if normalized == f"#{self._config.address:02X}Err".encode("ascii"):
            raise ProtocolError("The device rejected the command.")

        raise ProtocolError(f"Unexpected acknowledgement: {format_ascii(response)}")

    def _exchange(self, command: str, parameters: str) -> bytes:
        serial_port = self._require_serial()
        frame = build_frame(self._config.address, command, parameters)
        read_only = self._is_read_only_command(command, parameters)
        max_attempts = 2 if read_only else 1
        last_response = b""
        last_termination = "RESPONSE_START_TIMEOUT"
        last_complete = False

        for attempt in range(max_attempts):
            self._respect_minimum_command_interval()
            stray_data = self._drain_pre_tx_stray(serial_port)
            if self._verbose:
                print(f"PRE-TX STRAY ASCII: {format_ascii(stray_data) if stray_data else '<empty>'}")
                print(f"PRE-TX STRAY HEX:   {stray_data.hex(' ') if stray_data else '<empty>'}")
                if attempt:
                    print(f"RETRY: {attempt}/{max_attempts - 1}")

            started_at = time.monotonic()
            serial_port.write(frame)
            serial_port.flush()
            response, termination, complete = self._read_response(command, parameters)
            elapsed_s = time.monotonic() - started_at
            self._last_command_completed_at = time.monotonic()

            if self._verbose:
                print(f"TX ASCII: {format_ascii(frame)}")
                print(f"TX HEX:   {frame.hex(' ')}")
                print(f"RX ASCII: {format_ascii(response) if response else '<empty>'}")
                print(f"RX HEX:   {response.hex(' ') if response else '<empty>'}")
                print(f"RX TERMINATION: {termination}")
                print(f"RX COMPLETE: {'YES' if complete else 'NO'}")
                print(f"TIME:     {elapsed_s:.3f} s")

            if complete:
                return response

            last_response = response
            last_termination = termination
            last_complete = complete

        if not last_response and last_termination == "RESPONSE_START_TIMEOUT":
            raise ProtocolError("No response received.")
        raise ProtocolError(f"Incomplete response: {format_ascii(last_response)}")

    def _read_response(self, command: str, parameters: str) -> tuple[bytes, str, bool]:
        serial_port = self._require_serial()
        response = bytearray()
        start_deadline = time.monotonic() + self._config.timeout_s

        while time.monotonic() < start_deadline and len(response) < MAX_RESPONSE_BYTES:
            waiting = serial_port.in_waiting
            if waiting:
                response.extend(serial_port.read(min(waiting, MAX_RESPONSE_BYTES - len(response))))
                break
            time.sleep(0.005)

        if not response:
            return b"", "RESPONSE_START_TIMEOUT", False

        response, termination = self._read_until_complete_or_timeout(serial_port, response)
        complete = self._response_is_complete(command, parameters, response, termination)
        if complete:
            return response, termination, True

        if response and b"\r" not in response:
            response, termination = self._tail_read(serial_port, response)
            complete = self._response_is_complete(command, parameters, response, termination)

        return response, termination, complete

    def _read_until_complete_or_timeout(self, serial_port: Serial, response: bytearray) -> tuple[bytes, str]:
        if b"\r" in response:
            return bytes(response[: response.index(b"\r") + 1]), "CR"

        inter_deadline = time.monotonic() + self._config.inter_byte_timeout_s
        while time.monotonic() < inter_deadline and len(response) < MAX_RESPONSE_BYTES:
            waiting = serial_port.in_waiting
            if waiting:
                response.extend(serial_port.read(min(waiting, MAX_RESPONSE_BYTES - len(response))))
                if b"\r" in response:
                    return bytes(response[: response.index(b"\r") + 1]), "CR"
                inter_deadline = time.monotonic() + self._config.inter_byte_timeout_s
                continue
            time.sleep(0.005)

        return bytes(response), "INTER_BYTE_TIMEOUT"

    def _tail_read(self, serial_port: Serial, response: bytes) -> tuple[bytes, str]:
        combined = bytearray(response)
        tail_deadline = time.monotonic() + self._config.tail_read_timeout_s
        while time.monotonic() < tail_deadline and len(combined) < MAX_RESPONSE_BYTES:
            waiting = serial_port.in_waiting
            if waiting:
                combined.extend(serial_port.read(min(waiting, MAX_RESPONSE_BYTES - len(combined))))
                if b"\r" in combined:
                    return bytes(combined[: combined.index(b"\r") + 1]), "CR"
                tail_deadline = time.monotonic() + self._config.inter_byte_timeout_s
                continue
            time.sleep(0.005)

        return bytes(combined), "INTER_BYTE_TIMEOUT"

    def _drain_pre_tx_stray(self, serial_port: Serial) -> bytes:
        stray = bytearray()
        deadline = time.monotonic() + 0.05
        while time.monotonic() < deadline and len(stray) < MAX_RESPONSE_BYTES:
            waiting = serial_port.in_waiting
            if waiting:
                stray.extend(serial_port.read(min(waiting, MAX_RESPONSE_BYTES - len(stray))))
                deadline = time.monotonic() + 0.02
                continue
            time.sleep(0.005)
        return bytes(stray)

    def _respect_minimum_command_interval(self) -> None:
        elapsed_s = time.monotonic() - self._last_command_completed_at
        remaining_s = self._config.minimum_command_interval_s - elapsed_s
        if remaining_s > 0:
            time.sleep(remaining_s)

    def _is_read_only_command(self, command: str, parameters: str) -> bool:
        return (
            (command == "NAP" and parameters == "???")
            or (command == "OUT" and parameters == "?")
            or (command == "STA" and parameters == "?")
            or (command == "TOL" and parameters == "?")
            or (command == "ID?" and parameters == "")
        )

    def _response_is_complete(self, command: str, parameters: str, response: bytes, termination: str) -> bool:
        if not response:
            return False

        prefix = f"#{self._config.address:02X}".encode("ascii")
        normalized = response.rstrip(b"\r")

        if command == "ID?" and parameters == "":
            return normalized.startswith(prefix + b"TYP")
        if command == "NAP" and parameters == "???":
            value = normalized[len(prefix + b"NAP") :] if normalized.startswith(prefix + b"NAP") else b""
            return normalized.startswith(prefix + b"NAP") and len(value) == 3 and value.isdigit()
        if command == "OUT" and parameters == "?":
            return normalized in {prefix + b"OUT0", prefix + b"OUT1"}
        if command in {"OUT", "NAP", "STA"} and parameters != "?":
            return normalized in {prefix + b"OK", prefix + b"Err"} and response.endswith(b"\r")
        if command == "TOL" and parameters == "?":
            return normalized.startswith(prefix + b"TOL")
        return termination == "CR"

    def _require_serial(self) -> Serial:
        if self._serial is None:
            raise RuntimeError("The serial port is not open.")

        return self._serial


class RelayAwareVoltageController:
    """External voltage controller with relay-wear limits."""

    def __init__(self, device: DiametralAc250K1D, policy: RelayPolicy) -> None:
        self._device = device
        self._policy = policy
        self._adjustment_times: Deque[float] = deque()
        self._observations: Deque[tuple[int, int]] = deque(maxlen=self._policy.model_history_size)
        self._last_adjustment_time = 0.0
        self._last_adjustment_direction = 0
        self._last_model_gain = 1.0
        self._last_model_offset = 0.0
        self._last_model_samples = 0

    def set_and_measure(self, target_voltage_v: int) -> VoltageState:
        """Set voltage with the output off and read REAL voltage."""
        self._validate_target(target_voltage_v)
        self._device.output_off()
        self._device.set_voltage(target_voltage_v)
        time.sleep(self._policy.settle_after_set_s)
        real_voltage_v = self._read_filtered_real_voltage()
        self._record_observation(target_voltage_v, real_voltage_v)

        return VoltageState(
            device_set_voltage_v=target_voltage_v,
            target_real_voltage_v=target_voltage_v,
            real_voltage_v=real_voltage_v,
        )

    def tune_with_output_off(self, target_voltage_v: int) -> VoltageState:
        """Tune the device SET value while keeping the output disabled."""
        self._validate_target(target_voltage_v)
        self._device.output_off()

        current_set_v = target_voltage_v
        self._device.set_voltage(current_set_v)
        stable_cycles = 0
        hold_cycles = 0
        adjustments = 0
        used_setpoints = {current_set_v}

        while True:
            time.sleep(self._policy.settle_after_set_s)
            real_voltage_v = self._read_filtered_real_voltage()
            state = VoltageState(current_set_v, target_voltage_v, real_voltage_v)
            self._record_observation(current_set_v, real_voltage_v)
            self.print_state("TUNE", state)

            if abs(state.error_v) <= self._policy.tolerance_v:
                stable_cycles += 1
                hold_cycles = 0
                self.print_state("TUNE", state, "no action needed")
                if stable_cycles >= self._policy.stable_cycles:
                    return state
                continue

            stable_cycles = 0
            if adjustments >= self._policy.max_initial_adjustments:
                raise ControlError(
                    f"Initial tuning failed after {adjustments} adjustments. "
                    f"Observations: {self._format_observations()}"
                )

            next_set_v = self._predict_next_set(state, self._policy.tuning_max_step_v)
            self._print_model_prediction(state, next_set_v)

            if next_set_v == current_set_v:
                if self._can_accept_tuning_hold(state):
                    hold_cycles += 1
                    self.print_state("TUNE", state, "blocked by reversal guard")
                    if hold_cycles >= self._policy.stable_cycles:
                        saved_direction = self._last_adjustment_direction
                        self._last_adjustment_direction = 0
                        next_set_v = self._predict_next_set(
                            state,
                            min(self._policy.tuning_max_step_v, 2),
                        )
                        self._last_adjustment_direction = saved_direction
                        self._print_model_prediction(state, next_set_v)
                        if next_set_v == current_set_v:
                            continue
                        hold_cycles = 0
                    else:
                        continue
                if next_set_v == current_set_v:
                    raise ControlError(
                    "Initial tuning cannot derive a new SET value. "
                    f"Observations: {self._format_observations()}"
                )
            if next_set_v in used_setpoints:
                raise ControlError(
                    "Initial tuning would repeat a previous SET value. "
                    f"Observations: {self._format_observations()}"
                )

            hold_cycles = 0
            self._apply_set_adjustment(current_set_v, next_set_v)
            current_set_v = next_set_v
            used_setpoints.add(current_set_v)
            adjustments += 1

    def enable_after_tuning(self, state: VoltageState) -> VoltageState:
        """Enable the output after tuning and verify the result."""
        self._device.output_on()

        if not self._device.read_output_enabled():
            self._device.output_off()
            raise ControlError("The device did not report an enabled output.")

        time.sleep(self._policy.post_enable_settle_s)
        real_voltage_v = self._read_filtered_real_voltage()
        enabled_state = VoltageState(
            device_set_voltage_v=state.device_set_voltage_v,
            target_real_voltage_v=state.target_real_voltage_v,
            real_voltage_v=real_voltage_v,
        )
        self.print_state("ENABLED", enabled_state)

        if abs(enabled_state.error_v) > self._policy.emergency_deviation_v:
            self._device.output_off()
            raise ControlError("REAL voltage exceeded the emergency deviation after enabling.")

        return enabled_state

    def tune_and_enable(self, target_voltage_v: int) -> VoltageState:
        """Tune with the output off and enable only after stable measurements."""
        return self.enable_after_tuning(self.tune_with_output_off(target_voltage_v))

    def regulate(self, target_voltage_v: int) -> None:
        """Maintain REAL voltage while limiting relay operations."""
        state = self.tune_and_enable(target_voltage_v)
        current_set_v = state.device_set_voltage_v
        bad_cycles = 0

        while True:
            time.sleep(self._policy.regulation_sample_interval_s)
            real_voltage_v = self._read_filtered_real_voltage()
            state = VoltageState(current_set_v, target_voltage_v, real_voltage_v)
            self._record_observation(current_set_v, real_voltage_v)

            if abs(state.error_v) > self._policy.emergency_deviation_v:
                self._device.output_off()
                raise ControlError("REAL voltage exceeded the emergency deviation.")

            error_state = self._classify_regulation_error(state.error_v)
            if error_state == "stable":
                bad_cycles = 0
                self.print_state("REGULATE", state, "no action needed")
                continue
            if error_state == "hysteresis":
                self.print_state("REGULATE", state, "blocked by hysteresis")
                continue

            bad_cycles += 1
            if bad_cycles < self._policy.regulation_required_bad_cycles:
                self.print_state("REGULATE", state, "blocked by bad-cycle threshold")
                continue

            blocked_reason = self._regulation_block_reason()
            if blocked_reason is not None:
                self.print_state("REGULATE", state, blocked_reason)
                continue

            maximum_step_v = self._policy.regulation_max_step_v
            if abs(state.error_v) >= self._policy.regulation_large_error_threshold_v:
                maximum_step_v = self._policy.regulation_large_error_max_step_v

            next_set_v = self._predict_next_set(state, maximum_step_v)
            self._print_model_prediction(state, next_set_v)
            if next_set_v == current_set_v:
                self.print_state("REGULATE", state, "blocked by prediction limit")
                continue

            self.print_state("REGULATE", state, "intervention applied")
            self._apply_set_adjustment(current_set_v, next_set_v)
            current_set_v = next_set_v
            time.sleep(self._policy.settle_after_set_s)
            real_voltage_v = self._read_filtered_real_voltage()
            state = VoltageState(current_set_v, target_voltage_v, real_voltage_v)
            self._record_observation(current_set_v, real_voltage_v)
            self.print_state("VERIFY", state, "post-adjust check")
            if abs(state.error_v) > self._policy.emergency_deviation_v:
                self._device.output_off()
                raise ControlError("REAL voltage exceeded the emergency deviation.")
            bad_cycles = 0

    def _read_filtered_real_voltage(self) -> int:
        samples = []

        for sample_index in range(self._policy.measurement_samples):
            samples.append(self._device.read_real_voltage())

            if sample_index + 1 < self._policy.measurement_samples:
                time.sleep(self._policy.measurement_spacing_s)

        return round(statistics.median(samples))

    def _record_observation(self, set_voltage_v: int, real_voltage_v: int) -> None:
        """Store one filtered SET-to-REAL observation."""
        self._observations.append((set_voltage_v, real_voltage_v))

    def _calculate_model(self) -> tuple[float, float, int]:
        samples = len(self._observations)
        if samples == 0:
            gain = 1.0
            offset = 0.0
        elif samples == 1:
            set_voltage_v, real_voltage_v = self._observations[0]
            gain = 1.0
            offset = real_voltage_v - set_voltage_v
        else:
            best_pair: tuple[tuple[int, int], tuple[int, int]] | None = None
            best_span = -1
            observations = list(self._observations)
            for index, left in enumerate(observations):
                for right in observations[index + 1 :]:
                    span = abs(right[0] - left[0])
                    if span > best_span:
                        best_span = span
                        best_pair = (left, right)

            gain = 1.0
            if best_pair is not None and best_span >= self._policy.model_min_span_v:
                set_1, real_1 = best_pair[0]
                set_2, real_2 = best_pair[1]
                raw_gain = (real_2 - real_1) / (set_2 - set_1)
                if self._policy.model_gain_min <= raw_gain <= self._policy.model_gain_max:
                    gain = raw_gain

            offsets = [real_voltage_v - gain * set_voltage_v for set_voltage_v, real_voltage_v in observations]
            offset = statistics.median(offsets)

        self._last_model_gain = gain
        self._last_model_offset = offset
        self._last_model_samples = samples
        return gain, offset, samples

    def _predict_next_set(
        self,
        state: VoltageState,
        maximum_step_v: int,
    ) -> int:
        gain, offset, _samples = self._calculate_model()

        if gain == 0:
            predicted_set_v = state.device_set_voltage_v
        else:
            predicted_set_v = round((state.target_real_voltage_v - offset) / gain)

        predicted_set_v = self._apply_prediction_limits(state, predicted_set_v, maximum_step_v)
        if predicted_set_v == state.device_set_voltage_v and abs(state.error_v) > self._policy.tolerance_v:
            if state.error_v > 0:
                predicted_set_v += 1
            elif state.error_v < 0:
                predicted_set_v -= 1
            predicted_set_v = self._apply_prediction_limits(state, predicted_set_v, maximum_step_v)

        predicted_direction = direction(predicted_set_v - state.device_set_voltage_v)
        if (
            self._last_adjustment_direction
            and predicted_direction
            and predicted_direction != self._last_adjustment_direction
        ):
            if abs(state.error_v) <= self._policy.tolerance_v + self._policy.reversal_guard_v:
                return state.device_set_voltage_v

            reversal_step_v = min(maximum_step_v, 2)
            predicted_set_v = self._apply_prediction_limits(state, predicted_set_v, reversal_step_v)
            if predicted_set_v == state.device_set_voltage_v and abs(state.error_v) > self._policy.tolerance_v:
                if state.error_v > 0:
                    predicted_set_v += 1
                elif state.error_v < 0:
                    predicted_set_v -= 1
                predicted_set_v = self._apply_prediction_limits(state, predicted_set_v, reversal_step_v)

        return predicted_set_v

    def _apply_prediction_limits(self, state: VoltageState, candidate_set_v: int, maximum_step_v: int) -> int:
        minimum_set_v = max(MIN_VOLTAGE_V, state.target_real_voltage_v - self._policy.max_set_offset_v)
        maximum_set_v = min(MAX_VOLTAGE_V, state.target_real_voltage_v + self._policy.max_set_offset_v)
        minimum_step_set_v = state.device_set_voltage_v - maximum_step_v
        maximum_step_set_v = state.device_set_voltage_v + maximum_step_v
        bounded_v = clamp(candidate_set_v, minimum_set_v, maximum_set_v)
        bounded_v = clamp(bounded_v, minimum_step_set_v, maximum_step_set_v)
        return clamp(bounded_v, MIN_VOLTAGE_V, MAX_VOLTAGE_V)

    def _apply_set_adjustment(self, old_set_v: int, new_set_v: int) -> None:
        print(f"ADJUST: SET {old_set_v} V -> {new_set_v} V")
        self._device.set_voltage(new_set_v)
        now = time.monotonic()
        self._last_adjustment_time = now
        self._last_adjustment_direction = direction(new_set_v - old_set_v)
        self._adjustment_times.append(now)

    def _regulation_block_reason(self) -> str | None:
        now = time.monotonic()
        earliest_allowed = self._last_adjustment_time + self._policy.regulation_min_adjustment_interval_s

        if now < earliest_allowed:
            return "blocked by adjust interval"

        one_hour_ago = now - 3600.0
        while self._adjustment_times and self._adjustment_times[0] < one_hour_ago:
            self._adjustment_times.popleft()

        if len(self._adjustment_times) >= self._policy.max_adjustments_per_hour:
            return "blocked by hourly limit"

        return None

    def _classify_regulation_error(self, error_v: int) -> str:
        absolute_error_v = abs(error_v)
        if absolute_error_v <= self._policy.tolerance_v:
            return "stable"
        if absolute_error_v <= self._policy.tolerance_v + self._policy.regulation_hysteresis_v:
            return "hysteresis"
        return "bad"

    def _can_accept_tuning_hold(self, state: VoltageState) -> bool:
        return abs(state.error_v) <= self._policy.tolerance_v + self._policy.reversal_guard_v

    def _print_model_prediction(self, state: VoltageState, predicted_set_v: int) -> None:
        print(
            f"MODEL: gain={self._last_model_gain:.3f}, "
            f"offset={self._last_model_offset:.1f} V, "
            f"samples={self._last_model_samples}"
        )
        print(
            f"PREDICT: target={state.target_real_voltage_v} V, "
            f"current SET={state.device_set_voltage_v} V, "
            f"predicted SET={predicted_set_v} V"
        )

    def _format_observations(self) -> str:
        return ", ".join(
            f"SET={set_voltage_v}/REAL={real_voltage_v}"
            for set_voltage_v, real_voltage_v in self._observations
        )

    @staticmethod
    def _validate_target(target_voltage_v: int) -> None:
        if not 1 <= target_voltage_v <= MAX_VOLTAGE_V:
            raise ValueError(f"Target voltage must be between 1 and {MAX_VOLTAGE_V} V.")

    @staticmethod
    def print_state(stage: str, state: VoltageState, status: str | None = None) -> None:
        line = (
            f"{stage}: target={state.target_real_voltage_v} V, "
            f"SET={state.device_set_voltage_v} V, "
            f"REAL={state.real_voltage_v} V, "
            f"error={state.error_v:+d} V"
        )
        if status is not None:
            line += f", status={status}"
        print(line)


def create_parser() -> argparse.ArgumentParser:
    """Create the command-line parser."""
    parser = argparse.ArgumentParser(
        description="Relay-aware DIAMETRAL AC250K1D demonstration controller."
    )
    parser.add_argument("--port", default=DEFAULT_PORT)
    parser.add_argument("--address", type=int, default=DEFAULT_ADDRESS)
    parser.add_argument("--timeout", type=float, default=0.8)
    parser.add_argument("--verbose", action="store_true")
    parser.add_argument("--tolerance", type=int, default=2)
    parser.add_argument("--tuning-max-step", type=int, default=25)
    parser.add_argument("--max-step", type=int, default=5)
    parser.add_argument("--max-set-offset", type=int, default=30)
    parser.add_argument("--settle-time", type=float, default=2.0)
    parser.add_argument("--sample-interval", type=float, default=5.0)
    parser.add_argument("--min-adjust-interval", type=float, default=60.0)
    parser.add_argument("--max-adjustments-per-hour", type=int, default=6)

    commands = parser.add_subparsers(dest="command", required=True)

    set_parser = commands.add_parser("set", help="Disable output, set voltage, and read REAL voltage.")
    set_parser.add_argument("target_voltage", type=int)

    tune_parser = commands.add_parser("tune", help="Tune with output off and keep the output disabled.")
    tune_parser.add_argument("target_voltage", type=int)

    auto_on_parser = commands.add_parser(
        "auto-on",
        help="Tune with output off and enable after REAL is stable.",
    )
    auto_on_parser.add_argument("target_voltage", type=int)

    regulate_parser = commands.add_parser(
        "regulate",
        help="Tune, enable, and maintain REAL voltage with relay-wear limits.",
    )
    regulate_parser.add_argument("target_voltage", type=int)

    commands.add_parser("status", help="Read REAL voltage and output state.")
    commands.add_parser("off", help="Disable the output.")

    return parser


def create_policy(arguments: argparse.Namespace) -> RelayPolicy:
    """Create a validated relay policy from CLI arguments."""
    if arguments.tolerance < 0:
        raise ValueError("Tolerance must not be negative.")
    if arguments.tuning_max_step < 1:
        raise ValueError("Tuning maximum step must be at least 1 V.")
    if arguments.max_step < 1:
        raise ValueError("Maximum step must be at least 1 V.")
    if arguments.max_set_offset < 0:
        raise ValueError("Maximum SET offset must not be negative.")
    if arguments.settle_time < 0:
        raise ValueError("Settle time must not be negative.")
    if arguments.sample_interval <= 0:
        raise ValueError("Sample interval must be positive.")
    if arguments.min_adjust_interval < 0:
        raise ValueError("Minimum adjustment interval must not be negative.")
    if arguments.max_adjustments_per_hour < 1:
        raise ValueError("Maximum adjustments per hour must be positive.")

    return RelayPolicy(
        tolerance_v=arguments.tolerance,
        tuning_max_step_v=arguments.tuning_max_step,
        regulation_max_step_v=arguments.max_step,
        max_set_offset_v=arguments.max_set_offset,
        settle_after_set_s=arguments.settle_time,
        regulation_sample_interval_s=arguments.sample_interval,
        regulation_min_adjustment_interval_s=arguments.min_adjust_interval,
        max_adjustments_per_hour=arguments.max_adjustments_per_hour,
    )


def run_command(arguments: argparse.Namespace) -> int:
    """Run the selected command."""
    config = SerialConfig(
        port=arguments.port,
        address=arguments.address,
        timeout_s=arguments.timeout,
    )
    policy = create_policy(arguments)

    with DiametralAc250K1D(config, verbose=arguments.verbose) as device:
        controller = RelayAwareVoltageController(device, policy)

        if arguments.command == "off":
            device.output_off()
            print("OUTPUT: OFF")
            return 0

        if arguments.command == "status":
            real_voltage_v = device.read_real_voltage()
            output_enabled = device.read_output_enabled()
            print("SET: unknown; the documented protocol has no separate SET readback.")
            print(f"REAL: {real_voltage_v} V")
            print(f"OUTPUT: {'ON' if output_enabled else 'OFF'}")
            return 0

        if arguments.command == "set":
            state = controller.set_and_measure(arguments.target_voltage)
            controller.print_state("SET", state)
            print("OUTPUT: OFF")
            return 0

        if arguments.command == "tune":
            state = controller.tune_with_output_off(arguments.target_voltage)
            controller.print_state("TUNE DONE", state)
            print("OUTPUT: OFF")
            return 0

        if arguments.command == "auto-on":
            controller.tune_and_enable(arguments.target_voltage)
            print("OUTPUT: ON")
            return 0

        try:
            controller.regulate(arguments.target_voltage)
        except KeyboardInterrupt:
            print("\nSTOP: Regulation interrupted. Disabling output.")
            device.output_off()
            print("OUTPUT: OFF")
            return 0
        except (ControlError, ProtocolError, SerialException, OSError):
            print("FAILSAFE: Regulation failed. Attempting to disable output.", file=sys.stderr)

            try:
                device.output_off()
                print("OUTPUT: OFF")
            except (ProtocolError, SerialException, OSError) as shutdown_error:
                print(f"FAILSAFE ERROR: {shutdown_error}", file=sys.stderr)

            raise

    return 0


def main() -> int:
    """Run the application."""
    arguments = create_parser().parse_args()

    try:
        return run_command(arguments)
    except (ControlError, ProtocolError, SerialException, OSError, ValueError) as exc:
        print(f"ERROR: {exc}", file=sys.stderr)
        return 1


if __name__ == "__main__":
    raise SystemExit(main())
