#!/usr/bin/env python3
"""
ICOM ID-5100 CI-V link probe (pure stdlib, no pyserial).

Opens the RT Systems USB-29A cable (FTDI, plugged into the radio's [SP2]/CI-V
jack), sends a "read operating frequency" CI-V frame, and decodes the reply.
A valid reply proves the PC <-> radio control link end-to-end.

Defaults: /dev/ttyUSB0, 9600 baud, radio CI-V address 0x8C, controller 0xE0.

Usage:
    python3 civ_probe.py [PORT] [BAUD]
    python3 civ_probe.py /dev/ttyUSB0 9600
"""
import os
import sys
import time
import termios

RADIO_ADDR = 0x8C   # ID-5100 default CI-V address
CTRL_ADDR  = 0xE0   # this controller's address
PREAMBLE   = b"\xFE\xFE"
EOM        = 0xFD

BAUD_CONST = {
    1200: termios.B1200, 4800: termios.B4800, 9600: termios.B9600,
    19200: termios.B19200, 38400: termios.B38400, 115200: termios.B115200,
}


def open_port(port, baud):
    fd = os.open(port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK)
    speed = BAUD_CONST.get(baud)
    if speed is None:
        raise ValueError(f"unsupported baud {baud}; choose one of {sorted(BAUD_CONST)}")
    iflag, oflag, cflag, lflag, ispeed, ospeed, cc = termios.tcgetattr(fd)
    # raw 8N1, receiver on, ignore modem control lines
    iflag = 0
    oflag = 0
    lflag = 0
    cflag = termios.CS8 | termios.CREAD | termios.CLOCAL
    cc = list(cc)
    cc[termios.VMIN] = 0
    cc[termios.VTIME] = 0
    termios.tcsetattr(fd, termios.TCSANOW, [iflag, oflag, cflag, lflag, speed, speed, cc])
    termios.tcflush(fd, termios.TCIOFLUSH)
    return fd


def read_for(fd, seconds):
    """Collect raw bytes for `seconds`, return bytearray."""
    buf = bytearray()
    deadline = time.monotonic() + seconds
    while time.monotonic() < deadline:
        try:
            chunk = os.read(fd, 256)
        except BlockingIOError:
            chunk = b""
        if chunk:
            buf.extend(chunk)
        else:
            time.sleep(0.01)
    return buf


def split_frames(buf):
    """Yield CI-V frames (FE FE ... FD), skipping leading echo/noise."""
    i = 0
    n = len(buf)
    while i < n - 1:
        if buf[i] == 0xFE and buf[i + 1] == 0xFE:
            j = i + 2
            while j < n and buf[j] != EOM:
                j += 1
            if j < n:
                yield bytes(buf[i:j + 1])
                i = j + 1
                continue
        i += 1


def decode_freq(data):
    """5-byte little-endian BCD -> Hz."""
    hz = 0
    for i, byte in enumerate(data[:5]):
        ones = byte & 0x0F
        tens = byte >> 4
        hz += ones * (10 ** (2 * i)) + tens * (10 ** (2 * i + 1))
    return hz


def main():
    port = sys.argv[1] if len(sys.argv) > 1 else "/dev/ttyUSB0"
    baud = int(sys.argv[2]) if len(sys.argv) > 2 else 9600

    print(f"Port {port} @ {baud} baud | radio 0x{RADIO_ADDR:02X} ctrl 0x{CTRL_ADDR:02X}")
    fd = open_port(port, baud)
    try:
        # read operating frequency: FE FE <radio> <ctrl> 03 FD
        cmd = PREAMBLE + bytes([RADIO_ADDR, CTRL_ADDR, 0x03, EOM])
        print("TX:", cmd.hex(" "))
        os.write(fd, cmd)
        buf = read_for(fd, 1.0)
    finally:
        os.close(fd)

    if not buf:
        print("RX: (nothing)")
        print("\nNo bytes back. Check: SP2 jack seated, radio CI-V baud = "
              f"{baud}, radio powered on. Single-device port was confirmed as this one.")
        sys.exit(2)

    print("RX raw:", buf.hex(" "))
    got_freq = False
    for frame in split_frames(buf):
        # frame: FE FE <to> <from> <cn> [data...] FD
        if len(frame) < 6:
            continue
        to, frm, cn = frame[2], frame[3], frame[4]
        body = frame[5:-1]
        tag = "echo" if (to == RADIO_ADDR and frm == CTRL_ADDR) else "radio->us"
        print(f"  frame [{tag}] to=0x{to:02X} from=0x{frm:02X} cn=0x{cn:02X} body={body.hex(' ')}")
        if frm == RADIO_ADDR and cn == 0x03 and len(body) >= 5:
            hz = decode_freq(body)
            print(f"\n*** LINK OK -- VFO frequency: {hz/1e6:.6f} MHz ***")
            got_freq = True

    if not got_freq:
        print("\nGot bytes but no frequency frame. Link is alive but reply was "
              "unexpected -- check baud match and CI-V address.")
        sys.exit(1)


if __name__ == "__main__":
    main()