"""Reconstruct per-cycle duty from a G1-G4 logic capture and FFT it to find
the low-frequency (audible) modulation. G1=T1 (outer/Timer F), G2=T2 (inner/
Timer E). common = main-loop duty, diff = V_fly duty-asymmetry (vfly_correction)."""
import sys
import numpy as np

CSV = sys.argv[1] if len(sys.argv) > 1 else "digital.csv"

# --- load (int8 gates to keep memory down) ---
try:
    import pandas as pd
    df = pd.read_csv(CSV, dtype={"Time [s]": np.float64, "G1": np.int8,
                                 "G2": np.int8, "G3": np.int8, "G4": np.int8})
    t = df["Time [s]"].to_numpy()
    G = {c: df[c].to_numpy() for c in ("G1", "G2", "G3", "G4")}
except ImportError:
    raw = np.genfromtxt(CSV, delimiter=",", skip_header=1)
    t = raw[:, 0]
    G = {f"G{i}": raw[:, i].astype(np.int8) for i in range(1, 5)}

print(f"rows={len(t)}  span={t[-1]-t[0]:.4f}s")


def duty_series(g):
    """Return (cycle_start_times, duty[0..1]) from rising/falling edges of g."""
    dg = np.diff(g.astype(np.int16))
    tr = t[np.where(dg == 1)[0] + 1]      # rising-edge times (cycle starts)
    tf = t[np.where(dg == -1)[0] + 1]     # falling-edge times
    idx = np.searchsorted(tf, tr, side="right")
    valid = idx < len(tf)
    tr, hi = tr[valid], tf[np.clip(idx[valid], 0, len(tf) - 1)] - tr[valid]
    period = np.diff(tr)
    return tr[:-1], hi[:-1] / period


t_o, d_o = duty_series(G["G1"])           # outer
t_i, d_i = duty_series(G["G2"])           # inner
fsw = 1.0 / np.median(np.diff(t_o))
print(f"f_sw ~= {fsw/1e3:.1f} kHz   outer cycles={len(d_o)}  inner cycles={len(d_i)}")
print(f"D_outer mean={d_o.mean():.4f} std={d_o.std():.4f} | "
      f"D_inner mean={d_i.mean():.4f} std={d_i.std():.4f}")

# resample both onto a uniform grid at f_sw, align, build common / diff
t0, t1 = max(t_o[0], t_i[0]), min(t_o[-1], t_i[-1])
fs = fsw
tu = np.arange(t0, t1, 1.0 / fs)
do_u = np.interp(tu, t_o, d_o)
di_u = np.interp(tu, t_i, d_i)
common = 0.5 * (do_u + di_u)              # main control loop
diff = 0.5 * (do_u - di_u)                # V_fly asymmetry (= corr/MAX_DUTY)


def top_peaks(x, label, n=6, fmin=50.0, fmax=40e3):
    x = x - x.mean()
    win = np.hanning(len(x))
    X = np.abs(np.fft.rfft(x * win)) / (len(x) * 0.5)
    f = np.fft.rfftfreq(len(x), d=1.0 / fs)
    band = (f >= fmin) & (f <= fmax)
    fb, Xb = f[band], X[band]
    order = np.argsort(Xb)[::-1]
    # de-duplicate neighbouring bins (keep local maxima >= 25 Hz apart)
    picks, fseen = [], []
    for i in order:
        if all(abs(fb[i] - fs0) > 25 for fs0 in fseen):
            picks.append(i); fseen.append(fb[i])
        if len(picks) >= n:
            break
    print(f"\n[{label}]  rms={x.std()*1e3:.3f}e-3 duty")
    for i in picks:
        print(f"   {fb[i]:9.1f} Hz   amp={Xb[i]*1e3:8.4f}e-3   "
              f"({Xb[i]/Xb[order[0]]*100:5.1f}% of peak)")
    return fb[order[0]], Xb[order[0]]


top_peaks(common, "COMMON  (main loop)")
top_peaks(diff,   "DIFF    (V_fly asymmetry)")
top_peaks(do_u,   "OUTER   (G1/Timer F)")

# strongest line above the mains band, + harmonic-ladder check
def spectrum(x):
    x = x - x.mean()
    X = np.abs(np.fft.rfft(x * np.hanning(len(x)))) / (len(x) * 0.5)
    f = np.fft.rfftfreq(len(x), d=1.0 / fs)
    return f, X

fc, Xc = spectrum(common)
above = fc > 400.0
ipk = np.argmax(Xc[above])
fpk = fc[above][ipk]
print(f"\n[COMMON strongest >400Hz] {fpk:.1f} Hz")
print(f"  f_sw/f_pk = {fsw/fpk:.2f} ; 50kHz-loop/f_pk = {50e3/fpk:.3f}")

# --- zoom 300-1500 Hz: where is the 500-600 Hz energy, common or diff? ---
def band_peak(x, lbl, lo=300, hi=1500):
    f, X = spectrum(x)
    b = (f >= lo) & (f <= hi)
    i = np.argmax(X[b])
    print(f"  [{lbl}] {lo}-{hi}Hz peak: {f[b][i]:7.1f} Hz  amp={X[b][i]*1e3:.4f}e-3")
print("\n=== 300-1500 Hz band ===")
band_peak(common, "COMMON")
band_peak(diff,   "DIFF  ")

# --- detect large cycle-to-cycle duty steps (per-cycle, not resampled) ---
def steps(td, d, lbl, thr=0.02):
    jumps = np.abs(np.diff(d))
    idx = np.where(jumps > thr)[0]
    print(f"\n[{lbl}] cyc-to-cyc |dD|>{thr*100:.0f}%: {len(idx)} events "
          f"(of {len(d)} cyc), max step={jumps.max()*100:.2f}%")
    if len(idx) > 3:
        ev_t = td[idx + 1]
        gaps = np.diff(ev_t)
        gaps = gaps[gaps > 1e-4]              # ignore bursts within one event
        if len(gaps):
            med = np.median(gaps)
            print(f"   median spacing {med*1e3:.3f} ms -> {1/med:.1f} Hz "
                  f"(min {gaps.min()*1e3:.2f}ms max {gaps.max()*1e3:.2f}ms)")
steps(t_o, d_o, "OUTER step rate")
steps(t_i, d_i, "INNER step rate")

# plot
try:
    import matplotlib
    matplotlib.use("Agg")
    import matplotlib.pyplot as plt
    fig, ax = plt.subplots(3, 1, figsize=(11, 9), sharex=True)
    for a, (x, lbl) in zip(ax, [(common, "COMMON (main loop)"),
                                (diff, "DIFF (V_fly asymmetry)"),
                                (do_u, "OUTER (G1)")]):
        f, X = spectrum(x)
        m = f <= 50e3
        a.semilogy(f[m] / 1e3, X[m])
        a.set_ylabel(lbl + "\nduty amp"); a.grid(True, which="both", alpha=0.3)
        a.axvline(24.77, color="r", ls=":", lw=1)
    ax[-1].set_xlabel("kHz")
    fig.tight_layout()
    fig.savefig("debug_console/duty_fft.png", dpi=110)
    print("\nsaved debug_console/duty_fft.png")
except Exception as e:
    print("plot skipped:", e)