{
  "cells": [
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "\n# Using Phase Shifted Digital Control (draft)\n\nThis example shows the benefit of using the\nphase shifted digital control delay.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "import matplotlib.pyplot as plt\nimport numpy as np\nimport scipy.signal\nimport cbadc"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## The Analog System\n\nIn this example we commit to using a forth order leap-frog analog system,\nsee :py:class:`cbadc.analog_system.LeapFrog`.\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# We fix the number of analog states.\nN = 4\n# Set the amplification factor.\nbeta = 6250.\nrho = - 1e-2\nkappa = - 1.0\n# In this example, each nodes amplification and local feedback will be set\n# identically.\nbetaVec = beta * np.ones(N)\nrhoVec = betaVec * rho\nkappaVec = kappa * beta * np.eye(N)\n\n# Instantiate a chain-of-integrators analog system.\nanalog_system = cbadc.analog_system.LeapFrog(betaVec, rhoVec, kappaVec)\n# print the analog system such that we can very it being correctly initalized.\nprint(analog_system)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## The Digital Control\n\nwe use the delayed version :py:class:`cbadc.digital_control.PhaseDelayedControl`\nas well as the\n:py:class:`cbadc.digital_control.DigitalControl` for comparision.\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# Set the time period which determines how often the digital control updates.\nT = 1.0/(2 * beta)\n\n# Set the number of digital controls to be same as analog states.\nM = N\n# Initialize the digital control. Note that we decrease the control period by\n# M to have the same number of switches per unit-of-time as the reference.\ndigital_control_phase = cbadc.digital_control.PhaseDelayedControl(T / M, M)\ndigital_control_ref = cbadc.digital_control.DigitalControl(T, M)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## The Analog Signal\n\nThe final and third component of the simulation is an analog signal.\nFor this tutorial, we will choose a\n:py:class:`cbadc.analog_signal.Sinusodial`.\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# Set the peak amplitude.\namplitude = 0.5\n# Choose the sinusodial frequency via an oversampling ratio (OSR).\nOSR = 1 << 5\nfrequency = 1.0 / (T * (OSR << 3))\n\n# We also specify a phase an offset these are hovewer optional.\nphase = np.pi / 3\noffset = 0.0\n\n# Instantiate the analog signal\nanalog_signal = cbadc.analog_signal.Sinusodial(\n    amplitude, frequency, phase, offset)\n# print to ensure correct parametrization.\nprint(analog_signal)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Simulating\n\nNext, we set up the simulator. Here we use the\n:py:class:`cbadc.simulator.StateSpaceSimulator` for simulating the\ninvolved differential equations as outlined in\n:py:class:`cbadc.analog_system.AnalogSystem`.\n\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "size = 1 << 17\nend_time = T * (size + 100)\n\n# Instantiate the simulator.\nsimulator_phase = cbadc.simulator.StateSpaceSimulator(analog_system, digital_control_phase, [\n    analog_signal], t_stop=end_time)\nsimulator_ref = cbadc.simulator.StateSpaceSimulator(analog_system, digital_control_ref, [\n    analog_signal], t_stop=end_time / M)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Setting up the Digital Estimation Filters\n\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# Set the bandwidth of the estimator\n\neta2 = 1e4\n\n# Set the batch size\n\nK1_phase = 1 << 13\nK1_ref = K1_phase\n# K1_ref = K1_phase // M\n\n# Instantiate the digital estimator (this is where the filter coefficients are\n# computed).\n\ndigital_estimator_phase = cbadc.digital_estimator.FIRFilter(\n    analog_system, digital_control_phase, eta2, K1_phase, K1_phase, downsample=OSR * M)\ndigital_estimator_ref = cbadc.digital_estimator.FIRFilter(\n    analog_system, digital_control_ref, eta2, K1_ref, K1_ref, downsample=OSR)\n\n# Set control signal iterator\ndigital_estimator_phase(simulator_phase)\ndigital_estimator_ref(simulator_ref)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Post filtering the FIR filter coefficients\n\nYet another approach is to instead post filter\nthe resulting FIR filter digital_estimator.h with another lowpass FIR filter\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "numtaps = 1001\nf_cutoff = 1.0 / OSR\nfir_filter_phase = scipy.signal.firwin(numtaps, f_cutoff / M)\nfir_filter_ref = scipy.signal.firwin(numtaps, f_cutoff)\n\ndigital_estimator_phase.convolve(fir_filter_phase)\ndigital_estimator_ref.convolve(fir_filter_ref)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Simulating and Estimating\n\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "sequence_length = size // OSR // M\n\nu_hat_phase = np.zeros(sequence_length)\nu_hat_ref = np.zeros(sequence_length)\n\nfor index in range(sequence_length):\n    u_hat_phase[index] = next(digital_estimator_phase)\n    u_hat_ref[index] = next(digital_estimator_ref)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Visualize in Time Domain\n\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "t = np.arange(sequence_length)\nplt.plot(t, u_hat_phase)\nplt.plot(t, u_hat_ref)\nplt.xlabel('$t / T$')\nplt.ylabel('$\\hat{u}(t)$')\nplt.title(\"Estimated input signal\")\nplt.grid()\n# plt.xlim((0, T * sequence_length // M // OSR))\nplt.ylim((-0.75, 0.75))\nplt.tight_layout()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Plotting the PSD\n\nAs is typical for delta-sigma modulators, we often visualize the performance\nof the estimate by plotting the power spectral density (PSD).\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "f_phase, psd_phase = cbadc.utilities.compute_power_spectral_density(\n    u_hat_phase[K1_phase // OSR:], fs=1.0/digital_control_phase.T / M)\nf_ref, psd_ref = cbadc.utilities.compute_power_spectral_density(\n    u_hat_ref[K1_ref // OSR:], fs=1.0/digital_control_ref.T)\nplt.figure()\nplt.semilogx(f_phase, 10 * np.log10(psd_phase), label=\"Phase\")\nplt.semilogx(f_ref, 10 * np.log10(psd_ref), label=\"Ref\")\nplt.legend()\n# plt.xlim((1e1, 0.5/digital_control_phase.T))\nplt.xlabel('frequency [Hz]')\nplt.ylabel('$ \\mathrm{V}^2 \\, / \\, \\mathrm{Hz}$')\nplt.grid(which='both')"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Evaluating the Analog State Vector For both controls\n\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# Set sampling time three orders of magnitude smaller than the control period\nTs = T / M / 10.0\n\n# Simulate for 10000 control cycles.\nsize = 15000\nend_time = (size + 100) * Ts\n\n# Initialize a new digital control.\ndigital_control_phase = cbadc.digital_control.PhaseDelayedControl(T / M, M)\ndigital_control_ref = cbadc.digital_control.DigitalControl(T, M)\n\n# With or without input signal?\nanalog_signal = cbadc.analog_signal.Sinusodial(\n    0 * amplitude, frequency, phase, offset)\nanalog_signal = cbadc.analog_signal.Sinusodial(\n    amplitude, frequency, phase, offset)\n\n# Instantiate a new simulator with a sampling time.\nsimulator_phase = cbadc.simulator.extended_simulation_result(cbadc.simulator.StateSpaceSimulator(analog_system, digital_control_phase, [\n                                analog_signal], t_stop=end_time, Ts=Ts))\nsimulator_ref = cbadc.simulator.extended_simulation_result(cbadc.simulator.StateSpaceSimulator(analog_system, digital_control_ref, [\n                                analog_signal], t_stop=end_time, Ts=Ts))\n\n# Create data containers to hold the resulting data.\ntime_vector = np.arange(size) * Ts / T\nstates = np.zeros((2, N, size))\ncontrol_signals = np.zeros((2, M, size), dtype=np.int8)\n\n# Iterate through and store states and control_signals.\nfor index in range(size):\n    res = next(simulator_phase)\n    states[0, :, index] = res['analog_state']    \n    control_signals[0, :, index] = res['control_signal']\n    print(digital_control_phase._t_next, digital_control_phase.control_signal())\n    res = next(simulator_ref)\n    states[1, :, index] = res['analog_state']\n    control_signals[1, :, index] = res['control_signal']\n\n# reset figure size and plot individual results.\nplt.rcParams['figure.figsize'] = [6.40, 6.40 * 2]\nfig, ax = plt.subplots(N, 2)\nfor index in range(N):\n    color1 = next(ax[0, 0]._get_lines.prop_cycler)['color']\n    color2 = next(ax[0, 0]._get_lines.prop_cycler)['color']\n    ax[index, 0].grid(b=True, which='major', color='gray', alpha=0.6, lw=1.5)\n    ax[index, 1].grid(b=True, which='major', color='gray', alpha=0.6, lw=1.5)\n    ax[index, 0].plot(time_vector, states[0, index, :], color=color1, label=\"Phase\")\n    ax[index, 0].plot(time_vector, states[1, index, :], color=color2, label=\"Ref\")\n    ax[index, 1].plot(time_vector, control_signals[0, index, :],\n                    color=color1, label=\"Phase\")\n    ax[index, 1].plot(time_vector, control_signals[1, index, :],\n                    color=color2, label=\"Ref\")\n    ax[index, 0].set_ylabel(f\"$x_{index + 1}(t)$\")\n    ax[index, 1].set_ylabel(f\"$s_{index + 1}(t)$\")\n    ax[index, 0].set_xlim((0, 15))\n    ax[index, 1].set_xlim((0, 15))\n    ax[index, 0].set_ylim((-1, 1))\n    ax[index, 0].legend()\n    ax[index, 1].legend()\nfig.suptitle(\"Analog state and control contribution evolution\")\nax[-1, 0].set_xlabel(\"$t / T$\")\nax[-1, 1].set_xlabel(\"$t / T$\")\nfig.tight_layout()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Analog State Statistics\n\nAs in the previous section, visualizing the analog state trajectory is a\ngood way of identifying problems and possible errors. Another way of making\nsure that the analog states remain bounded is to estimate their\ncorresponding densities (assuming i.i.d samples).\n\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "# Compute L_2 norm of analog state vector.\nL_2_norm = np.linalg.norm(states, ord=2, axis=1)\n# Similarly, compute L_infty (largest absolute value) of the analog state\n# vector.\nL_infty_norm = np.linalg.norm(states, ord=np.inf, axis=1)\n\n# Estimate and plot densities using matplotlib tools.\nbins = 150\nplt.rcParams['figure.figsize'] = [6.40, 4.80]\nfig, ax = plt.subplots(2, sharex=True)\nax[0].grid(b=True, which='major', color='gray', alpha=0.6, lw=1.5)\nax[1].grid(b=True, which='major', color='gray', alpha=0.6, lw=1.5)\nax[0].hist(L_2_norm[0, :], bins=bins, density=True, label=\"Phase\")\nax[0].hist(L_2_norm[1, :], bins=bins, density=True, label=\"Ref\")\nax[1].hist(L_infty_norm[0, :], bins=bins, density=True, color=\"orange\", label=\"Phase\")\nax[1].hist(L_infty_norm[1, :], bins=bins, density=True, color=\"purple\", label=\"Ref\")\nplt.suptitle(\"Estimated probability densities\")\nax[0].set_xlabel(\"$\\|\\mathbf{x}(t)\\|_2$\")\nax[1].set_xlabel(\"$\\|\\mathbf{x}(t)\\|_\\infty$\")\nax[0].set_ylabel(\"$p ( \\| \\mathbf{x}(t) \\|_2 ) $\")\nax[1].set_ylabel(\"$p ( \\| \\mathbf{x}(t) \\|_\\infty )$\")\nax[0].legend()\nax[1].legend()\nfig.tight_layout()"
      ]
    }
  ],
  "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.8.5"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}