#!/usr/bin/env python3
"""
Remove the radar Doppler from a SigMF recording of a Moon or Venus bounce.

The Doppler of the sub-radar point is computed with SPICE once per second
(reception time), interpolated per sample with a cubic spline, and removed by
multiplying with the conjugate of the integrated phase. The expected start of
the echo (from the transmit time) is added to the SigMF metadata.

The transmit time is taken from (in order of preference):
  1. --tx-time
  2. the 3rd field of core:description, e.g.
     "venus-mfsk4096-rs12-29s.sigmf-meta;75;1791024362;moon" (unix time)
  3. the start of the file + --tx-offset (default 1.0 s)

The output is ci16_le by default. The rotation moves |x| onto I and Q, so a
recording with I and Q both near full scale (e.g. transmitter leakage) clips
and produces spurs; use --autoscale or --datatype cf32_le for such files.

--decimate N lowpass filters around the Doppler-corrected 0 Hz and keeps every
Nth sample. The filter is linear phase with its delay removed, so output sample
k lies at input sample k * N and all times (core:datetime, echo) stay valid.
"""

import argparse
import datetime
import json
import pathlib
import re
import sys

import numpy as np
import scipy.interpolate
import scipy.signal
import spiceypy as sp

KERNEL_DIR = "/Users/telkamp/notebooks/kernels"
KERNELS = [
    "naif0012.tls",
    "de440s.bsp",
    "pck00011.tpc",
    "earth_latest_high_prec.bpc",
    "antennas.fk",
    "antennas.bsp",
]

SITES = ["dwingeloo", "stockert", "effelsberg", "neugolm", "lovell"]
TARGETS = {"moon": "IAU_MOON", "venus": "IAU_VENUS"}

DTYPES = {
    "ci16_le": np.dtype("<i2"),
    "cf32_le": np.dtype("<f4"),
}


def radar_surface_dlt(target, surf_vec, surf_frame, t_rx, transmitter, receiver=None):
    """Like radar_surface_dlt in venus-predictions.ipynb, but also returns the light time."""
    if receiver is None:
        receiver = transmitter
    target_radius = sp.bodvrd(target, 'RADII', 3)[1][0]
    surf_vec = np.array(surf_vec)
    surf_vec = target_radius * sp.unorm(surf_vec)[0]
    ez_r, lt_r = sp.spkcpt(surf_vec, target, surf_frame, t_rx, 'J2000', 'OBSERVER', 'CN', receiver)
    ez_d, lt_d = sp.spkcpo(transmitter, t_rx - lt_r, 'J2000', 'OBSERVER', 'CN', surf_vec, target, surf_frame)
    dlt_r = sp.dvnorm(ez_r) / sp.clight()
    ez_d[3:] *= 1 - dlt_r
    dlt_d = sp.dvnorm(ez_d) / sp.clight()
    return dlt_r + dlt_d, lt_r + lt_d


def radar_srp(target, target_frame, t_rx, transmitter, receiver):
    """Light time derivative and round trip light time of the sub-radar point."""
    srp, _, _ = sp.subpnt('INTERCEPT/ELLIPSOID', target, t_rx, target_frame, 'CN', receiver)
    return radar_surface_dlt(target, srp, target_frame, t_rx, transmitter, receiver)


def echo_reception_time(target, target_frame, et_tx, transmitter, receiver):
    """Solve et_rx - lt(et_rx) = et_tx for the reception time of the leading edge."""
    et_rx = et_tx
    for _ in range(10):
        _, lt = radar_srp(target, target_frame, et_rx, transmitter, receiver)
        et_new = et_tx + lt
        if abs(et_new - et_rx) < 1e-9:
            break
        et_rx = et_new
    return et_new, lt


def et_to_iso(et):
    return sp.et2utc(et, "ISOC", 6)


def unix_to_et(t_unix):
    utc = datetime.datetime.fromtimestamp(t_unix, datetime.timezone.utc)
    return sp.str2et(utc.strftime("%Y-%m-%dT%H:%M:%S.%f"))


def parse_time(s):
    """Unix time (number) or ISO UTC string to ET."""
    try:
        return unix_to_et(float(s))
    except ValueError:
        return sp.str2et(s.rstrip("Z"))


