"""Local rover command state and watchdog safety logic for Lab038.

The educational control payload is carried inside the unchanged Lab033 common
packet.  It is intentionally small and is not a production motor-controller
protocol.
"""

from __future__ import annotations

from dataclasses import dataclass
import struct

from protocol.link_packet import LinkPacket, TrafficClass


CONTROL_MAGIC = b"CTL1"
CONTROL_PAYLOAD_FORMAT = "!4sffBB"
CONTROL_PAYLOAD_SIZE = struct.calcsize(CONTROL_PAYLOAD_FORMAT)


class ControlPayloadError(ValueError):
    """The educational Lab038 control payload is malformed."""


@dataclass(frozen=True)
class ControlState:
    desired_speed_mps: float
    desired_turn: float
    braking: bool
    movement_allowed: bool


SAFE_STATE = ControlState(0.0, 0.0, True, False)


def encode_control_state(state: ControlState) -> bytes:
    """Serialize the Lab038 teaching payload without changing LinkPacket."""

    if not isinstance(state, ControlState):
        raise TypeError("state must be ControlState")
    return struct.pack(
        CONTROL_PAYLOAD_FORMAT,
        CONTROL_MAGIC,
        float(state.desired_speed_mps),
        float(state.desired_turn),
        int(bool(state.braking)),
        int(bool(state.movement_allowed)),
    )


def decode_control_state(payload: bytes) -> ControlState:
    """Decode and validate the Lab038 teaching payload."""

    if not isinstance(payload, (bytes, bytearray)):
        raise TypeError("payload must be bytes or bytearray")
    if len(payload) != CONTROL_PAYLOAD_SIZE:
        raise ControlPayloadError("invalid control payload length")
    magic, speed, turn, braking, allowed = struct.unpack(CONTROL_PAYLOAD_FORMAT, bytes(payload))
    if magic != CONTROL_MAGIC:
        raise ControlPayloadError("invalid control payload magic")
    if braking not in (0, 1) or allowed not in (0, 1):
        raise ControlPayloadError("control booleans must be zero or one")
    return ControlState(speed, turn, bool(braking), bool(allowed))


class RoverControlFailsafe:
    """Strictly-monotonic command receiver with temporary and latched stops."""

    def __init__(self, watchdog_timeout_us: int, boot_time_us: int = 0) -> None:
        if watchdog_timeout_us <= 0:
            raise ValueError("watchdog_timeout_us must be positive")
        self.watchdog_timeout_us = watchdog_timeout_us
        self.last_sequence = -1
        self.last_new_command_time_us = boot_time_us
        self.requested_state = SAFE_STATE
        self.temporary_safe_stop = False
        self.emergency_stop_latched = False
        self.stale_commands = 0
        self.new_commands = 0
        self.watchdog_triggers = 0
        self.emergency_actions = 0
        self.emergency_duplicates = 0
        self._emergency_keys: set[tuple[int, int]] = set()

    @property
    def effective_state(self) -> ControlState:
        if self.emergency_stop_latched or self.temporary_safe_stop:
            return SAFE_STATE
        return self.requested_state

    def receive_state(self, packet: LinkPacket, receive_time_us: int) -> bool:
        """Apply a strictly newer normal command and refresh the watchdog."""

        if packet.traffic_class is not TrafficClass.CONTROL:
            raise ValueError("normal state must use CONTROL traffic class")
        state = decode_control_state(packet.payload)
        if packet.sequence_number <= self.last_sequence:
            self.stale_commands += 1
            return False
        self.last_sequence = packet.sequence_number
        self.last_new_command_time_us = receive_time_us
        self.requested_state = state
        self.temporary_safe_stop = False
        self.new_commands += 1
        return True

    def check_watchdog(self, now_us: int) -> bool:
        """Enter a temporary safe stop when fresh control has timed out."""

        if now_us < self.last_new_command_time_us:
            raise ValueError("watchdog time moved backwards")
        expired = now_us - self.last_new_command_time_us >= self.watchdog_timeout_us
        if expired and not self.temporary_safe_stop:
            self.temporary_safe_stop = True
            self.watchdog_triggers += 1
            return True
        return False

    def receive_emergency(self, packet: LinkPacket) -> bool:
        """Latch emergency stop once; return false for subsequent copies."""

        if packet.traffic_class is not TrafficClass.EMERGENCY:
            raise ValueError("emergency command must use EMERGENCY traffic class")
        key = packet.stream_id, packet.sequence_number
        if key in self._emergency_keys:
            self.emergency_duplicates += 1
            return False
        self._emergency_keys.add(key)
        self.emergency_stop_latched = True
        self.temporary_safe_stop = True
        self.emergency_actions += 1
        return True