#!/usr/bin/env python3
"""Reference implementation of outgoing SMS packet assembly, mirrored from the spec.

Written on the PC first so the tricky parts (CRC-32 with its byte-pair swap, the IP and
UDP checksums, the block accounting) can be checked against a known-good packet before
any of it goes into firmware. The firmware version will be a literal translation of this.
"""


def crc32_dmr(data: bytes) -> bytes:
    """ETSI/MOTOTRBO CRC-32: swap bytes within each 16-bit pair, then MSB-first
    CRC32 (poly 0x04C11DB7, init 0, no final xor). Returned little endian."""
    swapped = bytearray(data)
    for i in range(0, len(swapped) - 1, 2):
        swapped[i], swapped[i + 1] = swapped[i + 1], swapped[i]

    crc = 0
    for b in swapped:
        crc ^= b << 24
        for _ in range(8):
            crc = ((crc << 1) ^ 0x04C11DB7) & 0xFFFFFFFF if (crc & 0x80000000) else ((crc << 1) & 0xFFFFFFFF)
    return crc.to_bytes(4, "little")


def ones_complement(data: bytes) -> int:
    if len(data) % 2:
        data += b"\x00"
    total = 0
    for i in range(0, len(data), 2):
        total += (data[i] << 8) | data[i + 1]
        total = (total & 0xFFFF) + (total >> 16)
    return (~total) & 0xFFFF


def crc16_ccitt(data: bytes) -> int:
    crc = 0x0000
    for b in data:
        crc ^= b << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if (crc & 0x8000) else ((crc << 1) & 0xFFFF)
    return crc


def build_tms(text: str, flag: int = 0xA0, seq: int = 0x81) -> bytes:
    """[len 2B BE][flag][00][seq][04 0D 00 0A 00][text UTF-16LE]"""
    body = bytes([flag, 0x00, seq]) + bytes([0x04, 0x0D, 0x00, 0x0A, 0x00]) + text.encode("utf-16-le")
    tms_len = 8 + 2 * len(text)
    assert tms_len == len(body), "length mismatch: %d vs %d" % (tms_len, len(body))
    return tms_len.to_bytes(2, "big") + body


def build_packet(src_id: int, dst_id: int, is_group: bool, text: str,
                 flag: int = 0xA0, seq: int = 0x81, ip_id: int = 0x0001) -> bytes:
    tms = build_tms(text, flag, seq)

    udp_len = 8 + len(tms)
    src_ip = (0x0C << 24) | src_id
    dst_ip = ((0xE1 if is_group else 0x0C) << 24) | dst_id
    ttl = 0x01 if is_group else 0x40

    udp = bytearray()
    udp += (0x0FA7).to_bytes(2, "big")     # source port 4007
    udp += (0x0FA7).to_bytes(2, "big")     # destination port 4007
    udp += udp_len.to_bytes(2, "big")
    udp += b"\x00\x00"                     # checksum placeholder
    udp += tms

    pseudo = src_ip.to_bytes(4, "big") + dst_ip.to_bytes(4, "big") + b"\x00\x11" + udp_len.to_bytes(2, "big")
    udp_csum = ones_complement(pseudo + bytes(udp))
    udp[6:8] = udp_csum.to_bytes(2, "big")

    ip_len = 20 + udp_len
    ip = bytearray()
    ip += b"\x45\x00"
    ip += ip_len.to_bytes(2, "big")
    ip += ip_id.to_bytes(2, "big")
    ip += b"\x00\x00"
    ip += bytes([ttl, 0x11])
    ip += b"\x00\x00"                      # header checksum placeholder
    ip += src_ip.to_bytes(4, "big")
    ip += dst_ip.to_bytes(4, "big")
    ip[10:12] = ones_complement(bytes(ip)).to_bytes(2, "big")

    packet = bytes(ip) + bytes(udp)

    # pad to a multiple of 12, then append the CRC-32 as its own tail
    pad = (-len(packet)) % 12
    packet += b"\x00" * pad
    packet += crc32_dmr(packet)
    return packet, pad


def build_data_header(src_id: int, dst_id: int, is_group: bool, blocks: int, pad: int) -> bytes:
    h = bytearray(12)
    h[0] = (0x80 if is_group else 0x00) | 0x02          # DPF=2 unconfirmed
    h[1] = 0x40 | (pad & 0x0F)                          # SAP=4 (IP), pad octets
    h[2:5] = dst_id.to_bytes(3, "big")
    h[5:8] = src_id.to_bytes(3, "big")
    h[8] = 0x80 | (blocks & 0x7F)
    h[9] = 0x10
    crc = crc16_ccitt(bytes(h[0:10])) ^ 0xCCCC
    h[10] = (crc >> 8) & 0xFF
    h[11] = crc & 0xFF
    return bytes(h)


if __name__ == "__main__":
    text = "PRIVET"
    packet, pad = build_packet(777777, 6, True, text)
    blocks = len(packet) // 12
    hdr = build_data_header(777777, 6, True, blocks, pad)

    print("text        : %r" % text)
    print("packet len  : %d bytes, pad %d, blocks %d" % (len(packet), pad, blocks))
    print("packet hex  : %s" % packet.hex(" "))
    print("last 4 bytes: %s   (reference ends with 92 ba 1c 62)" % packet[-4:].hex(" "))
    print("data header : %s" % hdr.hex(" "))