{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Cruise Control Example"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, import necessary libraries:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "init_printing()\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy import linalg\n",
    "import numpy as np\n",
    "from numpy import linalg as LA\n",
    "from matplotlib.backends.backend_pdf import PdfPages\n",
    "from collections import deque"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following function allows us to ensure the correctness of symbolic manipulations. This is helpful to obtain the analytical solution of the cruise control example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def equals(a,b):\n",
    "    assert np.isclose(float(simplify(a - b)), 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following defines the symbolic equations for the example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\frac{- d v + k \\left(- v + vd\\right)}{m}$"
      ],
      "text/plain": [
       "-d⋅v + k⋅(-v + vd)\n",
       "──────────────────\n",
       "        m         "
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t, v, m, k, vd, d, v0 = symbols('t, v m k vd d v0')\n",
    "dv_dt = (1/m)*(k*(vd-v) - d*v)\n",
    "dv_dt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Rewrite the previous equation into the form $av + b$, so that we can define the analytical solution."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\frac{k vd}{m} - \\frac{v \\left(d + k\\right)}{m}$"
      ],
      "text/plain": [
       "k⋅vd   v⋅(d + k)\n",
       "──── - ─────────\n",
       " m         m    "
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a = -(1/m)*(k+d)\n",
    "b = (1/m)*(k*vd)\n",
    "dv_dt_norm = (a*v + b)\n",
    "equals(dv_dt, dv_dt_norm) # Proves correct rewrite.\n",
    "dv_dt_norm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define analytical solution, and prove that it is indeed a solution by differentiating it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 4852 characters]\n",
      "text/latex": [
       "$\\displaystyle \\frac{k vd}{d + k} + \\left(- \\frac{k vd}{d + k} + v_{0}\\right) e^{- \\frac{t \\left(d + k\\right)}{m}}$"
      ],
      "text/plain": [
       "                        -t⋅(d + k) \n",
       "                        ───────────\n",
       " k⋅vd   ⎛   k⋅vd     ⎞       m     \n",
       "───── + ⎜- ───── + v₀⎟⋅ℯ           \n",
       "d + k   ⎝  d + k     ⎠             "
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v_t = exp(a*t)*(v0 + b/a) - b/a\n",
    "equals(dv_dt.subs(v, v_t), diff(v_t, t))\n",
    "equals(v_t.subs(t, 0), v0)\n",
    "v_t"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following represents the explicit euler method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Explicit_Euler:\n",
    "    def simulate(self, f, x0, t0, h, max_steps):\n",
    "        x = x0\n",
    "        xs = [x]\n",
    "        for i in range(1, max_steps):\n",
    "            x = x + f(x)*h\n",
    "            xs.append(x)\n",
    "        return xs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following appplies the previous numerical solver to the cruise control example. \n",
    "The results are plotting against the analytical solution `v_t` obtained earlier."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 15596 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Instantiate the symbolic equations to the example.\n",
    "v0_num = 0.0\n",
    "v_t_example = v_t.subs(m,1576.0).subs(vd,40).subs(v0,v0_num).subs(k,1000).subs(d,0.5)\n",
    "dv_dt_example = dv_dt.subs(m,1576.0).subs(vd,40).subs(v0,v0_num).subs(k,1000).subs(d,0.5)\n",
    "\n",
    "# Define simulation step size, time range, and total number of steps to be taken.\n",
    "h=0.2\n",
    "t_range = np.arange(0,7,h)\n",
    "num_steps = len(t_range)\n",
    "\n",
    "# Compute analytical trajectory\n",
    "xs_c = [v_t_example.subs(t, x) for x in t_range]\n",
    "\n",
    "# Compute approximate trajectories\n",
    "xs_ee = Explicit_Euler().simulate(lambda v_val: dv_dt_example.subs(v,v_val), \n",
    "                                      v0_num, 0.0, h, num_steps)\n",
    "\n",
    "plt.figure(1)\n",
    "plt.plot(t_range,xs_c, label='v')\n",
    "plt.plot(t_range,xs_ee, 'o', markersize=3, label='v_exEuler')\n",
    "plt.xlabel('t (s)')\n",
    "plt.ylabel('speed (m/s)')\n",
    "\n",
    "plt.legend()\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise: Implement the Midpoint Method"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The midpoint method approximates the solution of a given IVP in the form of\n",
    "$$ \\dot{x} = f(x) $$\n",
    "by the following:\n",
    "$$ \\mathit{aux} = x(t) + f(x(t))\\frac{h}{2}$$\n",
    "$$ x(t+h) = x(t) + f(aux)h$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Midpoint:\n",
    "    def simulate(self, f, x0, t0, h, max_steps):\n",
    "        x = x0\n",
    "        xs = [x]\n",
    "        for i in range(1, max_steps):\n",
    "            # TODO: implement the following \n",
    "            x = ...\n",
    "            xs.append(x)\n",
    "        return xs\n",
    "\n",
    "# Run solver and plot\n",
    "xs_mid = Midpoint().simulate(lambda v_val: dv_dt_example.subs(v,v_val), \n",
    "                                v0_num, 0.0, h, num_steps)\n",
    "plt.figure(1)\n",
    "plt.plot(t_range, xs_c, label='v')\n",
    "plt.plot(t_range, xs_ee, 'o', markersize=3, label='v_exEuler')\n",
    "plt.plot(t_range, xs_mid, 'o', markersize=3, label='v_mid')\n",
    "plt.xlabel('t (s)')\n",
    "plt.ylabel('speed (m/s)')\n",
    "\n",
    "plt.legend()\n",
    "\n",
    "plt.show()"
   ]
  }
 ],
 "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.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