def parse_description(desc):
    """Parse 'waveform.sigmf-meta;nr;tx_unix_time;target' -> (tx_unix, target, duration)."""
    fields = desc.split(";") if desc else []
    tx_unix, target, duration = None, None, None
    if len(fields) >= 3:
        try:
            tx_unix = float(fields[2])
        except ValueError:
            pass
    if len(fields) >= 4 and fields[3].strip().lower() in TARGETS:
        target = fields[3].strip().lower()
    if fields:
        m = re.search(r"-(\d+(?:\.\d+)?)s(?:ec)?\.sigmf", fields[0])
        if m:
            duration = float(m.group(1))
    return tx_unix, target, duration


def decimation_filter(n):
    """Linear-phase lowpass for decimation by n: 64 n + 1 taps, cutoff 0.85 x output Nyquist, ~80 dB stopband."""
    return scipy.signal.firwin(64 * n + 1, 0.85 / n, window=("kaiser", 8.0))


def filter_bands(h, fs_in, fs_out):
    """(flat to Hz within 0.1 dB, attenuation in dB of everything that aliases into the output band)."""
    w, H = scipy.signal.freqz(h, worN=1 << 16, fs=fs_in)
    db = 20 * np.log10(np.maximum(np.abs(H), 1e-12))
    flat = w[np.argmax(db < -0.1)]
    alias_db = -db[w >= fs_out / 2].max()
    return flat, alias_db


def sigmf_paths(path):
    path = pathlib.Path(path)
    base = path.with_suffix("") if path.suffix in (".sigmf-meta", ".sigmf-data") else path
    return base, base.with_name(base.name + ".sigmf-meta"), base.with_name(base.name + ".sigmf-data")


def load_kernels(kernel_dir=KERNEL_DIR):
    for kernel in KERNELS:
        sp.furnsh(str(pathlib.Path(kernel_dir) / kernel))


# filename prefix (e.g. "wsrt_venus_...") -> SPICE receiver site
RX_SITES = {"dwingeloo": "DWINGELOO", "stockert": "STOCKERT", "wsrt": "WSRT_RT2", "effelsberg": "EFFELSBERG",
            "neugolm": "NEUGOLM", "lovell": "LOVELL", "jodrell": "LOVELL"}   # jodrell_...: the Jodrell recordings


def site_target_from_name(name):
    """'dwingeloo_venus_2026_...' -> ('DWINGELOO', 'venus'): receiver from the first field, target from any field."""
    fields = pathlib.Path(name).name.lower().split("_")
    receiver = RX_SITES.get(fields[0])
    target = next((f for f in fields if f in TARGETS), None)
    return receiver, target


class RadarDoppler:
    """
    Sub-radar point Doppler of a recording, removed from its samples in memory. Same model as process(): SPICE
    Doppler once per second of reception time, cubic spline, phase integrated from the start of the file.
    start_time: capture start (ISO UTC string or np.datetime64), sample index 0 is at start_time.
    """

    def __init__(self, start_time, duration_s, sample_rate, f_tx, target, transmitter, receiver):
        if target not in TARGETS:
            raise ValueError("target %r unknown, one of %s" % (target, list(TARGETS)))
        self.target, self.target_frame = target.upper(), TARGETS[target]
        self.transmitter, self.receiver = transmitter.upper(), receiver.upper()
        self.sample_rate, self.f_tx = sample_rate, f_tx
        self.et_start = sp.str2et(str(start_time).rstrip("Z"))
        self.t_grid = np.arange(-2.0, np.ceil(duration_s) + 3.0)
        dlt = np.array([radar_srp(self.target, self.target_frame, self.et_start + t, self.transmitter, self.receiver)[0]
                        for t in self.t_grid])
        self.doppler = -dlt * f_tx
        self.phase_cycles = scipy.interpolate.CubicSpline(self.t_grid, self.doppler).antiderivative()
        self.cycles0 = self.phase_cycles(0.0)

    def echo(self, tx_unix):
        """(ET of the leading edge of the echo of a transmission at unix time tx_unix, round trip time in s)."""
        return echo_reception_time(self.target, self.target_frame, unix_to_et(tx_unix),
                                   self.transmitter, self.receiver)

    def echo_time(self, tx_unix):
        """Reception time (ISO UTC) of the leading edge of the echo of a transmission at unix time tx_unix."""
        return sp.et2utc(self.echo(tx_unix)[0], "ISOC", 9) + "Z"

    def doppler_at(self, t_iso):
        """Removed Doppler in Hz at an ISO UTC time."""
        return float(np.interp(sp.str2et(t_iso.rstrip("Z")) - self.et_start, self.t_grid, self.doppler))

    def remove(self, x, start):
        """x (samples start, start + 1, ... of the recording) with the Doppler removed."""
        cycles = np.mod(self.phase_cycles((start + np.arange(len(x))) / self.sample_rate) - self.cycles0, 1.0)
        return x * np.exp(-2j * np.pi * cycles)


