{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7c270395",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy import signal\n",
    "import matplotlib.pyplot as plt\n",
    "import pandas as pd\n",
    "from scipy.fft import fft, fftfreq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b09956cf",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "348b4663",
   "metadata": {},
   "source": [
    "## Filter comparison"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eef8fc32",
   "metadata": {},
   "outputs": [],
   "source": [
    "alpha = 0.01\n",
    "mavg_b = [alpha]\n",
    "mavg_a = [1, -(1-alpha)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06cc8d91",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Sinc filter\n",
    "b_sinc = [\n",
    "    0.013166773445594984,\n",
    "    0.015510026576574206,\n",
    "    0.017943762856303655,\n",
    "    0.020436419999452039,\n",
    "    0.022953654956480787,\n",
    "    0.025459023223972144,\n",
    "    0.027914730737546672,\n",
    "    0.030282439536854874,\n",
    "    0.032524106530629059,\n",
    "    0.034602833419516990,\n",
    "    0.036483705201686277,\n",
    "    0.038134594717421900,\n",
    "    0.039526911389630152,\n",
    "    0.040636273671486346,\n",
    "    0.041443086683647982,\n",
    "    0.041933009054862178,\n",
    "    0.042097295996679322,\n",
    "    0.041933009054862178,\n",
    "    0.041443086683647982,\n",
    "    0.040636273671486346,\n",
    "    0.039526911389630152,\n",
    "    0.038134594717421900,\n",
    "    0.036483705201686277,\n",
    "    0.034602833419516990,\n",
    "    0.032524106530629059,\n",
    "    0.030282439536854874,\n",
    "    0.027914730737546672,\n",
    "    0.025459023223972144,\n",
    "    0.022953654956480787,\n",
    "    0.020436419999452039,\n",
    "    0.017943762856303655,\n",
    "    0.015510026576574206,\n",
    "    0.013166773445594984,\n",
    "]\n",
    "b_sinc = [\n",
    "    -0.000005301919181359,\n",
    "    -0.000020372384569462,\n",
    "    -0.000043393375246200,\n",
    "    -0.000071643508460196,\n",
    "    -0.000101272447699964,\n",
    "    -0.000127045709202358,\n",
    "    -0.000142084479366378,\n",
    "    -0.000137630058679112,\n",
    "    -0.000102865422361326,\n",
    "    -0.000024826753571648,\n",
    "    0.000111564545505229,\n",
    "    0.000323323938002668,\n",
    "    0.000629062120588115,\n",
    "    0.001048471626315150,\n",
    "    0.001601644955843253,\n",
    "    0.002308239744151386,\n",
    "    0.003186518744650963,\n",
    "    0.004252303894058680,\n",
    "    0.005517893567100900,\n",
    "    0.006990999568786174,\n",
    "    0.008673764799041549,\n",
    "    0.010561923390649710,\n",
    "    0.012644162210467481,\n",
    "    0.014901735907231652,\n",
    "    0.017308377414832196,\n",
    "    0.019830532444994709,\n",
    "    0.022427930710902887,\n",
    "    0.025054489273884574,\n",
    "    0.027659525485854094,\n",
    "    0.030189239563895277,\n",
    "    0.032588410933658420,\n",
    "    0.034802239101869442,\n",
    "    0.036778249821351520,\n",
    "    0.038468181363671021,\n",
    "    0.039829764251821352,\n",
    "    0.040828311002284692,\n",
    "    0.041438040179150003,\n",
    "    0.041643070995549647,\n",
    "    0.041438040179150003,\n",
    "    0.040828311002284692,\n",
    "    0.039829764251821366,\n",
    "    0.038468181363671021,\n",
    "    0.036778249821351520,\n",
    "    0.034802239101869456,\n",
    "    0.032588410933658420,\n",
    "    0.030189239563895291,\n",
    "    0.027659525485854094,\n",
    "    0.025054489273884584,\n",
    "    0.022427930710902898,\n",
    "    0.019830532444994709,\n",
    "    0.017308377414832207,\n",
    "    0.014901735907231647,\n",
    "    0.012644162210467488,\n",
    "    0.010561923390649715,\n",
    "    0.008673764799041547,\n",
    "    0.006990999568786178,\n",
    "    0.005517893567100902,\n",
    "    0.004252303894058680,\n",
    "    0.003186518744650965,\n",
    "    0.002308239744151388,\n",
    "    0.001601644955843254,\n",
    "    0.001048471626315152,\n",
    "    0.000629062120588114,\n",
    "    0.000323323938002668,\n",
    "    0.000111564545505229,\n",
    "    -0.000024826753571648,\n",
    "    -0.000102865422361327,\n",
    "    -0.000137630058679112,\n",
    "    -0.000142084479366378,\n",
    "    -0.000127045709202359,\n",
    "    -0.000101272447699963,\n",
    "    -0.000071643508460196,\n",
    "    -0.000043393375246200,\n",
    "    -0.000020372384569461,\n",
    "    -0.000005301919181359,\n",
    "]\n",
    "b_sinc = np.around([k * 56536 for k in b_sinc])/65536\n",
    "\n",
    "\n",
    "def combined_sinc_mavg(alpha = 0.4):\n",
    "    b1 = [alpha]\n",
    "    b2 = b_sinc\n",
    "    a1 = [1, -(1-alpha)]\n",
    "    a2 = [1]\n",
    "    \n",
    "    b = np.convolve(b1, b2)\n",
    "    a = np.convolve(a1, a2)\n",
    "    return b,a\n",
    "\n",
    "def iir_notch(freq, r, fsa):\n",
    "    b = [1, -2*np.cos(freq/fsa*2*np.pi), 1]\n",
    "    a = [1, -2*r*np.cos(freq/fsa*2*np.pi), r*r]\n",
    "    return b,a"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "299fc59e",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0af3fa7b",
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_transfer_func(b, a, fsa = 1):\n",
    "    omega, vals = signal.freqz(b, a, worN = 1024)\n",
    "    plt.plot(omega/(2*np.pi)*fsa, abs(vals))\n",
    "    \n",
    "sample_rate = 1e3/6\n",
    "plt.figure(figsize=(16,8))\n",
    "plt.yscale('log')\n",
    "plot_transfer_func(mavg_b, mavg_a, sample_rate)\n",
    "plt.ylabel('|H(w)| [dB]')\n",
    "plt.xlabel('Frequency [Hz]')\n",
    "\n",
    "plot_transfer_func(b_sinc, [1], sample_rate)\n",
    "\n",
    "# Combined filter\n",
    "b,a = combined_sinc_mavg(alpha = 1)\n",
    "b_notch, a_notch = iir_notch(50, 0.875, sample_rate)\n",
    "b = np.convolve(b, b_notch)\n",
    "a = np.convolve(a, a_notch)\n",
    "plot_transfer_func(b,a, sample_rate)\n",
    "plt.grid()\n",
    "\n",
    "plt.legend(['MAVG a = 0.01', 'SINC', 'SINC+MAVG'])\n",
    "plt.xlim(0, 70)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9b87b82",
   "metadata": {},
   "source": [
    "# Notch filters (Not used for implementation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c360b127",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(16,8))\n",
    "plt.yscale('log')\n",
    "a = [1]\n",
    "b = [1, -2*np.cos(50/sample_rate*2*np.pi), 1]\n",
    "plot_transfer_func(b,a, sample_rate)\n",
    "\n",
    "b = [1, -2*np.cos(60/sample_rate*2*np.pi), 1]\n",
    "plot_transfer_func(b,a, sample_rate)\n",
    "\n",
    "r = 0.875\n",
    "b,a = iir_notch(50, r, sample_rate)\n",
    "plot_transfer_func(b,a, sample_rate)\n",
    "\n",
    "b,a = iir_notch(60, r, sample_rate)\n",
    "plot_transfer_func(b,a, sample_rate)\n",
    "plt.xlim(40,70)\n",
    "plt.legend(['FIR 50 Hz Notch', 'FIR 60 Hz', 'IIR 50Hz', 'IIR 60Hz'])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "891bf909",
   "metadata": {},
   "source": [
    "# Singal testing"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29277520",
   "metadata": {},
   "outputs": [],
   "source": [
    "N = 30000\n",
    "time = np.linspace(0, 1/sample_rate*N, N, endpoint=False)\n",
    "sig = 10*signal.sawtooth(2*np.pi*0.2*time)+10 #+ 0.5 * np.sin(2*np.pi*50*time)+0.8 * np.sin(2*np.pi*80*time)\n",
    "\n",
    "plt.figure(figsize=(16,8))\n",
    "\n",
    "spur_len = 3\n",
    "\n",
    "sig[1000:1000+spur_len] = sig[1000]+5\n",
    "sig[2000:2000+spur_len] = sig[2000]-6\n",
    "sig[3000:3000+spur_len] = sig[3000]+2\n",
    "sig[4000:4000+spur_len] = sig[4000]+8\n",
    "\n",
    "\n",
    "plt.plot(time, sig)\n",
    "\n",
    "\n",
    "# Apply the combined filter:\n",
    "b,a = combined_sinc_mavg(alpha = 1)\n",
    "\n",
    "bn,an = iir_notch(50, 0.875, sample_rate)\n",
    "a = np.convolve(a, an)\n",
    "b = np.convolve(b, bn)\n",
    "\n",
    "#b,a = mavg_b, mavg_a\n",
    "\n",
    "w,h = signal.freqz(b,a)\n",
    "gain_corr = round(1/abs(h[0])*65536)/65536\n",
    "\n",
    "sig_f = signal.lfilter(b, a, sig) * gain_corr\n",
    "\n",
    "\n",
    "print('Gain correction:', gain_corr)\n",
    "\n",
    "plt.plot(time, sig_f)\n",
    "plt.xlim(5,10)\n",
    "plt.show()\n",
    "\n",
    "plt.figure(figsize=(16,8))\n",
    "plt.plot(time, abs(sig-sig_f))\n",
    "plt.xlim(5,10)\n",
    "plt.ylim(0,2)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13758640",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "window = signal.windows.hamming(N)\n",
    "\n",
    "h_sig = fft(sig*window)\n",
    "f_sig = fftfreq(N, 1/sample_rate)[:N//2]\n",
    "\n",
    "h_sig_f = fft(sig_f*window)\n",
    "f_sig_f = fftfreq(N, 1/sample_rate)[:N//2]\n",
    "\n",
    "plt.figure(figsize=(16,10))\n",
    "plt.yscale('log')\n",
    "plt.plot(f_sig, 2.0/N*abs(h_sig[0:N//2]))\n",
    "plt.plot(f_sig_f, 2.0/N*abs(h_sig_f[0:N//2]))\n",
    "plt.show();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f4b81600",
   "metadata": {},
   "source": [
    "# PT1000 HF Filtering"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a87370ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "raw_data = pd.read_csv(r'pt1000_hf_2kOhm_v1.3.dat')\n",
    "raw_data = pd.read_csv(r'pt1000_hf_changing.dat')\n",
    "time = np.linspace(0, 2000*6e-3, 2000, endpoint=False)\n",
    "\n",
    "plt.figure(figsize=(22,12))\n",
    "plt.plot(time, raw_data['hf_value']*2500/4096)\n",
    "\n",
    "alpha = 0.005\n",
    "\n",
    "mavg_b = [alpha]\n",
    "mavg_a = [1, -(1-alpha)]\n",
    "\n",
    "zi = signal.lfilter_zi(mavg_b, mavg_a)\n",
    "filtered, _ = signal.lfilter(mavg_b, mavg_a, raw_data['hf_value'], zi=zi*raw_data['hf_value'][0])\n",
    "plt.plot(time, filtered*2500/4096)\n",
    "filtered_avg_low = filtered\n",
    "\n",
    "alpha = 0.01\n",
    "mavg_b = [alpha]\n",
    "mavg_a = [1, -(1-alpha)]\n",
    "\n",
    "zi = signal.lfilter_zi(mavg_b, mavg_a)\n",
    "filtered, _ = signal.lfilter(mavg_b, mavg_a, raw_data['hf_value'], zi=zi*raw_data['hf_value'][0])\n",
    "plt.plot(time, filtered*2500/4096)\n",
    "filtered_avg = filtered\n",
    "\n",
    "\n",
    "\n",
    "# Apply the combined filter:\n",
    "b,a = combined_sinc_mavg(alpha = 0.08)\n",
    "\n",
    "bn,an = iir_notch(4, 0.75, sample_rate)\n",
    "a = np.convolve(a, an)\n",
    "b = np.convolve(b, bn)\n",
    "\n",
    "zi = signal.lfilter_zi(b, a)\n",
    "filtered, _ = signal.lfilter(b, a, raw_data['hf_value'], zi=zi*raw_data['hf_value'][0])\n",
    "\n",
    "w,h = signal.freqz(b,a)\n",
    "gain_corr = round(1/abs(h[0])*65536)/65536\n",
    "\n",
    "plt.plot(time, filtered*gain_corr*2500/4096)\n",
    "plt.savefig('expl.pdf', format='pdf', dpi=1200)\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3eae0e71",
   "metadata": {},
   "source": [
    "## Spectrum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8d082b99",
   "metadata": {},
   "outputs": [],
   "source": [
    "window =signal.windows.hamming(len(raw_data['hf_value']))\n",
    "raw_fft = fft(np.array(raw_data['hf_value'].to_list())*window)\n",
    "f_raw = fftfreq(len(raw_data), 1/sample_rate)[:len(raw_data)//2]\n",
    "plt.figure(figsize=(16,10));\n",
    "plt.yscale('log')\n",
    "plt.plot(f_raw, abs(raw_fft[:len(raw_data)//2]))\n",
    "\n",
    "avg_fft = fft(np.array(filtered_avg)*window)\n",
    "plt.plot(f_raw, abs(avg_fft[:len(raw_data)//2]))\n",
    "\n",
    "avg_fft = fft(np.array(filtered_avg_low)*window)\n",
    "plt.plot(f_raw, abs(avg_fft[:len(raw_data)//2]))\n",
    "\n",
    "sinc_fft = fft(np.array(filtered)*window)\n",
    "plt.plot(f_raw, abs(sinc_fft[:len(raw_data)//2]))\n",
    "plt.xscale('log')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e09d63f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(np.std(filtered_avg_low/4096*2500))\n",
    "print(np.std(filtered_avg/4096*2500))\n",
    "print(np.std(filtered/4096*2500))\n",
    "\n",
    "print('Min',min(filtered_avg/4096*2500), 'Max', max(filtered_avg)/4096*2500)\n",
    "print('Min',min(filtered)/4096*2500*gain_corr, 'Max', max(filtered)/4096*2500*gain_corr)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}