"""CW AFC (Automatic Frequency Control).

Schaetzt die Momentanfrequenz des komplexen Basisbandsignals waehrend
erkannter Marks (Phasendifferenz zwischen aufeinanderfolgenden Samples,
siehe /themen/synchronisation/frequenzregelung-afc/) und korrigiert damit
laufend die Mittenfrequenz des vorgeschalteten freq_xlating_fir_filter -
per Nachrichtenport 'freq' (PMT-Paar (intern("freq"), double)), das dieser
GNU-Radio-Block nativ unterstuetzt.

Reiner Integrator (kein PI-Regler): Bei perfekt zentriertem Ton ist die
gemessene Basisband-Momentanfrequenz 0 Hz; jede Restfrequenz wird gedaempft
(loop_gain) aufintegriert und der Mittenfrequenz aufaddiert, bis der Ton
wieder zentriert ist.

Zwei Lehren aus dem ersten (fehlgeschlagenen) Versuch, siehe Artikeltext:

1. Die geformten Ein-/Austastflanken (Abschnitt 3, Stufe 1) erzeugen waehrend
   der Anstiegs-/Abfallzeit kurzzeitig sehr grosse, aber bedeutungslose
   Momentanfrequenz-Ausschlaege (die Amplitudenrampe selbst sieht die
   Phasendifferenz-Schaetzung als "Frequenz"). Die ersten/letzten
   guard_samples jedes Marks werden deshalb von der Mittelung ausgenommen.
2. Das Gate darf NICHT vom Threshold-Zweig (nach Noise-Floor-Normalizer)
   kommen: Dieser hat durch den gleitenden Mittelwert im Noise-Floor-Block
   eine eigene Verzoegerung von mehreren zehn Samples, die relativ zum
   direkten Hilbert-Zweig nicht ausgeglichen ist. Ein aus einem verzoegerten
   Zweig abgeleitetes Gate liegt dadurch systematisch leicht neben den
   echten Mark-Graenzen - genug, um die Frequenzschaetzung mit falschem
   Vorzeichen zu verfaelschen (beobachtet: Korrektur lief in die falsche
   Richtung weg, obwohl das Testsignal nach oben driftete). Der AFC-Block
   bildet sein Mark/Space-Gate deshalb selbst, direkt aus der Huellkurve
   desselben komplexen Eingangssignals, dessen Phase er auswertet.

Args:
    samp_rate: Abtastrate des komplexen Eingangssignals in Hz (working_rate
        der Kette, z. B. 1000.0).
    pitch: Aktuelle Soll-Mittenfrequenz des vorgeschalteten Freq Xlating FIR
        Filters in Hz; dient als Basis, auf die die laufende Korrektur
        aufaddiert wird (new_center = pitch + correction).
    loop_gain: Schleifenverstaerkung des Integrators (dimensionslos, 0..1).
        Bestimmt, wie stark eine pro Mark gemessene Restfrequenz in die
        naechste Korrektur einfliesst - zu hoch schwingt ueber, zu niedrig
        hinkt der Drift hinterher. Empirisch ermittelt: 0.05.
    guard_samples: Anzahl Samples am Anfang und Ende jedes Marks, die von
        der Frequenzmittelung ausgeschlossen werden, um die durch die
        geformten Ein-/Austastflanken verursachten Momentanfrequenz-
        Ausschlaege nicht mit einzurechnen.

Ports:
    in: Komplexes Basisbandsignal (np.complex64) direkt aus dem Freq
        Xlating FIR Filter - dieselbe Huellkurve, deren Phase ausgewertet
        wird, liefert auch das Mark/Space-Gate (kein Fremdzweig).
    freq (Message-Out): PMT-Paar (intern("freq"), double) mit der neuen
        Mittenfrequenz - vom Freq Xlating FIR Filter nativ als Nachrichten-
        Handler fuer seinen center_freq-Parameter unterstuetzt.
"""
import numpy as np
from gnuradio import gr
import pmt


class blk(gr.sync_block):
    def __init__(self, samp_rate=1000.0, pitch=700.0, loop_gain=0.05, guard_samples=15):
        gr.sync_block.__init__(
            self, name="CW AFC",
            in_sig=[np.complex64], out_sig=None)
        self.samp_rate = samp_rate
        self.pitch = pitch
        self.loop_gain = loop_gain
        self.guard_samples = guard_samples
        self.correction = 0.0
        self.prev_sample = 0 + 0j
        self.prev_gate = 0
        self._mark_finst = []
        self._peak = 1e-6
        self.message_port_register_out(pmt.intern("freq"))
        self.last_correction_log = []  # (sample_idx, correction_hz) - fuer Auswertung/Plots

    def work(self, input_items, output_items):
        cx = input_items[0]
        base = self.nitems_read(0)

        for i in range(len(cx)):
            mag = abs(cx[i])
            # einfache, selbst gebildete Huellkurven-Gate-Logik (kein Fremdzweig,
            # keine Verzoegerungsprobleme) - langsam nachlaufendes Spitzenwertmass
            self._peak = max(mag, self._peak * 0.999)
            g = 1 if mag > 0.35 * self._peak else 0

            if g == 1:
                if self.prev_gate == 1:
                    inst = cx[i] * np.conj(self.prev_sample)
                    finst = np.angle(inst) * self.samp_rate / (2 * np.pi)
                    self._mark_finst.append(finst)
            else:
                if self.prev_gate == 1:
                    n = len(self._mark_finst)
                    g2 = self.guard_samples
                    if n > 2 * g2 + 3:
                        core = self._mark_finst[g2:n - g2]
                        avg_finst = float(np.median(core))
                        self.correction += self.loop_gain * avg_finst
                        new_center = self.pitch + self.correction
                        self.message_port_pub(
                            pmt.intern("freq"),
                            pmt.cons(pmt.intern("freq"), pmt.from_double(new_center)))
                        self.last_correction_log.append((base + i, self.correction))
                self._mark_finst = []

            self.prev_sample = cx[i]
            self.prev_gate = g

        return len(cx)