class DopplerCorrected:
    """A recording (venus_mfsk Recording or PolPair) with the radar Doppler removed on read. Reads outside the file stay zero."""

    def __init__(self, rec, target, transmitter, receiver, f_tx=None, label=None):
        self.rec, self.label = rec, label
        self.sample_rate, self.start_time = rec.sample_rate, rec.start_time
        self.sample_start, self.center_freq, self.n_samples = rec.sample_start, rec.center_freq, rec.n_samples
        self.meta = rec.meta
        self.radar = RadarDoppler(rec.start_time, rec.n_samples / rec.sample_rate, rec.sample_rate,
                                  float(rec.center_freq) if f_tx is None else f_tx, target, transmitter, receiver)
        self.target, self.receiver = self.radar.target, self.radar.receiver
        self.echo_time, self.doppler_at = self.radar.echo_time, self.radar.doppler_at

    def contains(self, start, count):
        return self.rec.contains(start, count)

    def sample_index(self, t_iso):
        return self.rec.sample_index(t_iso)

    def read(self, start, count):
        return self.radar.remove(self.rec.read(start, count), start - self.sample_start).astype(np.complex64)


def main():
    parser = argparse.ArgumentParser(description="Remove radar Doppler from a SigMF Moon/Venus bounce recording")
    parser.add_argument("sigmf", nargs="+",
                        help="SigMF file(s) (.sigmf-meta, .sigmf-data or base name), all processed with the same options")
    parser.add_argument("--tx", choices=SITES, default="dwingeloo", help="transmitter site (default: dwingeloo)")
    parser.add_argument("--rx", choices=SITES, default="dwingeloo", help="receiver site (default: dwingeloo)")
    parser.add_argument("--target", choices=list(TARGETS),
                        help="target (default: from core:description or tracker:object_name)")
    parser.add_argument("--tx-time", help="transmit start time, unix time or ISO UTC (overrides core:description)")
    parser.add_argument("--tx-offset", type=float, default=1.0,
                        help="transmit start relative to file start in s, if no tx time is known (default: 1.0)")
    parser.add_argument("--tx-length", "--tx-duration", dest="tx_length", type=float,
                        help="transmit length in s, for the echo annotation length (default: from waveform name)")
    parser.add_argument("--tx-freq", type=float,
                        help="transmit frequency in Hz (default: core:frequency of the capture)")
    parser.add_argument("--datatype", choices=list(DTYPES), default="ci16_le",
                        help="output datatype (default: ci16_le)")
    parser.add_argument("--autoscale", action="store_true",
                        help="scale an integer output down if the rotated signal would clip (default: off)")
    parser.add_argument("--decimate", type=int, default=1,
                        help="integer decimation factor, lowpass filtered around the Doppler-corrected 0 Hz "
                             "(default: 1, no decimation)")
    parser.add_argument("--kernel-dir", default=KERNEL_DIR, help=f"SPICE kernel directory (default: {KERNEL_DIR})")
    parser.add_argument("-o", "--output", help="output base name, only with a single input file (default: <input>_dedop)")
    parser.add_argument("--chunk", type=int, default=1 << 20, help="samples per processing chunk")
    args = parser.parse_args()

    if args.output and len(args.sigmf) > 1:
        parser.error("-o/--output can only be used with a single input file")

    load_kernels(args.kernel_dir)

    failed = []
    for i, sigmf in enumerate(args.sigmf):
        if i:
            print()
        try:
            process(sigmf, args)
        except (Exception, SystemExit) as e:          # report and continue with the next file
            print(f"Error:       {sigmf}: {e}")
            failed.append(sigmf)
    if len(args.sigmf) > 1:
        print(f"\nProcessed {len(args.sigmf) - len(failed)} of {len(args.sigmf)} files")
    if failed:
        for sigmf in failed:
            print(f"Failed:      {sigmf}")
        sys.exit(1)


