Azimuth

#!/usr/bin/env python3
"""Hardware-agnostic SDR receiver: live terminal spectrum + full-range survey."""
 
import argparse
import shutil
import sys
import time
 
import numpy as np
 
try:
    import SoapySDR
    from SoapySDR import (
        SOAPY_SDR_RX,
        SOAPY_SDR_CF32,
        SOAPY_SDR_OVERFLOW,
        SOAPY_SDR_TIMEOUT,
    )
except ImportError:
    sys.exit(
        "SoapySDR Python bindings not found.\n"
        "These do NOT come from pip. On a Raspberry Pi / Debian / Ubuntu:\n"
        "    sudo apt install python3-soapysdr soapysdr-tools\n"
        "    sudo apt install soapysdr-module-rtlsdr   # or -hackrf / -plutosdr / -lms7\n"
        "See SETUP.md for the full walkthrough."
    )
 
 
_SUFFIX = {"k": 1e3, "K": 1e3, "m": 1e6, "M": 1e6, "g": 1e9, "G": 1e9}
 
 
def parse_freq(text):
    text = text.strip()
    if text and text[-1] in _SUFFIX:
        return float(text[:-1]) * _SUFFIX[text[-1]]
    return float(text)
 
 
def human_hz(value):
    for unit, scale in (("GHz", 1e9), ("MHz", 1e6), ("kHz", 1e3)):
        if abs(value) >= scale:
            return f"{value / scale:.6g} {unit}"
    return f"{value:.6g} Hz"
 
 
def fmt_range(r):
    lo, hi, step = r.minimum(), r.maximum(), r.step()
    out = f"{human_hz(lo)} .. {human_hz(hi)}"
    if step:
        out += f" (step {human_hz(step)})"
    return out
 
 
def check_in_range(value, ranges, label):
    if not ranges:
        return
    if any(r.minimum() <= value <= r.maximum() for r in ranges):
        return
    spans = ", ".join(fmt_range(r) for r in ranges)
    sys.exit(
        f"Requested {label} {human_hz(value)} is out of range for this device.\n"
        f"Supported {label}: {spans}"
    )
 
 
def enumerate_devices(args_filter=""):
    return list(SoapySDR.Device.enumerate(args_filter))
 
 
def print_device_list(devices):
    if not devices:
        print(
            "No SDR devices found.\n"
            "  - Is the device plugged in / powered?\n"
            "  - Is the matching driver module installed? (SoapySDRUtil --info)\n"
            "  - For RTL-SDR, the dvb kernel driver may be claiming it (see SETUP.md)."
        )
        return
    print(f"Found {len(devices)} device(s):\n")
    for i, dev in enumerate(devices):
        label = dev.get("label") or dev.get("driver") or "unknown"
        print(f"  [{i}] {label}")
        for key in sorted(dev.keys()):
            if key != "label":
                print(f"        {key} = {dev[key]}")
        print()
 
 
def probe(sdr, chan):
    print("=" * 60)
    print(f"Driver:    {sdr.getDriverKey()}")
    print(f"Hardware:  {sdr.getHardwareKey()}")
    info = sdr.getHardwareInfo()
    for key in sorted(info.keys()):
        print(f"           {key} = {info[key]}")
 
    n_rx = sdr.getNumChannels(SOAPY_SDR_RX)
    print(f"\nRX channels: {n_rx}   (probing channel {chan})")
 
    try:
        ants = sdr.listAntennas(SOAPY_SDR_RX, chan)
        print(f"  Antennas:   {', '.join(ants) if ants else '(none listed)'}")
    except Exception as exc:
        print(f"  Antennas:   <unavailable: {exc}>")
 
    try:
        rates = sdr.getSampleRateRange(SOAPY_SDR_RX, chan)
        print("  Sample rate ranges:")
        for r in rates:
            print(f"     {fmt_range(r)}")
    except Exception as exc:
        print(f"  Sample rate: <unavailable: {exc}>")
 
    try:
        freqs = sdr.getFrequencyRange(SOAPY_SDR_RX, chan)
        print("  Frequency ranges:")
        for r in freqs:
            print(f"     {fmt_range(r)}")
    except Exception as exc:
        print(f"  Frequency:   <unavailable: {exc}>")
 
    try:
        bws = sdr.getBandwidthRange(SOAPY_SDR_RX, chan)
        if bws:
            print("  Bandwidth ranges:")
            for r in bws:
                print(f"     {fmt_range(r)}")
    except Exception:
        pass
 
    try:
        grange = sdr.getGainRange(SOAPY_SDR_RX, chan)
        print(f"  Overall gain: {grange.minimum():g} .. {grange.maximum():g} dB")
        for name in sdr.listGains(SOAPY_SDR_RX, chan):
            er = sdr.getGainRange(SOAPY_SDR_RX, chan, name)
            print(f"     element '{name}': {er.minimum():g} .. {er.maximum():g} dB")
        print(f"  Hardware AGC: {'yes' if sdr.hasGainMode(SOAPY_SDR_RX, chan) else 'no'}")
    except Exception as exc:
        print(f"  Gain:        <unavailable: {exc}>")
 
    try:
        sensors = sdr.listSensors()
        if sensors:
            print(f"  Sensors:    {', '.join(sensors)}")
    except Exception:
        pass
    print("=" * 60)
 
 
