#!/usr/bin/env python3
"""Расчёт ФНЧ-дециматора /5 и генерация src/fir_coefs_pkg.vhd.

Фильтр стоит после CIC sinc^4 /50 и решает две задачи сразу:
  - давит всё выше 6250 Гц, куда после децимации на 5 сворачивается
    шум сигма-дельта модулятора микрофона (CIC там ничего не подавляет);
  - выравнивает спад CIC в полосе (задаётся желаемая АЧХ 1/H_cic).

Масштаб выбран так, чтобы постоянная '1' на входе PDM давала полную шкалу
24 бит: коэффициенты домножены на 2^23 / 50^4 и сумма подогнана точно.

Запуск: py -3 tools/gen_fir_coefs.py
"""

import os
import sys

import numpy as np
from scipy.signal import firls, freqz

# --- параметры тракта, должны совпадать с Top.vhd ---
F_PDM = 3_125_000.0          # PDM_CLK = 50 МГц / 16
R_CIC = 50                   # децимация CIC
N_CIC = 4                    # порядок CIC
FS_IN = F_PDM / R_CIC        # 62500 Гц на входе FIR
DECIM = 5                    # децимация FIR
FS_OUT = FS_IN / DECIM       # 12500 Гц

# --- параметры фильтра ---
NTAPS = 221                  # нечётное: линейная фаза, симметрия
COEF_W = 18                  # разрядность коэффициента
SHIFT = 17                   # выход = (сумма произведений) >> SHIFT
PASS = 5000.0                # граница полосы пропускания
STOP = 6250.0                # граница полосы задерживания = FS_OUT/2
WSTOP = 300.0                # вес полосы задерживания
NSUB = 8                     # на сколько участков бьём полосу для компенсации

CIC_DC = R_CIC ** N_CIC      # коэффициент передачи CIC на постоянном токе
FS_CODE = 2 ** 23 - 1        # полная шкала выхода

PKG_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)),
                        "..", "src", "fir_coefs_pkg.vhd")


def cic_mag(f):
    """АЧХ CIC sinc^N по частоте входного сигнала."""
    f = np.atleast_1d(np.asarray(f, dtype=float))
    num = np.sin(np.pi * f * R_CIC / F_PDM)
    den = R_CIC * np.sin(np.pi * f / F_PDM)
    out = np.ones_like(f)
    nz = f != 0
    out[nz] = np.abs(num[nz] / den[nz])
    return out ** N_CIC


def design():
    """Желаемая АЧХ в полосе -- 1/H_cic, так спад CIC уходит в коэффициенты."""
    edges = np.linspace(0.0, PASS, NSUB + 1)
    bands, desired, weight = [], [], []
    for i in range(NSUB):
        bands += [edges[i], edges[i + 1]]
        desired += [1.0 / cic_mag(edges[i])[0], 1.0 / cic_mag(edges[i + 1])[0]]
        weight.append(1.0)
    bands += [STOP, FS_IN / 2]
    desired += [0.0, 0.0]
    weight.append(WSTOP)
    return firls(NTAPS, bands, desired, weight=weight, fs=FS_IN)


