SDR-Rover/protocol/safe_reset.py

"""Команда безопасного аварийного сброса и её подтверждение для Lab040.

Оба прикладных блока данных передаются внутри неизменного пакета LinkPacket
из Lab033.
"""

from __future__ import annotations

from dataclasses import dataclass
from enum import IntEnum
import struct

from protocol.link_packet import Direction, LinkPacket, TrafficClass
from protocol.two_stage_failsafe import SafetyState, TwoStageFailsafe


RESET_MAGIC = b"RST1"
RESET_ACK_MAGIC = b"RAK1"
RESET_FORMAT = "!4sIIfB"
RESET_ACK_FORMAT = "!4sIIBB"
RESET_SIZE = struct.calcsize(RESET_FORMAT)
RESET_ACK_SIZE = struct.calcsize(RESET_ACK_FORMAT)
STREAM_RESET = 6
STREAM_RESET_ACK = 7


class ResetReason(IntEnum):
    ACCEPTED = 0
    NOT_LATCHED = 1
    MOVING = 2
    STALE_LINK = 3
    EVENT_MISMATCH = 4
    OLD_SEQUENCE = 5
    NONZERO_REQUEST = 6
    MOVEMENT_PERMITTED = 7


@dataclass(frozen=True)
class ResetCommand:
    emergency_event_id: int
    reset_request_id: int
    requested_speed_mps: float
    movement_permitted: bool


@dataclass(frozen=True)
class ResetAcknowledgement:
    emergency_event_id: int
    reset_request_id: int
    accepted: bool
    reason: ResetReason


@dataclass(frozen=True)
class ResetDecision:
    acknowledgement: ResetAcknowledgement
    performed: bool
    duplicate: bool


def encode_reset(command: ResetCommand) -> bytes:
    if command.emergency_event_id < 0 or command.reset_request_id < 0:
        raise ValueError("идентификаторы сброса не могут быть отрицательными")
    return struct.pack(
        RESET_FORMAT,
        RESET_MAGIC,
        command.emergency_event_id,
        command.reset_request_id,
        float(command.requested_speed_mps),
        int(bool(command.movement_permitted)),
    )


def decode_reset(payload: bytes) -> ResetCommand:
    if len(payload) != RESET_SIZE:
        raise ValueError("неверная длина данных запроса сброса")
    magic, event_id, request_id, speed, permitted = struct.unpack(RESET_FORMAT, payload)
    if magic != RESET_MAGIC or permitted not in (0, 1):
        raise ValueError("неверные данные запроса сброса")
    return ResetCommand(event_id, request_id, speed, bool(permitted))


def encode_reset_ack(ack: ResetAcknowledgement) -> bytes:
    return struct.pack(
        RESET_ACK_FORMAT,
        RESET_ACK_MAGIC,
        ack.emergency_event_id,
        ack.reset_request_id,
        int(ack.accepted),
        int(ack.reason),
    )


def decode_reset_ack(payload: bytes) -> ResetAcknowledgement:
    if len(payload) != RESET_ACK_SIZE:
        raise ValueError("неверная длина подтверждения сброса")
    magic, event_id, request_id, accepted, reason = struct.unpack(RESET_ACK_FORMAT, payload)
    if magic != RESET_ACK_MAGIC or accepted not in (0, 1):
        raise ValueError("неверное подтверждение сброса")
    return ResetAcknowledgement(event_id, request_id, bool(accepted), ResetReason(reason))


def build_reset_packet(
    command: ResetCommand,
    sequence_number: int,
    generation_time_us: int,
) -> LinkPacket:
    return LinkPacket(
        traffic_class=TrafficClass.EMERGENCY,
        direction=Direction.GROUND_TO_ROVER,
        stream_id=STREAM_RESET,
        sequence_number=sequence_number,
        generation_time_us=generation_time_us,
        deadline_ms=50,
        payload=encode_reset(command),
    )


def build_reset_ack(
    decision: ResetDecision,
    sequence_number: int,
    generation_time_us: int,
) -> LinkPacket:
    return LinkPacket(
        traffic_class=TrafficClass.EMERGENCY,
        direction=Direction.ROVER_TO_GROUND,
        stream_id=STREAM_RESET_ACK,
        sequence_number=sequence_number,
        generation_time_us=generation_time_us,
        deadline_ms=50,
        payload=encode_reset_ack(decision.acknowledgement),
    )


class SafeResetReceiver:
    """Проверяет условия сброса и повторно подтверждает принятые дубликаты."""

    def __init__(self) -> None:
        self.last_sequence = -1
        self.accepted_keys: set[tuple[int, int, int]] = set()
        self.accepted = 0
        self.rejected = 0
        self.duplicates = 0
        self.reacknowledgements = 0

    def process(
        self,
        packet: LinkPacket,
        *,
        rover: TwoStageFailsafe,
        actual_speed_mps: float,
        last_link_age_seconds: float,
        emergency_event_id: int | None,
    ) -> ResetDecision:
        if packet.traffic_class is not TrafficClass.EMERGENCY:
            raise ValueError("запрос сброса должен использовать аварийный класс трафика")
        if packet.direction is not Direction.GROUND_TO_ROVER or packet.stream_id != STREAM_RESET:
            raise ValueError("неверная оболочка запроса сброса")
        command = decode_reset(packet.payload)
        key = command.emergency_event_id, command.reset_request_id, packet.sequence_number
        if key in self.accepted_keys:
            self.duplicates += 1
            self.reacknowledgements += 1
            ack = ResetAcknowledgement(command.emergency_event_id, command.reset_request_id, True, ResetReason.ACCEPTED)
            return ResetDecision(ack, False, True)

        reason = ResetReason.ACCEPTED
        if rover.state is not SafetyState.EMERGENCY_LATCHED:
            reason = ResetReason.NOT_LATCHED
        elif actual_speed_mps > 1e-9:
            reason = ResetReason.MOVING
        elif last_link_age_seconds > 0.250 + 1e-12:
            reason = ResetReason.STALE_LINK
        elif command.emergency_event_id != emergency_event_id:
            reason = ResetReason.EVENT_MISMATCH
        elif packet.sequence_number < self.last_sequence:
            reason = ResetReason.OLD_SEQUENCE
        elif abs(command.requested_speed_mps) > 1e-9:
            reason = ResetReason.NONZERO_REQUEST
        elif command.movement_permitted:
            reason = ResetReason.MOVEMENT_PERMITTED

        self.last_sequence = max(self.last_sequence, packet.sequence_number)
        accepted = reason is ResetReason.ACCEPTED
        if accepted:
            self.accepted_keys.add(key)
            self.accepted += 1
            rover.state = SafetyState.NORMAL
        else:
            self.rejected += 1
        ack = ResetAcknowledgement(command.emergency_event_id, command.reset_request_id, accepted, reason)
        return ResetDecision(ack, accepted, False)