def compute_psd(samples, fft_size, navg, window):
    usable = min(navg, max(1, len(samples) // fft_size))
    acc = np.zeros(fft_size)
    for i in range(usable):
        seg = samples[i * fft_size : (i + 1) * fft_size]
        if len(seg) < fft_size:
            break
        spec = np.fft.fftshift(np.fft.fft(seg * window))
        acc += (spec.real ** 2 + spec.imag ** 2)
    acc /= usable
    acc /= np.sum(window ** 2)
    return 10.0 * np.log10(acc + 1e-12)
 
 
def wideband_power_dbfs(samples):
    rms = np.sqrt(np.mean(samples.real ** 2 + samples.imag ** 2))
    return 20.0 * np.log10(rms + 1e-12)
 
 
_BLOCKS = " ▁▂▃▄▅▆▇█"
 
 
def spectrum_rows(col_db, height, top_db, floor_db):
    span = max(top_db - floor_db, 1e-9)
    eighths = np.clip((col_db - floor_db) / span, 0.0, 1.0) * (height * 8)
    eighths = eighths.astype(int)
    rows = []
    for r in range(height - 1, -1, -1):
        base = r * 8
        line = "".join(_BLOCKS[min(8, max(0, e - base))] for e in eighths)
        rows.append(line)
    return rows
 
 
def axis_line(center, rate, width):
    left = human_hz(center - rate / 2)
    mid = human_hz(center)
    right = human_hz(center + rate / 2)
    chars = [" "] * width
 
    def place(start, text):
        start = max(0, min(start, width - len(text)))
        for i, ch in enumerate(text):
            if 0 <= start + i < width:
                chars[start + i] = ch
 
    place(0, left)
    place((width - len(mid)) // 2, mid)
    place(width - len(right), right)
    return "".join(chars)
 
 
def render_frame(state, psd_db, center, rate, fft_size, height):
    width = max(20, min(shutil.get_terminal_size((80, 24)).columns, 200))
    n_cols = min(width, fft_size)
 
    cols = np.array_split(psd_db, n_cols)
    col_db = np.array([c.max() for c in cols])
 
    top_db = float(col_db.max())
    floor_db = top_db - state["dyn_range"]
 
    peak_bin = int(np.argmax(psd_db))
    peak_hz = center + (peak_bin - fft_size / 2) * (rate / fft_size)
    peak_db = float(psd_db[peak_bin])
 
    lines = []
    lines.append(f"Azimuth SDR RX  —  {state['driver']} / {state['hardware']}")
    lines.append(
        f"center {human_hz(center)}   rate {human_hz(rate)}   "
        f"gain {state['gain_str']}   ant {state['antenna']}"
    )
    lines.append(
        f"power {state['power']:6.1f} dBFS   "
        f"peak {human_hz(peak_hz)} @ {peak_db:6.1f} dB   "
        f"overflow {state['overflows']}   {state['fps']:4.1f} fps"
    )
    lines.append("")
    lines.extend(spectrum_rows(col_db, height, top_db, floor_db))
    lines.append(axis_line(center, rate, n_cols))
    lines.append(f"[scale: {floor_db:.0f} .. {top_db:.0f} dB]   Ctrl-C to stop")
 
    sys.stdout.write("\033[H" + "\n".join(ln + "\033[K" for ln in lines) + "\033[J")
    sys.stdout.flush()
 
 
def fill_buffer(sdr, stream, buff):
    n = 0
    overflows = 0
    size = len(buff)
    while n < size:
        sr = sdr.readStream(stream, [buff[n:]], size - n, timeoutUs=1_000_000)
        if sr.ret > 0:
            n += sr.ret
        elif sr.ret == SOAPY_SDR_OVERFLOW:
            overflows += 1
        elif sr.ret == SOAPY_SDR_TIMEOUT:
            break
        else:
            break
    return n, overflows
 
 
def run_rx(sdr, args):
    chan = args.channel
    center = parse_freq(args.freq)
    rate = parse_freq(args.rate)
 
    try:
        check_in_range(rate, sdr.getSampleRateRange(SOAPY_SDR_RX, chan), "sample rate")
        check_in_range(center, sdr.getFrequencyRange(SOAPY_SDR_RX, chan), "frequency")
    except SystemExit:
        raise
    except Exception:
        pass
 
    sdr.setSampleRate(SOAPY_SDR_RX, chan, rate)
    sdr.setFrequency(SOAPY_SDR_RX, chan, center)
 
    if args.bandwidth:
        try:
            sdr.setBandwidth(SOAPY_SDR_RX, chan, parse_freq(args.bandwidth))
        except Exception as exc:
            print(f"warning: could not set bandwidth: {exc}", file=sys.stderr)
 
    if args.antenna:
        sdr.setAntenna(SOAPY_SDR_RX, chan, args.antenna)
 
    if args.ppm:
        try:
            sdr.setFrequencyCorrection(SOAPY_SDR_RX, chan, args.ppm)
        except Exception as exc:
            print(f"warning: could not set ppm correction: {exc}", file=sys.stderr)
 
    if args.agc and sdr.hasGainMode(SOAPY_SDR_RX, chan):
        sdr.setGainMode(SOAPY_SDR_RX, chan, True)
        gain_str = "AGC"
    else:
        sdr.setGain(SOAPY_SDR_RX, chan, args.gain)
        gain_str = f"{args.gain:g} dB"
 
    antenna = ""
    try:
        antenna = sdr.getAntenna(SOAPY_SDR_RX, chan)
    except Exception:
        pass
 
    center = sdr.getFrequency(SOAPY_SDR_RX, chan)
    rate = sdr.getSampleRate(SOAPY_SDR_RX, chan)
 
    fft_size = args.fft
    navg = args.avg
    capture = fft_size * navg
    window = np.hanning(fft_size).astype(np.float32)
    buff = np.empty(capture, np.complex64)
 
    state = {
        "driver": sdr.getDriverKey(),
        "hardware": sdr.getHardwareKey(),
        "gain_str": gain_str,
        "antenna": antenna or "-",
        "dyn_range": args.dyn_range,
        "power": -120.0,
        "overflows": 0,
        "fps": 0.0,
    }
 
    stream = sdr.setupStream(SOAPY_SDR_RX, SOAPY_SDR_CF32, [chan])
    sdr.activateStream(stream)
 
    refresh = 1.0 / args.fps
    last_render = 0.0
    frame_t = time.time()
    total_overflows = 0
    end_time = time.time() + args.duration if args.duration else None
 
    sys.stdout.write("\033[2J\033[?25l")
    try:
        while True:
            n, ovf = fill_buffer(sdr, stream, buff)
            total_overflows += ovf
            if n < fft_size:
                sys.stdout.write("\033[H\033[Kwaiting for samples...\033[J")
                sys.stdout.flush()
                time.sleep(0.1)
                continue
 
            now = time.time()
            if now - last_render >= refresh:
                samples = buff[:n]
                psd = compute_psd(samples, fft_size, navg, window)
                state["power"] = wideband_power_dbfs(samples)
                state["overflows"] = total_overflows
                state["fps"] = 1.0 / max(now - frame_t, 1e-6)
                frame_t = now
                render_frame(state, psd, center, rate, fft_size, args.height)
                last_render = now
 
            if end_time and now >= end_time:
                break
    except KeyboardInterrupt:
        pass
    finally:
        sys.stdout.write("\033[?25h\n")
        sys.stdout.flush()
        sdr.deactivateStream(stream)
        sdr.closeStream(stream)
 
 
def notch_dc(psd_db, half_bins):
    c = len(psd_db) // 2
    lo, hi = max(0, c - half_bins), min(len(psd_db), c + half_bins + 1)
    edge = min(psd_db[lo - 1] if lo > 0 else psd_db[hi],
               psd_db[hi] if hi < len(psd_db) else psd_db[lo - 1])
    psd_db[lo:hi] = edge
    return psd_db
 
 
def rebin(freqs, vals, edges, reducer):
    n = len(edges) - 1
    idx = np.clip(np.searchsorted(edges, freqs, side="right") - 1, 0, n - 1)
    if reducer == "max":
        out = np.full(n, -np.inf)
        np.maximum.at(out, idx, vals)
        out[np.isneginf(out)] = np.nan
        return out
    sums = np.zeros(n)
    cnts = np.zeros(n)
    np.add.at(sums, idx, vals)
    np.add.at(cnts, idx, 1)
    out = np.full(n, np.nan)
    nz = cnts > 0
    out[nz] = sums[nz] / cnts[nz]
    return out
 
 
def save_outputs(basename, fcenters, avg_db, max_db, meta):
    csv_path = basename + ".csv"
    with open(csv_path, "w") as fh:
        fh.write("freq_hz,avg_dbfs,max_dbfs\n")
        for f, a, m in zip(fcenters, avg_db, max_db):
            fh.write(f"{f:.0f},{a:.2f},{m:.2f}\n")
    print(f"\nWrote {csv_path}  ({len(fcenters)} bins)")
 
    try:
        import matplotlib
        matplotlib.use("Agg")
        import matplotlib.pyplot as plt
    except ImportError:
        print("matplotlib not installed - skipping PNG (sudo apt install -y python3-matplotlib)")
        return
 
    fx = fcenters / 1e6
    fig, ax = plt.subplots(figsize=(15, 5))
    ax.fill_between(fx, np.nanmin(avg_db), max_db, color="tab:red", alpha=0.25, label="max-hold")
    ax.plot(fx, max_db, color="tab:red", lw=0.5)
    ax.plot(fx, avg_db, color="tab:blue", lw=0.6, label="average")
    ax.set_xlabel("Frequency (MHz)")
    ax.set_ylabel("Power (dBFS)")
    ax.set_title(meta)
    ax.grid(True, alpha=0.3)
    ax.legend(loc="upper right")
    png_path = basename + ".png"
    fig.tight_layout()
    fig.savefig(png_path, dpi=120)
    plt.close(fig)
    print(f"Wrote {png_path}")
 
 
def print_pollution_summary(fcenters, max_db, top=10):
    floor = float(np.nanmedian(max_db))
    print(f"\nNoise floor (median max-hold): {floor:.1f} dBFS")
    print(f"Top {top} most active frequencies:")
    order = np.argsort(np.nan_to_num(max_db, nan=-1e9))[::-1]
    shown = []
    for i in order:
        f = fcenters[i]
        if any(abs(f - g) < 1e6 for g in shown):
            continue
        print(f"  {human_hz(f):>12}   {max_db[i]:6.1f} dBFS   (+{max_db[i] - floor:4.1f} dB)")
        shown.append(f)
        if len(shown) >= top:
            break
 
 
def run_sweep(sdr, args):
    chan = args.channel
    rate = parse_freq(args.rate)
    fft_size = args.fft
    navg = args.avg
 
    franges = sdr.getFrequencyRange(SOAPY_SDR_RX, chan)
    dev_lo = min(r.minimum() for r in franges)
    dev_hi = max(r.maximum() for r in franges)
    start = parse_freq(args.start) if args.start else dev_lo
    stop = parse_freq(args.stop) if args.stop else dev_hi
    start, stop = max(start, dev_lo), min(stop, dev_hi)
    if stop <= start:
        sys.exit(f"sweep stop ({human_hz(stop)}) must be above start ({human_hz(start)})")
 
    try:
        check_in_range(rate, sdr.getSampleRateRange(SOAPY_SDR_RX, chan), "sample rate")
    except SystemExit:
        raise
    except Exception:
        pass
    sdr.setSampleRate(SOAPY_SDR_RX, chan, rate)
    rate = sdr.getSampleRate(SOAPY_SDR_RX, chan)
    if args.bandwidth:
        try:
            sdr.setBandwidth(SOAPY_SDR_RX, chan, parse_freq(args.bandwidth))
        except Exception:
            pass
    if args.agc and sdr.hasGainMode(SOAPY_SDR_RX, chan):
        sdr.setGainMode(SOAPY_SDR_RX, chan, True)
    else:
        sdr.setGain(SOAPY_SDR_RX, chan, args.gain)
 
    usable_bw = rate * args.usable
    df = rate / fft_size
    freqs_rel = (np.arange(fft_size) - fft_size // 2) * df
    lo_idx = int(fft_size * (1.0 - args.usable) / 2.0)
    keep = slice(lo_idx, fft_size - lo_idx)
    dc_half = max(1, fft_size // 200)
 
    centers = []
    fc = start + usable_bw / 2.0
    while fc - usable_bw / 2.0 < stop:
        centers.append(min(max(fc, dev_lo + rate / 2.0), dev_hi - rate / 2.0))
        fc += usable_bw
    n_steps = len(centers)
 
    freqs = np.concatenate([c + freqs_rel[keep] for c in centers])
 
    window = np.hanning(fft_size).astype(np.float32)
    buff = np.empty(fft_size * navg, np.complex64)
 
    print(f"Sweep: {human_hz(start)} .. {human_hz(stop)}   "
          f"step {human_hz(usable_bw)} x {n_steps} steps x {args.sweeps} sweep(s)")
    est = n_steps * args.sweeps * (args.settle + 0.01)
    print(f"Estimated time: ~{est:.0f} s\n")
 
    stream = sdr.setupStream(SOAPY_SDR_RX, SOAPY_SDR_CF32, [chan])
    sdr.activateStream(stream)
 
    maxhold = np.full(len(freqs), -np.inf)
    avgacc = np.zeros(len(freqs))
    t0 = time.time()
    try:
        for s in range(args.sweeps):
            powers = np.empty(len(freqs))
            offset = 0
            seg_len = keep.stop - keep.start
            for i, c in enumerate(centers):
                sdr.setFrequency(SOAPY_SDR_RX, chan, c)
                if args.settle > 0:
                    time.sleep(args.settle)
                fill_buffer(sdr, stream, buff)
                n, _ = fill_buffer(sdr, stream, buff)
                psd = compute_psd(buff[:n], fft_size, navg, window)
                notch_dc(psd, dc_half)
                powers[offset:offset + seg_len] = psd[keep]
                offset += seg_len
                if i % 16 == 0 or i == n_steps - 1:
                    done = (s * n_steps + i + 1) / (args.sweeps * n_steps)
                    sys.stdout.write(
                        f"\rsweep {s + 1}/{args.sweeps}  {human_hz(c):>12}  "
                        f"[{done * 100:5.1f}%]  {time.time() - t0:5.0f}s")
                    sys.stdout.flush()
            maxhold = np.maximum(maxhold, powers)
            avgacc += powers
    except KeyboardInterrupt:
        print("\ninterrupted - writing partial results")
    finally:
        sdr.deactivateStream(stream)
        sdr.closeStream(stream)
 
    avg = avgacc / args.sweeps
 
    n_bins = min(args.points, len(freqs))
    edges = np.linspace(freqs.min(), freqs.max(), n_bins + 1)
    fcenters = (edges[:-1] + edges[1:]) / 2.0
    max_db = rebin(freqs, maxhold, edges, "max")
    avg_db = rebin(freqs, avg, edges, "mean")
 
    meta = (f"{sdr.getHardwareKey()} spectrum survey  "
            f"{human_hz(start)}-{human_hz(stop)}  rate {human_hz(rate)}  "
            f"{args.sweeps} sweep(s) max-hold")
    save_outputs(args.out, fcenters, avg_db, max_db, meta)
    print_pollution_summary(fcenters, max_db)
 
 
def build_parser():
    p = argparse.ArgumentParser(
        description="Connect to an SDR (via SoapySDR), probe it, and receive.",
        formatter_class=argparse.ArgumentDefaultsHelpFormatter,
    )
    p.add_argument("--list", action="store_true", help="list detected devices and exit")
    p.add_argument("--probe", action="store_true", help="probe device capabilities and exit")
    p.add_argument("--args", default="", help="raw SoapySDR device args, e.g. 'driver=plutosdr,uri=ip:192.168.2.1'")
    p.add_argument("--driver", default="", help="shortcut for --args 'driver=NAME' (rtlsdr, hackrf, plutosdr, lime, uhd)")
    p.add_argument("--device", type=int, default=0, help="index into the enumerated device list when several are present")
    p.add_argument("--channel", type=int, default=0, help="RX channel index")
 
    p.add_argument("--freq", default="100M", help="center frequency (accepts k/M/G suffix)")
    p.add_argument("--rate", default="2.048M", help="sample rate (accepts k/M/G suffix)")
    p.add_argument("--gain", type=float, default=30.0, help="overall RX gain in dB")
    p.add_argument("--agc", action="store_true", help="enable hardware AGC instead of fixed gain")
    p.add_argument("--bandwidth", default="", help="analog filter bandwidth (optional)")
    p.add_argument("--antenna", default="", help="antenna port name (optional; see --probe)")
    p.add_argument("--ppm", type=float, default=0.0, help="frequency correction in ppm")
 
    p.add_argument("--fft", type=int, default=1024, help="FFT size")
    p.add_argument("--avg", type=int, default=8, help="spectra averaged per frame (Welch)")
    p.add_argument("--height", type=int, default=14, help="spectrum height in terminal rows")
    p.add_argument("--dyn-range", type=float, default=60.0, help="vertical scale span in dB")
    p.add_argument("--fps", type=float, default=15.0, help="max display refresh rate")
    p.add_argument("--duration", type=float, default=0.0, help="stop after N seconds (0 = run forever)")
 
    sw = p.add_argument_group("sweep mode")
    sw.add_argument("--sweep", action="store_true", help="step across the whole range and graph power vs frequency")
    sw.add_argument("--start", default="", help="sweep start frequency (default: device minimum)")
    sw.add_argument("--stop", default="", help="sweep stop frequency (default: device maximum)")
    sw.add_argument("--sweeps", type=int, default=1, help="number of full passes; >1 builds a max-hold (catches bursty signals)")
    sw.add_argument("--usable", type=float, default=0.8, help="fraction of each step kept (drops band-edge roll-off)")
    sw.add_argument("--settle", type=float, default=0.005, help="settle time after each retune, seconds")
    sw.add_argument("--points", type=int, default=4000, help="output resolution (frequency bins in the graph/CSV)")
    sw.add_argument("--out", default="sweep", help="output basename (writes <name>.png and <name>.csv)")
    return p
 
 
def resolve_device_args(args):
    if args.args:
        return args.args
    if args.driver:
        return f"driver={args.driver}"
 
    devices = enumerate_devices()
    if not devices:
        print_device_list(devices)
        sys.exit(1)
    if args.device >= len(devices):
        print(f"--device {args.device} out of range; {len(devices)} device(s) found.")
        print_device_list(devices)
        sys.exit(1)
    if len(devices) > 1:
        chosen = devices[args.device].get("label", devices[args.device])
        print(f"Multiple devices found; using [{args.device}] {chosen}")
        print("(override with --device N, --driver NAME, or --args ...)\n")
    return devices[args.device]
 
 
def main():
    args = build_parser().parse_args()
 
    if args.list:
        print_device_list(enumerate_devices(args.args))
        return
 
    dev_args = resolve_device_args(args)
    try:
        sdr = SoapySDR.Device(dev_args)
    except Exception as exc:
        sys.exit(f"Failed to open SDR ({dev_args!r}): {exc}")
 
    if args.probe:
        probe(sdr, args.channel)
        return
 
    if args.sweep:
        run_sweep(sdr, args)
        return
 
    probe(sdr, args.channel)
    print("\nStarting RX... (Ctrl-C to stop)\n")
    time.sleep(0.8)
    run_rx(sdr, args)
 
 
if __name__ == "__main__":
    main()

#!/usr/bin/env python3
"""Neighbour discovery: each node beacons a unique chirp and matched-filters for the rest."""
 
import argparse
import random
import sys
import time
 
import numpy as np
 
try:
    import SoapySDR
    from SoapySDR import (
        SOAPY_SDR_RX,
        SOAPY_SDR_TX,
        SOAPY_SDR_CF32,
        SOAPY_SDR_OVERFLOW,
        SOAPY_SDR_TIMEOUT,
        SOAPY_SDR_END_BURST,
    )
except ImportError:
    sys.exit(
        "SoapySDR Python bindings not found.\n"
        "  sudo apt install python3-soapysdr soapysdr-module-bladerf\n"
        "See SETUP.md."
    )
 
from sdr_rx import parse_freq, human_hz
 
 
def beacon_template(node_id, n_samples, fs, n_ids):
    t = np.arange(n_samples) / fs
    band = fs * 0.8
    slice_bw = band / n_ids
    f_lo = -band / 2 + node_id * slice_bw
    f_hi = f_lo + slice_bw
    k = (f_hi - f_lo) / (n_samples / fs)
    phase = 2 * np.pi * (f_lo * t + 0.5 * k * t * t)
    return np.exp(1j * phase).astype(np.complex64)
 
 
def build_templates(n_ids, n_samples, fs):
    return {i: beacon_template(i, n_samples, fs, n_ids) for i in range(n_ids)}
 
 
def template_crosstalk(templates):
    ids = sorted(templates)
    worst = 0.0
    for a in ids:
        for b in ids:
            if a >= b:
                continue
            cc = normalized_xcorr(templates[a], templates[b])
            worst = max(worst, float(np.max(cc)) if cc is not None else 0.0)
    return worst
 
 
def normalized_xcorr(rx, tmpl):
    m = len(tmpl)
    length = len(rx)
    if length < m:
        return None
    nfft = 1 << int(np.ceil(np.log2(length + m - 1)))
    corr = np.fft.ifft(np.fft.fft(rx, nfft) * np.fft.fft(np.conj(tmpl[::-1]), nfft))
    num = np.abs(corr[m - 1 : m - 1 + (length - m + 1)])
    csum = np.concatenate(([0.0], np.cumsum(np.abs(rx) ** 2)))
    win_energy = csum[m : length + 1] - csum[0 : length - m + 1]
    denom = np.sqrt(win_energy) * np.linalg.norm(tmpl) + 1e-12
    return num / denom
 
 
def detect(buf, templates, scan_ids, threshold):
    hits = []
    for nid in scan_ids:
        cc = normalized_xcorr(buf, templates[nid])
        if cc is None:
            continue
        peak = int(np.argmax(cc))
        if cc[peak] >= threshold:
            m = len(templates[nid])
            seg = buf[peak : peak + m]
            power_db = 10.0 * np.log10(np.mean(np.abs(seg) ** 2) + 1e-12)
            hits.append((nid, peak, float(cc[peak]), float(power_db)))
    return hits
 
 
def make_burst(template, repeats, guard, amp):
    g = np.zeros(guard, np.complex64)
    body = (amp * np.tile(template, repeats)).astype(np.complex64)
    return np.concatenate([g, body, g])
 
 
def transmit(sdr, tx_stream, samples, mtu):
    idx, total = 0, len(samples)
    while idx < total:
        chunk = samples[idx : idx + mtu]
        flags = SOAPY_SDR_END_BURST if idx + len(chunk) >= total else 0
        sr = sdr.writeStream(tx_stream, [chunk], len(chunk), flags, timeoutUs=500_000)
        if sr.ret > 0:
            idx += sr.ret
        else:
            break
 
 
def configure(sdr, chan, freq, rate, rx_gain, tx_gain, agc):
    for direction in (SOAPY_SDR_RX, SOAPY_SDR_TX):
        sdr.setSampleRate(direction, chan, rate)
        sdr.setFrequency(direction, chan, freq)
    if agc and sdr.hasGainMode(SOAPY_SDR_RX, chan):
        sdr.setGainMode(SOAPY_SDR_RX, chan, True)
    else:
        sdr.setGain(SOAPY_SDR_RX, chan, rx_gain)
 
    tx_range = sdr.getGainRange(SOAPY_SDR_TX, chan)
    if tx_gain is None:
        tx_gain = tx_range.minimum()
    sdr.setGain(SOAPY_SDR_TX, chan, tx_gain)
    return tx_gain, tx_range
 
 
def log(my_id, msg):
    ts = time.strftime("%H:%M:%S") + f".{int((time.time() % 1) * 1000):03d}"
    print(f"[{ts}] node {my_id}: {msg}", flush=True)
 
 
def run_peer(sdr, args):
    chan = args.channel
    freq = parse_freq(args.freq)
    rate = parse_freq(args.rate)
 
    tx_gain, tx_range = configure(sdr, chan, freq, rate, args.rx_gain, args.tx_gain, args.agc)
    rate = sdr.getSampleRate(SOAPY_SDR_RX, chan)
 
    n_beacon = int(args.beacon_ms * 1e-3 * rate)
    templates = build_templates(args.nodes, n_beacon, rate)
    burst = make_burst(templates[args.id], args.repeats, guard=n_beacon // 4, amp=0.6)
    burst_secs = len(burst) / rate
 
    scan_ids = [i for i in range(args.nodes) if i != args.id]
    crosstalk = template_crosstalk(templates)
 
    log(args.id, f"freq {human_hz(freq)}  rate {human_hz(rate)}  "
                 f"beacon {args.beacon_ms} ms x{args.repeats}  "
                 f"listening for nodes {scan_ids}")
    log(args.id, f"TX gain {tx_gain:g} dB "
                 f"(range {tx_range.minimum():g}..{tx_range.maximum():g}; raise gradually if no detections)")
    log(args.id, f"template crosstalk (worst): {crosstalk:.2f} "
                 f"({'good separation' if crosstalk < 0.4 else 'WARNING: IDs may alias'})")
 
    rx_stream = sdr.setupStream(SOAPY_SDR_RX, SOAPY_SDR_CF32, [chan])
    tx_stream = sdr.setupStream(SOAPY_SDR_TX, SOAPY_SDR_CF32, [chan])
    sdr.activateStream(rx_stream)
    tx_mtu = sdr.getStreamMTU(tx_stream)
 
    tx_active = False
    if args.tx_always:
        sdr.activateStream(tx_stream)
        tx_active = True
    log(args.id, f"TX power mode: {'always-on' if args.tx_always else 'pulsed (low power)'}")
 
    chunk = np.empty(args.chunk, np.complex64)
    carry = np.zeros(n_beacon - 1, np.complex64)
 
    peers = {}
    last_trigger = {}
    next_tx = time.monotonic() + random.uniform(0, args.interval)
    tx_quiet_until = 0.0
    last_status = 0.0
    read_errors = 0
 
    try:
        while True:
            now = time.monotonic()
 
            if now >= next_tx:
                if not tx_active:
                    sdr.activateStream(tx_stream)
                    tx_active = True
                transmit(sdr, tx_stream, burst, tx_mtu)
                tx_quiet_until = time.monotonic() + burst_secs + 0.02
                if not args.tx_always:
                    time.sleep(burst_secs + 0.01)
                    sdr.deactivateStream(tx_stream)
                    tx_active = False
                next_tx = time.monotonic() + args.interval + random.uniform(
                    -args.jitter, args.jitter)
                log(args.id, "TX beacon")
 
            sr = sdr.readStream(rx_stream, [chunk], len(chunk), timeoutUs=200_000)
            if sr.ret > 0:
                n = sr.ret
                read_errors = 0
            else:
                n = 0
                if sr.ret == SOAPY_SDR_TIMEOUT:
                    continue
                read_errors += 1
                if read_errors == 1 or read_errors % 50 == 0:
                    log(args.id, f"RX error {sr.ret} (x{read_errors})")
                if read_errors >= 50:
                    log(args.id, "device stopped responding (likely USB power/"
                                 "bandwidth - see SETUP troubleshooting). Aborting.")
                    break
                time.sleep(0.02)
                continue
 
            if time.monotonic() >= tx_quiet_until:
                buf = np.concatenate([carry, chunk[:n]])
                for nid, _off, corr, rssi in detect(buf, templates, scan_ids, args.threshold):
                    if now - last_trigger.get(nid, -1e9) < args.interval * 0.5:
                        peers[nid].update(last=now, corr=corr, rssi=rssi)
                        continue
                    last_trigger[nid] = now
                    was_present = peers.get(nid, {}).get("present", False)
                    peers[nid] = dict(last=now, corr=corr, rssi=rssi, present=True)
                    verb = "still present" if was_present else "DISCOVERED"
                    log(args.id, f"peer {nid} {verb}  rssi {rssi:6.1f} dBFS  corr {corr:.2f}")
                carry = buf[-(n_beacon - 1):]
 
            for nid, p in peers.items():
                if p["present"] and now - p["last"] > args.timeout:
                    p["present"] = False
                    log(args.id, f"peer {nid} LOST (silent {now - p['last']:.1f}s)")
 
            if now - last_status >= args.status:
                present = [f"{nid}({p['rssi']:.0f}dB,{now - p['last']:.1f}s)"
                           for nid, p in sorted(peers.items()) if p["present"]]
                log(args.id, f"status: peers present = {present or 'none'}")
                last_status = now
    except KeyboardInterrupt:
        log(args.id, "shutting down")
    finally:
        sdr.deactivateStream(rx_stream)
        if tx_active:
            sdr.deactivateStream(tx_stream)
        sdr.closeStream(rx_stream)
        sdr.closeStream(tx_stream)
 
 
def build_parser():
    p = argparse.ArgumentParser(
        description="SDR neighbor discovery: beacon + listen so nodes become aware of each other.",
        formatter_class=argparse.ArgumentDefaultsHelpFormatter,
    )
    p.add_argument("--id", type=int, required=True, help="this node's ID (0-based, unique per node)")
    p.add_argument("--nodes", type=int, default=2, help="total number of nodes in the network")
 
    p.add_argument("--serial", default="", help="bladeRF serial to open (from 'bladeRF-cli -e info')")
    p.add_argument("--args", default="", help="raw SoapySDR device args (overrides --serial/--driver)")
    p.add_argument("--driver", default="bladerf", help="SoapySDR driver name")
    p.add_argument("--channel", type=int, default=0, help="RX/TX channel index")
 
    p.add_argument("--freq", default="868M", help="center frequency (k/M/G suffix ok)")
    p.add_argument("--rate", default="1M", help="sample rate = beacon bandwidth")
    p.add_argument("--rx-gain", type=float, default=40.0, help="RX gain in dB")
    p.add_argument("--tx-gain", type=float, default=None, help="TX gain in dB (default: device minimum, safe for a bench)")
    p.add_argument("--agc", action="store_true", help="use RX AGC instead of fixed gain")
    p.add_argument("--tx-always", action="store_true", help="keep TX powered continuously (more current; default pulses TX only per beacon to save power)")
 
    p.add_argument("--beacon-ms", type=float, default=2.0, help="chirp duration in ms")
    p.add_argument("--repeats", type=int, default=2, help="chirp repeats per burst")
    p.add_argument("--interval", type=float, default=1.0, help="seconds between our beacons")
    p.add_argument("--jitter", type=float, default=0.3, help="+/- random jitter on interval (collision avoidance)")
    p.add_argument("--timeout", type=float, default=3.0, help="declare a peer LOST after this many silent seconds")
    p.add_argument("--threshold", type=float, default=0.35, help="detection correlation threshold (0-1)")
 
    p.add_argument("--chunk", type=int, default=8192, help="RX samples processed per loop")
    p.add_argument("--status", type=float, default=5.0, help="seconds between status summary lines")
    return p
 
 
def resolve_args(args):
    if args.args:
        return args.args
    if args.serial:
        return f"driver={args.driver},serial={args.serial}"
    return f"driver={args.driver}"
 
 
def main():
    args = build_parser().parse_args()
    if args.id >= args.nodes:
        sys.exit(f"--id {args.id} must be < --nodes {args.nodes}")
 
    dev_args = resolve_args(args)
    try:
        sdr = SoapySDR.Device(dev_args)
    except Exception as exc:
        sys.exit(f"Failed to open SDR ({dev_args!r}): {exc}")
    run_peer(sdr, args)
 
 
if __name__ == "__main__":
    main()

#!/usr/bin/env python3
"""Distributed RSS geolocation across 3 networked SDR nodes."""
 
import argparse
import json
import math
import os
import signal
import socket
import subprocess
import sys
import threading
import time
 
import numpy as np
 
DEFAULT_PORT = 52001
RESULT_PORT = 52002
SPECTRUM_PORT = 52003
 
DEFAULT_POSITIONS = [(0.0, 0.0), (-4.0, -8.0), (3.8, -5.0)]
 
PRESETS = {
    "carkey": ("433.42M", "434.42M"),
    "fpv58": ("5799M", "5801M"),
    "fpv24": ("2410M", "2430M"),
    "ism433": ("433.05M", "434.79M"),
}
 
 
_SUFFIX = {"k": 1e3, "K": 1e3, "m": 1e6, "M": 1e6, "g": 1e9, "G": 1e9}
 
 
def parse_freq(text):
    text = str(text).strip()
    if text and text[-1] in _SUFFIX:
        return float(text[:-1]) * _SUFFIX[text[-1]]
    return float(text)
 
 
def human_hz(value):
    for unit, scale in (("GHz", 1e9), ("MHz", 1e6), ("kHz", 1e3)):
        if abs(value) >= scale:
            return f"{value / scale:.6g} {unit}"
    return f"{value:.6g} Hz"
 
 
def parse_positions(text):
    out = []
    for pair in text.split(";"):
        x, y = pair.split(",")
        out.append((float(x), float(y)))
    return out
 
 
def band_spectrum(samples, fs, center, flo, fhi, fft_size, navg, window, cal_db=0.0):
    usable = min(navg, max(1, len(samples) // fft_size))
    acc = np.zeros(fft_size)
    for i in range(usable):
        seg = samples[i * fft_size : (i + 1) * fft_size]
        if len(seg) < fft_size:
            break
        spec = np.fft.fftshift(np.fft.fft(seg * window))
        acc += spec.real ** 2 + spec.imag ** 2
    acc /= usable
    acc /= np.sum(window ** 2)
    freqs = center + (np.arange(fft_size) - fft_size // 2) * (fs / fft_size)
    mask = (freqs >= flo) & (freqs <= fhi)
    psd_db = 10.0 * np.log10(acc + 1e-12) + cal_db
    if not mask.any():
        nan = float("nan")
        return {"freqs": freqs, "psd": psd_db, "power": nan, "noise": nan,
                "noise_bin": nan, "snr": nan, "flo": flo, "fhi": fhi, "center": center}
 
    peak_lin = float(acc[mask].max())
    noise_per_bin = float(np.median(acc))
    return {
        "freqs": freqs, "psd": psd_db,
        "power": 10.0 * np.log10(peak_lin + 1e-12) + cal_db,
        "noise": 10.0 * np.log10(noise_per_bin + 1e-12) + cal_db,
        "noise_bin": 10.0 * np.log10(noise_per_bin + 1e-12) + cal_db,
        "snr": 10.0 * np.log10(peak_lin / (noise_per_bin + 1e-12) + 1e-12),
        "flo": flo, "fhi": fhi, "center": center,
    }
 
 
def band_measure(samples, fs, center, flo, fhi, fft_size, navg, window, cal_db=0.0):
    s = band_spectrum(samples, fs, center, flo, fhi, fft_size, navg, window, cal_db)
    return s["power"], s["noise"], s["snr"]
 
 
def weighted_centroid(positions, powers_db):
    pos = np.asarray(positions, float)
    p = np.asarray(powers_db, float)
    w = np.power(10.0, (p - np.nanmax(p)) / 10.0)
    w = np.nan_to_num(w)
    return (w[:, None] * pos).sum(axis=0) / w.sum()
 
 
def rss_to_distance(powers_db, n_exp, p_ref_db, d_ref):
    p = np.asarray(powers_db, float)
    return d_ref * np.power(10.0, (p_ref_db - p) / (10.0 * n_exp))
 
 
def trilaterate(positions, distances):
    pos = np.asarray(positions, float)
    d = np.asarray(distances, float)
    x0, y0 = pos[0]
    rows, rhs = [], []
    for i in range(1, len(pos)):
        xi, yi = pos[i]
        rows.append([2.0 * (x0 - xi), 2.0 * (y0 - yi)])
        rhs.append(d[i] ** 2 - d[0] ** 2 - xi ** 2 + x0 ** 2 - yi ** 2 + y0 ** 2)
    sol, *_ = np.linalg.lstsq(np.array(rows), np.array(rhs), rcond=None)
    return sol
 
 
def compute_offsets(avg_db, positions, ref_pos, n_exp):
    pos = np.asarray(positions, float)
    if ref_pos is not None:
        d = np.hypot(pos[:, 0] - ref_pos[0], pos[:, 1] - ref_pos[1])
        expected = -10.0 * n_exp * np.log10(np.maximum(d, 1e-3))
    else:
        expected = np.zeros(len(positions))
 
    present = [i for i, p in enumerate(avg_db) if p is not None]
    gain = {i: avg_db[i] - expected[i] for i in present}
    target = float(np.mean([gain[i] for i in present])) if present else 0.0
    return [target - gain[i] if i in gain else None for i in range(len(positions))]
 
 
def bearing_from_center(positions, est):
    c = np.asarray(positions, float).mean(axis=0)
    dx, dy = est[0] - c[0], est[1] - c[1]
    compass = (90.0 - math.degrees(math.atan2(dy, dx))) % 360.0
    return compass, math.hypot(dx, dy), c
 
 
_COMPASS = ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]
 
 
def compass_label(deg):
    return _COMPASS[int((deg + 22.5) % 360 // 45)]
 
 
def ascii_map(positions, est, width=46, height=18):
    pts = list(positions) + ([tuple(est)] if est is not None else [])
    xs = [p[0] for p in pts]
    ys = [p[1] for p in pts]
    minx, maxx = min(xs), max(xs)
    miny, maxy = min(ys), max(ys)
    spanx = (maxx - minx) or 1.0
    spany = (maxy - miny) or 1.0
    pad_x, pad_y = spanx * 0.1, spany * 0.1
    minx, maxx = minx - pad_x, maxx + pad_x
    miny, maxy = miny - pad_y, maxy + pad_y
 
    grid = [[" "] * width for _ in range(height)]
 
    def put(x, y, ch):
        col = int((x - minx) / (maxx - minx) * (width - 1))
        row = int((y - miny) / (maxy - miny) * (height - 1))
        row = height - 1 - row
        if 0 <= row < height and 0 <= col < width:
            grid[row][col] = ch
 
    for i, p in enumerate(positions):
        put(p[0], p[1], str(i))
    if est is not None:
        put(est[0], est[1], "*")
    body = "\n".join("|" + "".join(r) + "|" for r in grid)
    top = "+" + "-" * width + "+   N up"
    return top + "\n" + body + "\n" + "+" + "-" * width + "+"
 
 
def sensor_loop(args, stop_event, shared=None):
    try:
        import SoapySDR
        from SoapySDR import (SOAPY_SDR_RX, SOAPY_SDR_CF32,
                              SOAPY_SDR_TIMEOUT, SOAPY_SDR_OVERFLOW)
    except ImportError:
        print("SoapySDR not found - this device runs as a viewer only "
              "(install python3-soapysdr / radioconda to make it a sensor).", file=sys.stderr)
        return
 
    if args.freq_lo and args.freq_hi:
        flo, fhi = parse_freq(args.freq_lo), parse_freq(args.freq_hi)
    elif args.preset in PRESETS:
        flo, fhi = (parse_freq(x) for x in PRESETS[args.preset])
    else:
        print(f"give --freq-lo/--freq-hi or a valid --preset {list(PRESETS)}", file=sys.stderr)
        return
    if fhi <= flo:
        print("--freq-hi must be above --freq-lo", file=sys.stderr)
        return
    center = (flo + fhi) / 2.0
    rate = parse_freq(args.rate) if args.rate else max((fhi - flo) * 1.5, 1e6)
 
    dev_args = args.args or (f"driver={args.driver},serial={args.serial}"
                             if args.serial else f"driver={args.driver}")
    try:
        sdr = SoapySDR.Device(dev_args)
    except Exception as exc:
        print(f"could not open SDR ({dev_args!r}): {exc}", file=sys.stderr)
        return
 
    chan = args.channel
    sdr.setSampleRate(SOAPY_SDR_RX, chan, rate)
    sdr.setFrequency(SOAPY_SDR_RX, chan, center)
    rate = sdr.getSampleRate(SOAPY_SDR_RX, chan)
    if args.agc and sdr.hasGainMode(SOAPY_SDR_RX, chan):
        sdr.setGainMode(SOAPY_SDR_RX, chan, True)
    else:
        sdr.setGain(SOAPY_SDR_RX, chan, args.gain)
 
    fft_size, navg = args.fft, args.avg
    window = np.hanning(fft_size).astype(np.float32)
    buff = np.empty(fft_size * navg, np.complex64)
 
    stream = sdr.setupStream(SOAPY_SDR_RX, SOAPY_SDR_CF32, [chan])
    sdr.activateStream(stream)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    dest = (args.coord_host, args.port)
    emit_spectrum = getattr(args, "emit_spectrum", False)
    spec_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) if emit_spectrum else None
 
    print(f"sensor {args.id}: {human_hz(flo)}..{human_hz(fhi)} @ {human_hz(rate)} "
          f"-> {args.coord_host}:{args.port}", flush=True)
    period = 1.0 / args.rate_hz
    last_send = 0.0
    best = None
    try:
        while not stop_event.is_set():
            n = 0
            while n < len(buff):
                sr = sdr.readStream(stream, [buff[n:]], len(buff) - n, timeoutUs=1_000_000)
                if sr.ret > 0:
                    n += sr.ret
                elif sr.ret == SOAPY_SDR_OVERFLOW:
                    continue
                else:
                    break
            if n < fft_size:
                continue
            s = band_spectrum(buff[:n], rate, center, flo, fhi, fft_size, navg, window, args.cal)
            if best is None or s["snr"] > best["snr"]:
                best = s
            now = time.monotonic()
            if now - last_send < period:
                continue
            last_send = now
            s, best = best, None
            power, noise, snr = s["power"], s["noise"], s["snr"]
            if emit_spectrum:
                step = max(1, len(s["psd"]) // 256)
                try:
                    spec_sock.sendto(json.dumps({
                        "freqs": np.asarray(s["freqs"])[::step].round(0).tolist(),
                        "psd": np.asarray(s["psd"])[::step].round(1).tolist(),
                        "noise_bin": round(float(s["noise_bin"]), 1),
                        "snr": round(float(s["snr"]), 1), "flo": s["flo"], "fhi": s["fhi"],
                    }).encode(), ("127.0.0.1", SPECTRUM_PORT))
                except OSError:
                    pass
            try:
                sock.sendto(json.dumps({
                    "node": args.id, "ts": time.time(), "flo": flo, "fhi": fhi,
                    "power_db": round(power, 2), "noise_db": round(noise, 2),
                    "snr_db": round(snr, 2),
                }).encode(), dest)
            except OSError:
                pass
            if args.verbose:
                print(f"  node {args.id}: {power:7.1f} dBFS  SNR {snr:5.1f} dB", flush=True)
    finally:
        try:
            sdr.deactivateStream(stream)
        except Exception:
            pass
        sdr.closeStream(stream)
 
 
def run_sensor(args):
    stop = threading.Event()
    try:
        sensor_loop(args, stop)
    except KeyboardInterrupt:
        stop.set()
 
 
def run_coordinator(args):
    positions = parse_positions(args.positions) if args.positions else DEFAULT_POSITIONS
    n_nodes = len(positions)
    cal = [float(c) for c in args.node_cal.split(",")] if args.node_cal else [0.0] * n_nodes
 
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(("0.0.0.0", args.port))
    sock.settimeout(0.2)
 
    latest = {}
    state = {"smooth_est": None}
    last_render = 0.0
 
    print(f"coordinator: listening on UDP :{args.port} for {n_nodes} nodes")
    print("node positions (m):", positions)
    sys.stdout.write("\033[2J")
    try:
        while True:
            try:
                data, _addr = sock.recvfrom(2048)
                m = json.loads(data.decode())
                nid = int(m["node"])
                if 0 <= nid < n_nodes:
                    m["power_db"] = float(m["power_db"]) + cal[nid]
                    latest[nid] = m
            except socket.timeout:
                pass
            except (ValueError, KeyError):
                continue
 
            now = time.time()
            if now - last_render >= 1.0 / args.refresh:
                render_coordinator(args, positions, latest, cal, now, state)
                last_render = now
    except KeyboardInterrupt:
        sys.stdout.write("\n")
 
 
def compute_estimate(args, positions, latest, now, state):
    n_nodes = len(positions)
    fresh = {i: m for i, m in latest.items() if now - m["ts"] <= args.window}
    nodes_info, powers, detected = [], [None] * n_nodes, 0
    for i in range(n_nodes):
        if i in fresh:
            p = fresh[i]["power_db"]
            snr = float(fresh[i].get("snr_db", float("nan")))
            powers[i] = p
            hit = (snr >= args.min_snr) and (p >= args.threshold)
            detected += hit
            nodes_info.append({"id": i, "x": positions[i][0], "y": positions[i][1],
                               "power": round(p, 1), "snr": round(snr, 1),
                               "det": bool(hit), "age": round(now - fresh[i]["ts"], 1)})
        else:
            nodes_info.append({"id": i, "x": positions[i][0], "y": positions[i][1],
                               "power": None, "snr": None, "det": False, "age": None})
 
    have = [i for i in range(n_nodes) if powers[i] is not None]
    present = detected >= args.min_detect
    est = bearing = dist = tri = None
    if present and len(have) >= 2:
        pw = np.array([powers[i] for i in have])
        pos = [positions[i] for i in have]
        c = np.asarray(weighted_centroid(pos, pw), float)
        center = np.asarray(positions, float).mean(axis=0)
        e = center + args.proj_gain * (c - center)
        if len(have) >= 3:
            d = rss_to_distance(pw, args.pathloss_n, args.p_ref, args.d_ref)
            try:
                tri = [float(v) for v in trilaterate(pos, d)]
            except Exception:
                tri = None
        if args.smooth > 0 and state.get("smooth_est") is not None:
            e = args.smooth * e + (1 - args.smooth) * np.asarray(state["smooth_est"], float)
        state["smooth_est"] = e
        bearing, dist, _ = bearing_from_center(positions, e)
        est = [float(e[0]), float(e[1])]
 
    return {
        "target": args.target_name, "present": present, "detected": detected,
        "min_detect": args.min_detect, "have": len(have), "min_snr": args.min_snr,
        "online": len(have), "online_ids": have,
        "est": est, "tri": tri,
        "bearing": (round(bearing, 1) if bearing is not None else None),
        "dist": (round(dist, 1) if dist is not None else None),
        "nodes": nodes_info, "n_nodes": n_nodes,
        "positions": [[float(x), float(y)] for x, y in positions],
    }
 
 
def render_coordinator(args, positions, latest, cal, now, state):
    res = compute_estimate(args, positions, latest, now, state)
    lines = ["RSS geolocation coordinator", ""]
    for nd in res["nodes"]:
        if nd["power"] is None:
            lines.append(f"  node {nd['id']} @ ({nd['x']},{nd['y']}):    --- no data ---")
        else:
            flag = "DET" if nd["det"] else "   "
            lines.append(f"  node {nd['id']} @ ({nd['x']},{nd['y']}): "
                         f"{nd['power']:7.1f} dBFS  SNR {nd['snr']:5.1f} dB  {flag}  ({nd['age']:.1f}s ago)")
    lines.append(f"\nSDRs online: {res['online']}/{res['n_nodes']}  (ids {res['online_ids']})")
    lines.append("")
    if res["present"]:
        e = res["est"]
        lines.append(f">>> {res['target']} DETECTED on {res['detected']} node(s) <<<")
        lines.append(f"  weighted-centroid estimate : x={e[0]:6.1f}  y={e[1]:6.1f} m")
        lines.append(f"  direction from array center: {res['bearing']:5.1f} deg "
                     f"({compass_label(res['bearing'])}), {res['dist']:.1f} m out")
        if res["tri"]:
            lines.append(f"  trilateration: x={res['tri'][0]:6.1f}  y={res['tri'][1]:6.1f} m")
    elif res["have"] < 3:
        lines.append(f"waiting for >=3 nodes (have {res['have']})...")
    else:
        lines.append(f"signal below noise threshold ({res['detected']}/{res['min_detect']} "
                     f"nodes with SNR >= {args.min_snr} dB) - no emitter")
    lines.append("")
    lines.append(ascii_map(positions, res["est"]))
    lines.append("\n(0/1/2 = nodes, * = estimate)   Ctrl-C to stop")
    sys.stdout.write("\033[H" + "\n".join(ln + "\033[K" for ln in lines) + "\033[J")
    sys.stdout.flush()
 
 
def run_calibrate(args):
    positions = parse_positions(args.positions) if args.positions else DEFAULT_POSITIONS
    n_nodes = len(positions)
    ref = parse_positions(args.ref_pos)[0] if args.ref_pos else None
 
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(("0.0.0.0", args.port))
    sock.settimeout(0.2)
 
    lin = {i: [] for i in range(n_nodes)}
    where = "equidistant source" if ref is None else f"source at {ref}"
    print(f"Calibrating {n_nodes} nodes over {args.duration:.0f}s ({where}).")
    print("Make sure all sensors are running and hearing the reference signal...\n")
 
    end = time.time() + args.duration
    while time.time() < end:
        try:
            data, _ = sock.recvfrom(2048)
            m = json.loads(data.decode())
            nid = int(m["node"])
            if 0 <= nid < n_nodes:
                lin[nid].append(10.0 ** (float(m["power_db"]) / 10.0))
        except socket.timeout:
            pass
        except (ValueError, KeyError):
            continue
        remaining = end - time.time()
        counts = " ".join(f"n{i}:{len(lin[i])}" for i in range(n_nodes))
        sys.stdout.write(f"\r  {remaining:4.1f}s left   samples [{counts}]   ")
        sys.stdout.flush()
 
    avg_db = [10.0 * math.log10(np.mean(lin[i])) if lin[i] else None
              for i in range(n_nodes)]
    cal = compute_offsets(avg_db, positions, ref, args.pathloss_n)
 
    print("\n\nResults:")
    for i in range(n_nodes):
        if avg_db[i] is None:
            print(f"  node {i}: NO DATA - is sensor {i} running and hearing the signal?")
        else:
            print(f"  node {i}: avg {avg_db[i]:7.2f} dBFS  ->  cal {cal[i]:+.2f} dB")
 
    if all(c is not None for c in cal):
        node_cal = ",".join(f"{c:.2f}" for c in cal)
        print("\nApply on the coordinator (single place, no sensor restart):")
        print(f"  python3 geolocate.py --role coordinator --target-name DRONE "
              f"--node-cal {node_cal}")
        print("\nOr bake into each sensor with its own --cal value above.")
    else:
        print("\nIncomplete: not all nodes reported. Fix the missing node(s) and rerun.")
 
 
def synth_latest(positions, emitter, args, now):
    out = {}
    for i, (x, y) in enumerate(positions):
        d = max(math.hypot(x - emitter[0], y - emitter[1]), args.d_ref)
        power = args.p_ref - 10.0 * args.pathloss_n * math.log10(d / args.d_ref)
        out[i] = {"node": i, "ts": now, "power_db": power, "snr_db": 25.0,
                  "flo": 0, "fhi": 0}
    return out
 
 
def fuse_thread(args, positions, cal, stop_event):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(("0.0.0.0", args.port))
    sock.settimeout(0.2)
    out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    latest, addrs, state, last = {}, set(), {"smooth_est": None}, 0.0
    n_nodes = len(positions)
    sim = getattr(args, "simulate", False)
    sim_center = parse_positions(args.sim_pos)[0] if sim and args.sim_pos else None
    t0 = time.monotonic()
    print(f"  [fuser] collecting on :{args.port}, broadcasting on :{RESULT_PORT}"
          f"{'  [SIMULATE around ' + str(sim_center) + ']' if sim else ''}")
    while not stop_event.is_set():
        try:
            data, addr = sock.recvfrom(2048)
            m = json.loads(data.decode())
            nid = int(m["node"])
            if 0 <= nid < n_nodes:
                m["power_db"] = float(m["power_db"]) + cal[nid]
                latest[nid] = m
                addrs.add(addr[0])
        except socket.timeout:
            pass
        except (ValueError, KeyError, OSError):
            continue
        now = time.time()
        if now - last >= 1.0 / args.refresh:
            if sim:
                ang = 2.0 * math.pi * (time.monotonic() - t0) / 20.0
                emitter = (sim_center[0] + 3.0 * math.cos(ang),
                           sim_center[1] + 3.0 * math.sin(ang))
                data_now = synth_latest(positions, emitter, args, now)
            else:
                data_now = latest
            payload = json.dumps(compute_estimate(args, positions, data_now, now, state)).encode()
            for ip in addrs | {"127.0.0.1"}:
                try:
                    out.sendto(payload, (ip, RESULT_PORT))
                except OSError:
                    pass
            last = now
 
 
def result_listener(shared, stop_event):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        sock.bind(("0.0.0.0", RESULT_PORT))
    except OSError as exc:
        print(f"could not bind result port {RESULT_PORT}: {exc}", file=sys.stderr)
        return
    sock.settimeout(0.3)
    while not stop_event.is_set():
        try:
            data, _ = sock.recvfrom(16384)
            shared["result"] = json.loads(data.decode())
        except socket.timeout:
            pass
        except (ValueError, OSError):
            continue
 
 
def spectrum_listener(shared, stop_event):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        sock.bind(("127.0.0.1", SPECTRUM_PORT))
    except OSError as exc:
        print(f"could not bind spectrum port {SPECTRUM_PORT}: {exc}", file=sys.stderr)
        return
    sock.settimeout(0.3)
    while not stop_event.is_set():
        try:
            data, _ = sock.recvfrom(65535)
            shared["spectrum"] = json.loads(data.decode())
        except socket.timeout:
            pass
        except (ValueError, OSError):
            continue
 
 
def gui_ascii_fallback(args, positions, shared, stop_event):
    n_total = len(positions)
    try:
        while not stop_event.is_set():
            res = shared.get("result")
            est = res["est"] if res and res.get("est") else None
            online_ids = res.get("online_ids", []) if res else []
            others = len([i for i in online_ids if i != args.id])
            sys.stdout.write("\033[H\033[J")
            print(f"SDRs online: {len(online_ids)}/{n_total}   "
                  f"(other SDRs: {others}/{n_total - 1})   this = node {args.id}")
            if est:
                print(f">>> {args.target_name} DETECTED  {res['bearing']:.0f} deg "
                      f"({compass_label(res['bearing'])}), {res['dist']:.0f} m from center")
            else:
                got = res["detected"] if res else 0
                print(f"{args.target_name}: scanning...  ({got} node detection(s))")
            print(ascii_map([tuple(p) for p in positions], est))
            print("\n(0/1/2 = nodes, * = drone)   Ctrl-C to stop")
            time.sleep(1.0 / max(args.refresh, 1))
    except KeyboardInterrupt:
        pass
    finally:
        stop_event.set()
 
 
def run_gui(args, positions, shared, stop_event):
    try:
        import pyqtgraph as pg
        from pyqtgraph.Qt import QtWidgets, QtCore
        pos = np.asarray(positions, float)
        app = QtWidgets.QApplication.instance() or QtWidgets.QApplication([])
 
        win = QtWidgets.QWidget()
        win.setWindowTitle("Azimuth")
        win.resize(1320 if args.spectrum else 760, 740)
        root = QtWidgets.QVBoxLayout(win)
        alert = QtWidgets.QLabel("")
        alert.setAlignment(QtCore.Qt.AlignCenter)
        alert.setVisible(False)
        root.addWidget(alert)
        status = QtWidgets.QLabel("")
        status.setStyleSheet("font-size:13px; padding:4px; color:#555;")
        root.addWidget(status)
        row = QtWidgets.QHBoxLayout()
        root.addLayout(row)
        blink = {"on": False, "was_detected": False}
 
        mapw = pg.PlotWidget()
        mapw.setAspectLocked(True)
        mapw.showGrid(x=True, y=True, alpha=0.3)
        mapw.setLabel("bottom", "x (m)")
        mapw.setLabel("left", "y (m)")
        pad = max(8.0, 0.5 * max(np.ptp(pos[:, 0]), np.ptp(pos[:, 1]), 1.0))
        mapw.setXRange(pos[:, 0].min() - pad, pos[:, 0].max() + pad)
        mapw.setYRange(pos[:, 1].min() - pad, pos[:, 1].max() + pad)
        mapw.setMouseEnabled(x=False, y=False)
        row.addWidget(mapw, 3)
        line_item = pg.PlotCurveItem(pen=pg.mkPen((220, 40, 40), width=2,
                                                  style=QtCore.Qt.DashLine))
        node_item = pg.ScatterPlotItem(size=20, symbol="s", pen=pg.mkPen("k"))
        drone_item = pg.ScatterPlotItem(size=30, symbol="star",
                                        brush=pg.mkBrush(220, 40, 40), pen=pg.mkPen("k"))
        mapw.addItem(line_item)
        mapw.addItem(node_item)
        mapw.addItem(drone_item)
        labels = []
        for i, (x, y) in enumerate(positions):
            t = pg.TextItem("", anchor=(0, 1))
            t.setPos(x, y)
            mapw.addItem(t)
            labels.append(t)
 
        scurve = noiseline = specw = None
        if args.spectrum:
            specw = pg.PlotWidget()
            specw.showGrid(x=True, y=True, alpha=0.3)
            specw.setLabel("bottom", "MHz")
            specw.setLabel("left", "power (dB)")
            row.addWidget(specw, 2)
            scurve = specw.plot(pen=pg.mkPen((40, 120, 220), width=1))
            noiseline = pg.InfiniteLine(angle=0, pen=pg.mkPen((150, 150, 150),
                                                              style=QtCore.Qt.DashLine))
            specw.addItem(noiseline)
 
        def tick():
            if stop_event.is_set():
                app.quit()
                return
            res = shared.get("result")
            nodes = (res["nodes"] if res else
                     [{"id": i, "x": pos[i, 0], "y": pos[i, 1], "snr": None, "power": None}
                      for i in range(len(pos))])
            online = sum(1 for nd in nodes if nd.get("power") is not None)
            spots = []
            for nd in nodes:
                up = nd.get("power") is not None
                spots.append({"pos": (nd["x"], nd["y"]),
                              "brush": pg.mkBrush(40, 120, 220) if up else pg.mkBrush(160, 160, 160)})
                labels[nd["id"]].setText(f"{nd['id']}: {nd['power']:.0f} dBFS" if up else f"{nd['id']}: --")
            node_item.setData(spots)
            status.setText(f"online {online}/{len(pos)}")
            if res and res.get("est"):
                ex, ey = res["est"]
                cx, cy = pos[:, 0].mean(), pos[:, 1].mean()
                drone_item.setData([{"pos": (ex, ey)}])
                line_item.setData([cx, ex], [cy, ey])
                dist_me = float(np.hypot(ex - pos[args.id, 0], ey - pos[args.id, 1]))
                blink["on"] = not blink["on"]
                bg = "#e02424" if blink["on"] else "#7a0000"
                alert.setStyleSheet(f"font-size:22px; font-weight:bold; padding:12px; "
                                    f"color:white; background:{bg}; border-radius:8px;")
                alert.setText(f"{args.target_name}   {res['bearing']:.0f}°   {dist_me:.0f} m")
                alert.setVisible(True)
                if not blink["was_detected"]:
                    QtWidgets.QApplication.beep()
                    blink["was_detected"] = True
            else:
                drone_item.setData([])
                line_item.setData([], [])
                alert.setVisible(False)
                blink["was_detected"] = False
            if specw is not None:
                sp = shared.get("spectrum")
                if sp:
                    scurve.setData([f / 1e6 for f in sp["freqs"]], sp["psd"])
                    noiseline.setValue(sp["noise_bin"])
                    det = sp["snr"] >= args.min_snr
                    specw.setTitle(f"SNR {sp['snr']:.0f} dB",
                                   color="#dd2222" if det else "#888888")
 
        timer = QtCore.QTimer()
        timer.timeout.connect(tick)
        timer.start(int(1000 / max(args.refresh, 1)))
        win.show()
        try:
            app.exec_()
        finally:
            stop_event.set()
    except Exception as exc:
        print(f"GUI unavailable ({exc}); using terminal map instead.", file=sys.stderr)
        return gui_ascii_fallback(args, positions, shared, stop_event)
 
 
def run_node(args):
    positions = parse_positions(args.positions) if args.positions else DEFAULT_POSITIONS
    cal = [float(c) for c in args.node_cal.split(",")] if args.node_cal else [0.0] * len(positions)
    stop = threading.Event()
    shared = {"result": None, "spectrum": None}
 
    sensor_proc = None
    if not getattr(args, "simulate", False):
        sensor_cmd = [sys.executable, os.path.abspath(__file__), "--role", "sensor",
                      "--id", str(args.id), "--coord-host", args.coord_host,
                      "--port", str(args.port), "--gain", str(args.gain),
                      "--rate-hz", str(args.rate_hz), "--preset", args.preset,
                      "--driver", args.driver]
        for flag, val in (("--freq-lo", args.freq_lo), ("--freq-hi", args.freq_hi),
                          ("--rate", args.rate), ("--serial", args.serial), ("--args", args.args)):
            if val:
                sensor_cmd += [flag, val]
        if args.cal:
            sensor_cmd += ["--cal", str(args.cal)]
        if args.agc:
            sensor_cmd += ["--agc"]
        if args.spectrum:
            sensor_cmd += ["--emit-spectrum"]
        creationflags = subprocess.CREATE_NEW_PROCESS_GROUP if os.name == "nt" else 0
        sensor_proc = subprocess.Popen(sensor_cmd, creationflags=creationflags)
 
    workers = [threading.Thread(target=result_listener, args=(shared, stop), daemon=True)]
    if args.spectrum:
        workers.append(threading.Thread(target=spectrum_listener, args=(shared, stop), daemon=True))
    if args.fuse:
        workers.append(threading.Thread(target=fuse_thread, args=(args, positions, cal, stop), daemon=True))
 
    print(f"node {args.id}: {'FUSER + ' if args.fuse else ''}sensor(subprocess) + GUI  "
          f"(fuser at {args.coord_host}:{args.port})")
    for w in workers:
        w.start()
    try:
        run_gui(args, positions, shared, stop)
    except KeyboardInterrupt:
        pass
    finally:
        stop.set()
        try:
            if sensor_proc is None:
                raise RuntimeError("no sensor (simulate)")
            if os.name == "nt":
                sensor_proc.send_signal(signal.CTRL_BREAK_EVENT)
            else:
                sensor_proc.send_signal(signal.SIGINT)
            sensor_proc.wait(timeout=3)
        except Exception:
            try:
                sensor_proc.terminate()
            except Exception:
                pass
 
 
def build_parser():
    p = argparse.ArgumentParser(
        description="Distributed RSS geolocation across 3 networked SDR nodes.",
        formatter_class=argparse.ArgumentDefaultsHelpFormatter,
    )
    p.add_argument("--role", required=True, choices=["node", "sensor", "coordinator", "calibrate"],
                   help="'node' = sensor + GUI on each device (the demo); one node adds --fuse")
    p.add_argument("--port", type=int, default=DEFAULT_PORT, help="UDP power port")
 
    s = p.add_argument_group("sensor / node")
    s.add_argument("--id", type=int, default=0, help="this node's id (0-based)")
    s.add_argument("--fuse", action="store_true", help="this node also fuses + broadcasts results (run on exactly one node)")
    s.add_argument("--spectrum", action="store_true", help="also open a live spectrum window of the watched band")
    s.add_argument("--emit-spectrum", action="store_true", help=argparse.SUPPRESS)
    s.add_argument("--coord-host", default="127.0.0.1", help="fuser IP/hostname (Tailscale IP; 127.0.0.1 on the fuser itself)")
    s.add_argument("--preset", default="carkey", help=f"band preset if --freq-lo/-hi omitted: {list(PRESETS)}")
    s.add_argument("--freq-lo", default="", help="low edge of band (overrides --preset)")
    s.add_argument("--freq-hi", default="", help="high edge of band (overrides --preset)")
    s.add_argument("--rate", default="", help="sample rate (default: auto from band width)")
    s.add_argument("--gain", type=float, default=40.0, help="RX gain dB")
    s.add_argument("--agc", action="store_true", help="use AGC instead of fixed gain")
    s.add_argument("--cal", type=float, default=0.0, help="per-sensor power offset dB (match nodes' gains)")
    s.add_argument("--rate-hz", type=float, default=5.0, help="measurements per second to send")
    s.add_argument("--fft", type=int, default=1024)
    s.add_argument("--avg", type=int, default=8)
    s.add_argument("--channel", type=int, default=0)
    s.add_argument("--driver", default="bladerf")
    s.add_argument("--serial", default="")
    s.add_argument("--args", default="", help="raw SoapySDR device args")
    s.add_argument("--verbose", action="store_true")
 
    c = p.add_argument_group("coordinator")
    c.add_argument("--positions", default="", help="'x,y;x,y;x,y' in meters (default: ~100 m triangle)")
    c.add_argument("--min-snr", type=float, default=11.0, help="in-band SNR over noise floor (dB) to count as a detection")
    c.add_argument("--threshold", type=float, default=-200.0, help="optional absolute power floor dBFS (default off)")
    c.add_argument("--min-detect", type=int, default=2, help="nodes detecting to declare a signal present")
    c.add_argument("--window", type=float, default=2.0, help="seconds a measurement stays 'fresh'")
    c.add_argument("--refresh", type=float, default=4.0, help="display updates per second")
    c.add_argument("--smooth", type=float, default=0.3, help="estimate smoothing 0..1 (lower=smoother); 0 disables")
    c.add_argument("--proj-gain", dest="proj_gain", type=float, default=2.0, help="push estimate outward from array center (1=centroid/inside, >1=outside)")
    c.add_argument("--target-name", default="DRONE", help="label shown on detection")
    c.add_argument("--node-cal", dest="node_cal", default="", help="coordinator-side per-node cal 'd0,d1,d2' dB")
    c.add_argument("--pathloss-n", type=float, default=2.5, help="path-loss exponent for trilateration")
    c.add_argument("--p-ref", type=float, default=-30.0, help="power (dBFS) at reference distance d-ref")
    c.add_argument("--d-ref", type=float, default=1.0, help="reference distance (m) for p-ref")
 
    cal = p.add_argument_group("calibrate")
    cal.add_argument("--duration", type=float, default=8.0, help="seconds to average during calibration")
    cal.add_argument("--ref-pos", default="", help="calibration source position 'x,y' (default: assume equidistant)")
 
    sim = p.add_argument_group("simulation")
    sim.add_argument("--simulate", action="store_true", help="fuser fabricates a detection (no real RF) to rehearse the demo")
    sim.add_argument("--sim-pos", dest="sim_pos", default="6,5", help="simulated emitter position 'x,y' (outside the triangle)")
    return p
 
 
def main():
    args = build_parser().parse_args()
    if args.role == "node":
        run_node(args)
    elif args.role == "sensor":
        run_sensor(args)
    elif args.role == "calibrate":
        run_calibrate(args)
    else:
        run_coordinator(args)
 
 
if __name__ == "__main__":
    main()