#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# MIT License
# Copyright (c) 2018 zleffke
# Ported to GNU Radio 3.10+ by gr-mcp

"""
SARP (Search And Rescue Processor) Message Extractor.

Each 72-byte PDS frame contains three 24-byte SARP messages.
Each SARP message starts with a 12-bit sync word: 0xD60

This block:
1. Receives PDS frame PDUs
2. Splits into three 24-byte SARP messages
3. Validates sync word (0xD60)
4. Routes to 'valid' or 'invalid' output port
"""

import numpy as np
import pmt
import binascii
from gnuradio import gr


class sarp_msg_extract(gr.basic_block):
    """
    SARP Message Extractor.

    Input: PDU from PDS Frame Sync Block (72 bytes)
    Output: Individual SARP messages (24 bytes each) on 'valid' or 'invalid' ports

    A SARP message is considered valid if its sync word matches 0xD60.
    Even invalid messages may contain recoverable beacon data via BCH codes.
    """

    def __init__(self):
        gr.basic_block.__init__(
            self,
            name="sarp_msg_extract",
            in_sig=None,
            out_sig=None,
        )

        self.message_port_register_in(pmt.intern("in"))
        self.set_msg_handler(pmt.intern("in"), self.handle_msg)
        self.message_port_register_out(pmt.intern("valid"))
        self.message_port_register_out(pmt.intern("invalid"))

    def _check_sync_word(self, msg: bytearray) -> bool:
        """Check if SARP message has valid sync word (0xD60)."""
        # Sync word is first 12 bits: 0xD6 followed by upper nibble 0x0
        return (msg[0] == 0xD6) and ((msg[1] & 0xF0) == 0x00)

    def _publish_msg(self, msg: bytearray, valid: bool):
        """Publish SARP message to appropriate output port."""
        port = "valid" if valid else "invalid"
        pdu = pmt.cons(pmt.PMT_NIL, pmt.init_u8vector(len(msg), msg))
        self.message_port_pub(pmt.intern(port), pdu)

    def handle_msg(self, msg_pmt):
        """Process incoming PDS frame PDU."""
        pds_msg = pmt.cdr(msg_pmt)

        if not pmt.is_u8vector(pds_msg):
            print("[ERROR] Received invalid message type. Expected u8vector")
            return

        buff = bytearray(pmt.u8vector_elements(pds_msg))

        if len(buff) < 72:
            print(f"[ERROR] PDS frame too short: {len(buff)} bytes (expected 72)")
            return

        # Extract three 24-byte SARP messages
        sarp_msg_1 = bytearray(buff[0:24])
        sarp_msg_2 = bytearray(buff[24:48])
        sarp_msg_3 = bytearray(buff[48:72])

        # Validate and publish each message
        self._publish_msg(sarp_msg_1, self._check_sync_word(sarp_msg_1))
        self._publish_msg(sarp_msg_2, self._check_sync_word(sarp_msg_2))
        self._publish_msg(sarp_msg_3, self._check_sync_word(sarp_msg_3))