"""Voogedastuse DSP (olekuga, plokkide piiridel katkestusteta): IQ -> kanal -> FM stereo 48 kHz.

Kasutavad fm_listen.py (kuulamine) ja music_recorder.py. Plokk võib olla suvalise pikkusega.
MPX: (L+R) + piloot sin(θ) 19 kHz + (L-R) sin(2θ) 38 kHz. Analüütilisest piloodist z = -i·e^{iθ}
-> (z/|z|)² = -e^{2iθ} -> sin(2θ) = -Im(...). (Vana stereo_from_mpx võttis Re -> L-R oli ~0, s.t mono.)
"""
import numpy as np
from scipy.signal import firwin, oaconvolve, lfilter

AUDIO_FS = 48000


class StreamFir:
    """FIR + dekimeerimine ploki kaupa: ajalugu hoitakse, faas jookseb üle plokkide."""
    def __init__(self, h, down=1):
        self.h, self.down = np.asarray(h), down
        self.hist = None
        self.phase = 0

    def __call__(self, x):
        k = len(self.h) - 1
        if self.hist is None:
            self.hist = np.zeros(k, dtype=np.result_type(x, self.h))
        buf = np.concatenate([self.hist, x])
        self.hist = buf[len(buf) - k:]
        y = oaconvolve(buf, self.h, mode="valid")          # len(y) == len(x)
        if self.down > 1:
            y = y[self.phase::self.down]
            self.phase = (self.phase - len(x)) % self.down
        return y


class Delay:
    def __init__(self, n):
        self.n, self.hist = n, None

    def __call__(self, x):
        if self.hist is None:
            self.hist = np.zeros(self.n, dtype=x.dtype)
        buf = np.concatenate([self.hist, x])
        self.hist = buf[len(buf) - self.n:]
        return buf[:len(x)]


class Ddc:
    """Kompleksne IQ fs_in -> kanal nihkega `offset` Hz, fs_out (fs_in / fs_out peab olema täisarv)."""
    def __init__(self, fs_in, offset, fs_out=240_000, cutoff=100_000):
        self.fs, self.offset = fs_in, offset
        self.down = int(round(fs_in / fs_out))
        self.fs_out = fs_in / self.down
        self.fir = StreamFir(firwin(129, cutoff, fs=fs_in).astype(np.float32), self.down)
        self.ph = 0.0

    def __call__(self, iq):
        if self.offset:
            n = np.arange(len(iq))
            w = -2 * np.pi * self.offset / self.fs
            iq = iq * np.exp(1j * (self.ph + w * n)).astype(np.complex64)
            self.ph = (self.ph + w * len(iq)) % (2 * np.pi)
        return self.fir(iq)


class FmStereo:
    """240 kHz kompleksne kanal -> (n, 2) float32 48 kHz L/R. Piloodita (või --mono) -> mono."""
    def __init__(self, fs=240_000, deviation=75e3, mono=False, tau=50e-6):
        self.fs, self.mono = fs, mono
        self.k = fs / (2 * np.pi * deviation)                 # täis-deviatsioon -> 1.0
        self.prev = np.complex64(1)
        self.down = int(round(fs / AUDIO_FS))
        n_p = 257
        lp_p = firwin(n_p, 400, fs=fs)
        # kandja faas nulli viite keskpunktis -> klapib mpx_delay((n_p-1)/2)-ga (muidu 38 kHz-l ~96° viga)
        self.pilot = StreamFir((lp_p * np.exp(2j * np.pi * 19e3 * (np.arange(n_p) - (n_p - 1) / 2) / fs)).astype(np.complex64))
        self.mpx_delay = Delay((n_p - 1) // 2)
        lp_a = firwin(255, 15_000, fs=fs).astype(np.float32)
        self.lpr = StreamFir(lp_a, self.down)
        self.lmr = StreamFir(lp_a, self.down)
        a = np.exp(-1 / (AUDIO_FS * tau))
        self.de_b, self.de_a = [1 - a], [1, -a]
        self.de_z = np.zeros((1, 2))
        self.pilot_level = 0.0

    def mpx(self, x):
        y = np.concatenate([[self.prev], x])
        self.prev = x[-1]
        return (np.angle(y[1:] * np.conj(y[:-1])) * self.k).astype(np.float32)

    def __call__(self, x):
        m = self.mpx(x)
        self.mpx_last = m                                       # RDS jaoks (normeeritud: 75 kHz = 1.0)
        z = self.pilot(m)
        lvl = float(np.mean(np.abs(z)))                     # piloot ~0.05 (9 % deviatsioonist); < 0.01 = mono jaam
        self.pilot_level = lvl if self.pilot_level == 0 else 0.9 * self.pilot_level + 0.1 * lvl
        md = self.mpx_delay(m)
        lpr = self.lpr(md)
        if self.mono or self.pilot_level < 0.01:
            lmr = np.zeros_like(lpr)
            self.lmr(md * 0)                                    # hoia olek sünkroonis
        else:
            c = z / (np.abs(z) + 1e-9)
            s38 = -np.imag(c * c).astype(np.float32)           # sin(2θ)
            lmr = self.lmr(2 * md * s38)
        out = np.stack([lpr + lmr, lpr - lmr], axis=1)
        out, self.de_z = lfilter(self.de_b, self.de_a, out, axis=0, zi=self.de_z)
        return out.astype(np.float32)
