{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Notebook for the design of a simple FIR filter for feasiblity testing\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",
    "# Chirp data generate\n",
    "n=3000 # number of samples to use for the chirp\n",
    "fs=20000 # The sampling rate for the chrip\n",
    "f0=100# the start frequency in Hz for the chirp\n",
    "f1=1000 # the stop frequency of the chirp\n",
    "t1=n/fs # the total length of the chirp in s\n",
    "\n",
    "t_chrip = np.linspace(0, t1, n)\n",
    "# generate a chrip and scale to int16 (1 bit for sign)\n",
    "y_chrip = signal.chirp(t_chrip, f0=f0, f1=f1, t1=t1, method='linear')\n",
    "# y_chrip = y_chrip\n",
    "# t_chrip = t_chrip"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0.5, 0, 'Normalized frequency (1.0 = Nyquist)')"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "015c9a01145c459396405bc08f6fb031",
       "version_major": 2,
       "version_minor": 0
      },
      "image/png": "[encoded data, 37320 characters]",
      "text/html": [
       "\n",
       "            <div style=\"display: inline-block;\">\n",
       "                <div class=\"jupyter-widgets widget-label\" style=\"text-align: center;\">\n",
       "                    Figure\n",
       "                </div>\n",
       "                <img src='data:image/png;base64,[encoded data, 37320 characters]' width=640.0/>\n",
       "            </div>\n",
       "        "
      ],
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Plot a iir filter with fixed coefficients in sos form\n",
    "%matplotlib widget\n",
    "\n",
    "sos = np.array([ #[b0,b1,b2,a0,a1,a2]\n",
    "    [1, 0, 0, 1.0, 0.75,0]\n",
    "                ])\n",
    "w, h = signal.sosfreqz(sos, worN=1500)\n",
    "plt.subplot(2, 1, 1)\n",
    "db = 20*np.log10(np.maximum(np.abs(h), 1e-5))\n",
    "plt.plot(w/np.pi, db)\n",
    "plt.ylim(-75, 5)\n",
    "plt.grid(True)\n",
    "plt.yticks([20, 0, -20, -40, -60])\n",
    "plt.ylabel('Gain [dB]')\n",
    "plt.title('Frequency Response')\n",
    "plt.subplot(2, 1, 2)\n",
    "plt.plot(w/np.pi, np.angle(h))\n",
    "plt.grid(True)\n",
    "plt.yticks([-np.pi, -0.5*np.pi, 0, 0.5*np.pi, np.pi],\n",
    "           [r'$-\\pi$', r'$-\\pi/2$', '0', r'$\\pi/2$', r'$\\pi$'])\n",
    "plt.ylabel('Phase [rad]')\n",
    "plt.xlabel('Normalized frequency (1.0 = Nyquist)')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "cfc43d67d8ae4489a8f7831a5fee2c7e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(IntSlider(value=2, description='order'), IntSlider(value=500, description='f_cut', max=5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "<function __main__.update(order=2, f_cut=500, fix_point=False, scale_bits=30)>"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "81cc4ad2fcfa4cb3b9bfdc1c8c6e558b",
       "version_major": 2,
       "version_minor": 0
      },
      "image/png": "[encoded data, 173732 characters]",
      "text/html": [
       "\n",
       "            <div style=\"display: inline-block;\">\n",
       "                <div class=\"jupyter-widgets widget-label\" style=\"text-align: center;\">\n",
       "                    Figure\n",
       "                </div>\n",
       "                <img src='data:image/png;base64,[encoded data, 173732 characters]' width=640.0/>\n",
       "            </div>\n",
       "        "
      ],
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "def iir_filter(b, data, fix_point=False, scale_bits=31):\n",
    "    \"\"\"single iir biquad filter element implementation in plain python\n",
    "\n",
    "    Args:\n",
    "        b (list): The filter coefficients in second order notation [b0,b1,b2,a0,a1,a2] a0 has to be normalized to 1.\n",
    "        data (list): The data to filter\n",
    "        fix_point (bool, optional): If checked, fixed point implementation is used. Defaults to False.\n",
    "        scale_bits (int, optional): The bits to use for the scaling. Defaults to 31. Use e.g. 30 if there is a coefficient > 1\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "    # // file : iirdirect.c\n",
    "    # // Low pass filter:\n",
    "    # // Sample frequency (Hz) : 44000\n",
    "    # // Cut off frequency (Hz) : 5000\n",
    "    # // Damping factor : 1.5\n",
    "    # const double a = 0.0409501; // m = 2, s = 1\n",
    "    # const double b = 0.170625;\n",
    "    # const double g = 0.506825;\n",
    "    # const int C[5] = {\n",
    "    # as_int(a), as_int(2*a), as_int(a), as_int(g), as_int(-b)\n",
    "    # };\n",
    "    # int xd[2];\n",
    "    # int yd[2];\n",
    "    # int low_pass(int x)\n",
    "    # {\n",
    "    # accum_t sum = fract_mult(x, C[0])\n",
    "    # + fract_mult(xd[0],C[1]) + fract_mult(xd[1],C[2])\n",
    "    # + fract_mult(yd[0],C[3]) + fract_mult(yd[1],C[4]);\n",
    "    # int y = rnd_saturate(sum << 1);\n",
    "    # xd[1] = xd[0];\n",
    "    # xd[0] = x;\n",
    "    # yd[1] = yd[0];\n",
    "    # yd[0] = y;\n",
    "    # return y;\n",
    "\n",
    "    assert b[3] == 1, \"a0 has to be normalized to 1\"    \n",
    "    scale = 2**scale_bits - 1\n",
    "    y=[]\n",
    "    if fix_point:\n",
    "        C = (b*scale).astype(int).tolist() #* scale).astype(int).tolist()\n",
    "        # xd = np.zeros(2, dtype=int)\n",
    "        # yd = np.zeros(2, dtype=int)\n",
    "        xd =[0]*2 \n",
    "        yd = [0]*2\n",
    "    else:\n",
    "        C = b.tolist() #* scale).astype(int).tolist()\n",
    "        xd =[0]*2 \n",
    "        yd = [0]*2\n",
    "    del C[3] # remove a0\n",
    "\n",
    "\n",
    "    y_i=0\n",
    "    for j in range(0, len(data)):\n",
    "        if fix_point:\n",
    "            x = int(scale*data[j])\n",
    "        else:\n",
    "            x = data[j]\n",
    "\n",
    "        sum = x * C[0] + xd[0]*C[1] + xd[1]*C[2] - yd[0]*C[3] - yd[1]*C[4]\n",
    "        \n",
    "        if fix_point:\n",
    "            y_i = sum >> scale_bits\n",
    "        else:\n",
    "            y_i = sum\n",
    "\n",
    "        xd[1] = xd[0]\n",
    "        xd[0] = x\n",
    "        yd[1] = yd[0]\n",
    "        yd[0] = y_i\n",
    "        y.append(y_i)\n",
    "    return y\n",
    "\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 iir implementation\")\n",
    "\n",
    "def update(order = 2, f_cut=500, fix_point=False, scale_bits=30):\n",
    "    # Calculate the filter coefficients for given paramters\n",
    "    b=signal.iirfilter(order, f_cut, btype='lowpass', ftype='butter', output=\"sos\", fs=fs) #returns [b0,b1,b2,a0,a1,a2]\n",
    "    print(f\"Filter coeffs for order {order} and {f_cut}Hz cutoff are:\\n\", b)\n",
    "    \n",
    "    # bits=16\n",
    "    # if min(b)<0:\n",
    "    #     bits=bits-1\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.sosfilt(b, 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",
    "    # Apply the coefficents with own implementation\n",
    "    data = iir_filter(b[0], y_chrip, fix_point=fix_point, scale_bits=scale_bits)\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/iir_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, order=(0, 100, 1), f_cut=(100,5000), fix_point=True, scale_bits=(1, 31, 1))\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "jupyter",
   "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.10.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
