{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c3a0b4a",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0, 0, 0, 0, 0, 0, 0, 0, 0]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "[0, 0, 0, 0, 0, 0, 0, 0, 0]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# FIR Filter anlegen\n",
    "\n",
    "from scipy import signal\n",
    "from scipy.fft import fft, fftfreq\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from ipywidgets import interact\n",
    "\n",
    "\n",
    "def fir_filter(taps, input):                    # taps, input sind 1d Eingabelisten mit Koeffizienten und Samples\n",
    "    fir=[]                                      # Ausgabeliste anlegen\n",
    "    for j in range(0, len(input) - len(taps)):  # Erste Samples (Koeffizientenzahl) zählen nicht zur Filterantwort\n",
    "        fir_i=0\n",
    "        for i in range (len(taps)):             # Durch Koeffizienten durchiterieren\n",
    "            taps_i = taps[i]                    # taps_i ist Laufvariable\n",
    "            fir_i += taps_i*input[j+i]          # fir_i ist Laufvariable für Filterergebnis - jeweiliger Koeffizient wird mit dem i-ten Input-Sample der reduzierten Liste j multipliziert\n",
    "        fir.append(fir_i)                       # hänge Ergebnis an Ergebnisliste an\n",
    "    return fir\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f78fe4f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Chirp Generator\n",
    "\n",
    "n=3000 #Sampleanzahl\n",
    "fs=20000 #Samplingrate\n",
    "f0=100 #Startfrequenz\n",
    "f1=1000 #Stopfrequenz\n",
    "t1=n/fs #Chirpdauer (Samples/Samplingrate)\n",
    "\n",
    "t_chrip = np.linspace(0, t1, n) #Array mit Anzahl der Samples anlegen für Zeitachse\n",
    "y_chrip = np.round(signal.chirp(t_chrip, f0=f0, f1=f1, t1=t1, method='linear')*(2**15-1)).astype(int) #Chirp erstellen, auf Ganzzahlen runden, auf 16 Bit Integer skalieren\n",
    "\n",
    "# Erste 4 Samples wegschneiden\n",
    "cutsamps = 45\n",
    "y_chrip = y_chrip[cutsamps:]\n",
    "t_chrip = t_chrip[cutsamps:]\n",
    "\n",
    "# Doppelt so langes Signal mit abwechselnd Original- und invertierten Werten - Struktur für Symmetrie und 2-Kanal-Systeme\n",
    "y_chrip_interleaved = np.empty((2*y_chrip.size), dtype=y_chrip.dtype)pa\n",
    "y_chrip_interleaved[0::2] = y_chrip\n",
    "y_chrip_interleaved[1::2] = -1*y_chrip[::-1]\n",
    "\n",
    "# Chirp in Header-Datei schreiben, welche über PCM eingelesen werden kann\n",
    "file_str= f\"#define CHIRP_DATA_SAMPLE_RATE {int(fs)}\\n\"\\\n",
    "          \"#define CHIRP_DATA_LEN\"f\" {y_chrip.size}\" \"\\n\"\\\n",
    "          \"#define CHIRP_DATA_INTERLEAVED_LEN\"f\" {y_chrip_interleaved.size}\" \"\\n\"\\\n",
    "          \"#define CHIRP_DATA {\" + \",\".join(y_chrip.astype(str)) +\"}\\n\"\\\n",
    "          \"#define CHIRP_DATA_INTERLEAVED_INVERTED {\" + \",\".join(y_chrip_interleaved.astype(str)) +\"}\" \"\\n\"\n",
    "\n",
    "with open(\"pcm_chirp/include/chirp_data.h\", \"w\") as f:\n",
    "    f.write(file_str)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dc1c61df",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ScyPyFIR Filter anlegen"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dea7ae97",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Vergleich und Plot\n",
    "\n",
    "%matplotlib widget\n",
    "\n",
    "b=signal.firwin(20, 100, fs=fs)\n",
    "y_lfiltered = signal.lfilter(b, [1.0], y_chrip)\n",
    "yf=fft(y_lfiltered)\n",
    "xf = fftfreq(n, t1/n)[:n//2]\n",
    "\n",
    "cols = 1\n",
    "rows = 3\n",
    "\n",
    "fig = plt.figure(1)\n",
    "ax1 = fig.add_subplot(rows, cols, 1)\n",
    "line1, = ax1.plot([0], \".-\", label = \"chrip\")\n",
    "ax2 = fig.add_subplot(rows, cols, 2)\n",
    "line2, = ax2.plot([0], label=\"chrip filtered signal.lfilter\")\n",
    "ax3 = fig.add_subplot(rows, cols, 3)\n",
    "line3, = ax3.plot([0], label=\"own fir implementation\")\n",
    "\n",
    "def update(numtaps = 40, f_cut=100):\n",
    "    # Calculate the filter coefficients for given paramters\n",
    "    b=signal.firwin(numtaps, f_cut, fs=fs)\n",
    "    print(f\"Filter coeffs for {numtaps} tabs and {f_cut}Hz cutoff are:\\n\", b)\n",
    "    \n",
    "    bits=16\n",
    "    if min(b)<0:\n",
    "        bits=bits-1\n",
    "    \n",
    "    print(f\"Filter coeffs converted to Q1.{bits}bit int are :\\n\", \n",
    "          \", \".join(\n",
    "                np.array(np.array(np.round(b*(2**(bits)-1)), dtype=np.int32), dtype=str)\n",
    "            )\n",
    "        )\n",
    "\n",
    "    # plot the chirp\n",
    "    line1.set_data(range(len(y_chrip)), y_chrip)\n",
    "    ax1.set_xlim(0, len(y_chrip))\n",
    "    ax1.set_ylim(min(y_chrip), max(y_chrip))\n",
    "\n",
    "    # Apply the coefficents with scipy function\n",
    "    y_lfiltered = signal.lfilter(b, [1.0], y_chrip)\n",
    "    line2.set_data(range(len(y_lfiltered)), y_lfiltered)\n",
    "    ax2.set_xlim(0, len(y_lfiltered))\n",
    "    ax2.set_ylim(min(y_lfiltered), max(y_lfiltered))\n",
    "\n",
    "    # yf=2.0/n*np.abs(fft(y_chrip[:n//2]))\n",
    "    # xf = fftfreq(n, t1/n)[:n//2]\n",
    "    data = simple_fir(b, y_chrip)\n",
    "    line3.set_data(range(len(data)), data)\n",
    "    ax3.set_xlim(0, len(data))\n",
    "    ax3.set_ylim(min(data), max(data))\n",
    "\n",
    "    fig.canvas.draw_idle()\n",
    "    # save coefficients to file\n",
    "    with open(\"pcm_data_processing/include/coefficients.h\", \"w\") as f:\n",
    "        f.write(\n",
    "            \"#define NUMTAPS \" + str(numtaps) + \"\\n\" +\n",
    "            \"#define COEFFICIENTS {\" + \",\".join(np.array(np.array(np.round(b*(2**(bits)-1)), dtype=np.int32), dtype=str)) +\"}\" \"\\n\"\n",
    "        )\n",
    "\n",
    "plt.tight_layout()\n",
    "interact(update, numtaps=(0, 100,2), f_cut=(100,5000))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