def process(sigmf, args):
    """Doppler-correct one SigMF file and write <input>_dedop (or --output)."""
    base, meta_path, data_path = sigmf_paths(sigmf)
    meta = json.loads(meta_path.read_text())
    glob = meta["global"]
    capture = meta["captures"][0]

    if glob["core:datatype"] not in DTYPES:
        raise SystemExit(f"Unsupported datatype {glob['core:datatype']}")
    fs = float(glob["core:sample_rate"])
    in_dtype = DTYPES[glob["core:datatype"]]
    nsamples = data_path.stat().st_size // (2 * in_dtype.itemsize)
    duration = nsamples / fs
    et_start = sp.str2et(capture["core:datetime"].rstrip("Z"))

    decim = args.decimate
    if decim < 1:
        raise SystemExit("--decimate must be a positive integer")
    fs_out = fs / decim
    n_out = nsamples // decim
    f_tx = args.tx_freq if args.tx_freq is not None else float(capture["core:frequency"])

    desc_tx_unix, desc_target, desc_duration = parse_description(glob.get("core:description", ""))

    target = args.target or desc_target or glob.get("tracker:object_name", "").lower() or None
    if target not in TARGETS:
        raise SystemExit("Target unknown, specify --target")
    target_frame = TARGETS[target]
    transmitter, receiver = args.tx.upper(), args.rx.upper()

    if args.tx_time is not None:
        et_tx, tx_source = parse_time(args.tx_time), "option"
    elif desc_tx_unix is not None:
        et_tx, tx_source = unix_to_et(desc_tx_unix), "core:description"
    else:
        et_tx, tx_source = et_start + args.tx_offset, "offset"
    tx_length = args.tx_length if args.tx_length is not None else desc_duration

    # Expected start of the echo (leading edge = sub-radar point)
    et_echo, rtt = echo_reception_time(target.upper(), target_frame, et_tx, transmitter, receiver)
    echo_offset = et_echo - et_start
    echo_sample = int(round(echo_offset * fs_out))

    # Doppler per second of reception time, covering the whole file
    t_grid = np.arange(-2.0, np.ceil(duration) + 3.0)
    dlt = np.array([radar_srp(target.upper(), target_frame, et_start + t, transmitter, receiver)[0]
                    for t in t_grid])
    doppler = -dlt * f_tx
    doppler_echo = float(np.interp(echo_offset, t_grid, doppler))

    print(f"File:        {data_path.name}")
    print(f"Start:       {et_to_iso(et_start)}  ({duration:.3f} s, {nsamples} samples, {fs:g} sps)")
    print(f"Target:      {target}  TX: {transmitter}  RX: {receiver}  f_tx: {f_tx / 1e6:.6f} MHz")
    print(f"TX time:     {et_to_iso(et_tx)}  (from {tx_source}, {et_tx - et_start:+.6f} s rel. to file start)")
    print(f"Round trip:  {rtt:.6f} s")
    print(f"Echo start:  {et_to_iso(et_echo)}  ({echo_offset:+.6f} s, sample {echo_sample})")
    print(f"Doppler:     {doppler[2]:.2f} Hz at file start, {doppler[-3]:.2f} Hz at file end, "
          f"{doppler_echo:.2f} Hz at echo start")

    if decim > 1:
        h = decimation_filter(decim)
        half = (len(h) - 1) // 2                    # filter delay in input samples, removed below
        flat_hz, alias_db = filter_bands(h, fs, fs_out)
        print(f"Decimation:  {decim}x to {fs_out:g} sps, {len(h)} taps, flat (0.1 dB) to +-{flat_hz:.1f} Hz, "
              f"aliases suppressed by {alias_db:.0f} dB")
        if fs % decim:
            print(f"Warning:     output sample rate {fs_out:.6f} sps is not an integer")

    # Per-sample phase from the integral of the interpolated Doppler
    phase_cycles = scipy.interpolate.CubicSpline(t_grid, doppler).antiderivative()
    cycles0 = phase_cycles(0.0)

    out_base = pathlib.Path(args.output) if args.output else base.with_name(base.name + "_dedop")
    out_base, out_meta, out_data = sigmf_paths(out_base)
    out_dtype = DTYPES[args.datatype]

    raw = np.memmap(data_path, dtype=in_dtype, mode="r", shape=(nsamples * 2,))

    def dedop(n0, n1):
        """Doppler-corrected samples [n0, n1), zeros outside the file."""
        y = np.zeros(n1 - n0, dtype=np.complex128)
        a, b = max(n0, 0), min(n1, nsamples)
        if b > a:
            iq = raw[2 * a:2 * b].astype(np.float64)
            cycles = np.mod(phase_cycles(np.arange(a, b) / fs) - cycles0, 1.0)
            y[a - n0:b - n0] = (iq[0::2] + 1j * iq[1::2]) * np.exp(-2j * np.pi * cycles)
        return y

    def blocks():
        """Output samples in chunks. Decimated output sample k is centred on input sample k * decim."""
        if decim == 1:
            for n0 in range(0, nsamples, args.chunk):
                yield dedop(n0, min(n0 + args.chunk, nsamples))
            return
        lag = 2 * half // decim                     # 2 * half is a multiple of decim
        step = max(args.chunk // decim, 1)
        for k0 in range(0, n_out, step):
            k1 = min(k0 + step, n_out)
            x = dedop(k0 * decim - half, (k1 - 1) * decim + half + 1)
            yield scipy.signal.upfirdn(h, x, down=decim)[lag:lag + k1 - k0]

    # The rotation maps |x| onto I and Q, so an integer output needs headroom for the peak magnitude
    scale = 1.0
    if args.autoscale and out_dtype.kind == "i":
        peak = max(np.abs(y).max() for y in blocks())
        full_scale = np.iinfo(out_dtype).max
        if peak > full_scale:
            scale = full_scale / peak
            print(f"Scaling:     peak |x| = {peak:.0f} > {full_scale}, output scaled by {scale:.4f} "
                  f"({20 * np.log10(scale):.2f} dB) to avoid clipping")

    clipped = 0
    with open(out_data, "wb") as f:
        for y in blocks():
            y = scale * y
            out = np.empty(2 * len(y), dtype=np.float64)
            out[0::2], out[1::2] = y.real, y.imag
            if out_dtype.kind == "i":
                info = np.iinfo(out_dtype)
                out = np.round(out)
                clipped += np.count_nonzero((out > info.max) | (out < info.min))
                out = np.clip(out, info.min, info.max)
            out.astype(out_dtype).tofile(f)
    if clipped:
        print(f"Warning:     {clipped} output values clipped, use --autoscale or --datatype cf32_le")

    # Metadata
    glob["core:datatype"] = args.datatype
    if decim > 1:
        glob["core:sample_rate"] = int(fs_out) if fs_out == int(fs_out) else fs_out
        glob.update({
            "radar:decimation": decim,
            "radar:original_sample_rate": glob.get("radar:original_sample_rate", fs),
            "radar:decimation_filter": f"Kaiser FIR, {len(h)} taps, cutoff {0.85 * fs_out / 2:.3f} Hz",
            "radar:passband_hz": round(float(flat_hz), 3),
        })
        for c in meta["captures"]:
            c["core:sample_start"] = c["core:sample_start"] // decim
        for a in meta.get("annotations", []):
            a["core:sample_start"] = a["core:sample_start"] // decim
            if "core:sample_count" in a:
                a["core:sample_count"] = max(a["core:sample_count"] // decim, 1)
    glob.update({
        "radar:target": target,
        "radar:transmitter": args.tx,
        "radar:receiver": args.rx,
        "radar:tx_frequency": f_tx,
        "radar:tx_time": et_to_iso(et_tx),
        "radar:tx_time_source": tx_source,
        "radar:round_trip_time_s": rtt,
        "radar:echo_time": et_to_iso(et_echo),
        "radar:echo_start_s": echo_offset,
        "radar:echo_sample": echo_sample,
        "radar:doppler_removed": True,
        "radar:output_scale": scale,
        "radar:doppler_reference": "sub-radar point",
        "radar:doppler_at_echo_hz": doppler_echo,
        "radar:doppler_time_s": t_grid.tolist(),
        "radar:doppler_hz": [round(float(d), 4) for d in doppler],
    })
    if tx_length is not None:
        glob["radar:tx_length_s"] = tx_length

    # Echo annotation, clipped to the file
    echo_end = n_out if tx_length is None else int(round((echo_offset + tx_length) * fs_out))
    a0, a1 = max(echo_sample, 0), min(echo_end, n_out)
    annotations = [a for a in meta.get("annotations", []) if a.get("core:label") != "echo"]
    if a1 > a0:
        annotation = {
            "core:sample_start": a0,
            "core:label": "echo",
            "core:comment": f"expected {target} echo, tx {et_to_iso(et_tx)}, rtt {rtt:.6f} s",
        }
        if tx_length is not None:
            annotation["core:sample_count"] = a1 - a0
        annotations.append(annotation)
    else:
        print("Warning: expected echo is outside the recording")
    meta["annotations"] = sorted(annotations, key=lambda a: a["core:sample_start"])

    out_meta.write_text(json.dumps(meta, indent=4) + "\n")
    print(f"Wrote:       {out_data.name}, {out_meta.name}")


if __name__ == "__main__":
    main()
