"""SSB kandja peenhäälestus hääle harmoonikute järgi: proovi nihkeid ±300 Hz, igaühe juures demoduleeri ja mõõda, kui hästi
spekter sobib kammiga k·f0 (f0 80–300 Hz). Õige nihe → harmoonikud täpselt kordsetel. Väljund: parim nihe + demod (*_usb/_lsb.wav).
Kasutus: python ssbpitch.py iq.wav 3.739 --lsb [--range 300 --step 20]"""
import argparse, os, sys
import numpy as np

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from iq import load_iq, center_freq_from_name, extract_channel, ssb_demod, save_wav

ap = argparse.ArgumentParser(); ap.add_argument("wav"); ap.add_argument("mhz", type=float); ap.add_argument("--lsb", action="store_true")
ap.add_argument("--range", type=float, default=300); ap.add_argument("--step", type=float, default=20)
a = ap.parse_args()
fc = center_freq_from_name(a.wav); fs, iq = load_iq(a.wav)
x0, fs2 = extract_channel(iq, fs, int(round(a.mhz * 1e6)) - fc, out_fs=12e3)
t = np.arange(len(x0)) / fs2
N = 4096; f = np.fft.rfftfreq(N, 1 / fs2)
best = None
for d in np.arange(-a.range, a.range + 1, a.step):
    x = x0 * np.exp(-2j * np.pi * d * t)
    au, afs = ssb_demod(x, fs2, usb=not a.lsb)
    # raamid 85 ms, ainult energilised (kõne) raamid
    Nf = 1024; ff = np.fft.rfftfreq(Nf, 1 / afs); hop = Nf // 2
    frames = [au[i:i + Nf] for i in range(0, len(au) - Nf, hop)]
    e = np.array([np.sum(fr ** 2) for fr in frames]); keep = e > np.percentile(e, 60)
    score = 0.0
    for fr, k in zip(frames, keep):
        if not k:
            continue
        S = np.abs(np.fft.rfft(fr * np.hanning(Nf))); S = S / (S.sum() + 1e-9)
        bestc = 0.0
        for f0 in np.arange(90, 300, 4):
            h = np.arange(1, int(2500 / f0) + 1) * f0
            idx = np.round(h / (afs / Nf)).astype(int)
            bestc = max(bestc, S[idx].sum())
        score += bestc
    if best is None or score > best[0]:
        best = (score, d, au, afs)
    print(f"  nihe {d:+5.0f} Hz  kammiskoor {score:.3f}")
score, d, au, afs = best
mhz = a.mhz + d / 1e6
path = os.path.splitext(a.wav)[0] + f"_{mhz:.4f}MHz_{'lsb' if a.lsb else 'usb'}.wav"
save_wav(path, au, afs)
print(f"parim nihe {d:+.0f} Hz → kandja {mhz:.4f} MHz -> {path}")