def quantize(h):
    """Целые коэффициенты. Сумму подгоняем центральным отводом: он сам себе
    зеркало, симметрия не рушится, а передача на постоянном токе становится
    ровно такой, какую проверяет тестбенч."""
    # Наибольшая сумма, при которой полная шкала PDM ещё не даёт переполнения
    sum_target = ((FS_CODE * 2 ** SHIFT) - 2 ** (SHIFT - 1)) // CIC_DC
    scale = sum_target / h.sum()
    c = np.round(h * scale).astype(np.int64)
    c[NTAPS // 2] += sum_target - c.sum()
    return c, sum_target


def check(c):
    """Проверка уже квантованных коэффициентов: неравномерность считаем
    по сквозной АЧХ CIC+FIR, подавление -- по одному FIR."""
    lo = -2 ** (COEF_W - 1)
    hi = 2 ** (COEF_W - 1) - 1
    assert c.min() >= lo and c.max() <= hi, f"не влезает в {COEF_W} бит: {c.min()}..{c.max()}"
    assert np.array_equal(c, c[::-1]), "симметрия нарушена"

    h = c / 2 ** SHIFT / (FS_CODE / CIC_DC)   # обратно к единичному усилению
    w, H = freqz(h, worN=1 << 16, fs=FS_IN)
    mag = np.abs(H)
    total = mag * cic_mag(w)
    pb, sb = w <= PASS, w >= STOP
    ripple = 20 * np.log10(total[pb].max() / total[pb].min())
    att = 20 * np.log10(mag[sb].max())

    # Худшее подавление среди полос, сворачивающихся в 0..PASS
    alias = -999.0
    for k in range(1, DECIM):
        band = (w >= k * FS_OUT - PASS) & (w <= k * FS_OUT + PASS)
        if band.any():
            alias = max(alias, 20 * np.log10(mag[band].max()))
    return ripple, att, alias, w, total


def emit(c, sum_c, dc_code, ripple, att, alias):
    lines = []
    a = lines.append
    a("-" * 82)
    a("-- Module Name:    fir_coefs_pkg")
    a("-- Project Name:   DigMic_prj")
    a("-- Target Devices: Gowin GW5A-25")
    a("-- Description:")
    a("--   Коэффициенты ФНЧ-дециматора /5, стоящего после CIC.")
    a("--   Сгенерировано tools/gen_fir_coefs.py, руками не править.")
    a("--")
    a(f"--   вход {FS_IN:.0f} Гц -> выход {FS_OUT:.0f} Гц, отводов {NTAPS}, симметричный")
    a(f"--   полоса 0..{PASS:.0f} Гц, задерживание от {STOP:.0f} Гц")
    a(f"--   неравномерность полосы с учётом CIC: {ripple:.3f} дБ")
    a(f"--   подавление в полосе задерживания:    {att:.1f} дБ")
    a(f"--   худшее подавление сворачиваемых полос: {alias:.1f} дБ")
    a("-" * 82)
    a("library ieee;")
    a("use ieee.std_logic_1164.all;")
    a("use ieee.numeric_std.all;")
    a("")
    a("package fir_coefs_pkg is")
    a("")
    a(f"  constant FIR_NTAPS  : integer := {NTAPS};")
    a(f"  constant FIR_COEF_W : integer := {COEF_W};")
    a(f"  constant FIR_SHIFT  : integer := {SHIFT};")
    a("")
    a("  -- Отклик тракта на постоянную '1' на выводе данных микрофона:")
    a("  -- сумма коэффициентов x усиление CIC, округлённая как в fir_decim.")
    a(f"  constant FIR_SUM    : integer := {sum_c};")
    a(f"  constant FIR_DC_CODE : integer := {dc_code};")
    a("")
    a("  type fir_coefs_t is array (0 to FIR_NTAPS - 1) of signed(FIR_COEF_W - 1 downto 0);")
    a("")
    a("  constant FIR_COEFS : fir_coefs_t := (")
    per_line = 4
    for i in range(0, NTAPS, per_line):
        chunk = c[i:i + per_line]
        body = ", ".join(f'"{v & (2 ** COEF_W - 1):0{COEF_W}b}"' for v in chunk)
        tail = "," if i + per_line < NTAPS else ""
        lines.append(f"    {body}{tail}  -- {i}")
    a("  );")
    a("")
    a("end package fir_coefs_pkg;")

    with open(PKG_PATH, "w", encoding="utf-8", newline="\n") as f:
        f.write("\n".join(lines) + "\n")


def main():
    h = design()
    c, sum_target = quantize(h)
    ripple, att, alias, w, total = check(c)

    dc_code = (CIC_DC * int(c.sum()) + 2 ** (SHIFT - 1)) >> SHIFT
    assert dc_code <= FS_CODE, dc_code
    assert int(c.sum()) == sum_target

    emit(c, int(c.sum()), dc_code, ripple, att, alias)

    print(f"отводов                : {NTAPS}, {COEF_W} бит, сдвиг {SHIFT}")
    print(f"диапазон коэффициентов : {c.min()} .. {c.max()}")
    print(f"сумма коэффициентов    : {c.sum()}")
    print(f"полная шкала PDM -> код: {dc_code} из {FS_CODE}")
    print(f"неравномерность 0..{PASS:.0f} Гц (CIC+FIR): {ripple:.3f} дБ")
    print(f"подавление от {STOP:.0f} Гц   : {att:.1f} дБ")
    print(f"подавление сворачиваемых полос: {alias:.1f} дБ")
    for f0 in (100, 1000, 3000, 5000, 6250):
        i = np.argmin(np.abs(w - f0))
        print(f"  АЧХ на {f0:5d} Гц      : {20 * np.log10(total[i]):+7.3f} дБ")
    print(f"записано               : {os.path.normpath(PKG_PATH)}")
    return 0


if __name__ == "__main__":
    sys.exit(main())