{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, import necessary libraries and configure them:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%matplotlib notebook\n",
    "\n",
    "from sympy import *\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy import linalg\n",
    "import numpy as np\n",
    "from matplotlib.backends.backend_pdf import PdfPages\n",
    "from collections import deque\n",
    "\n",
    "init_printing()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define the Mass-spring-damper symbolic equations and print $\\dot{v_1}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "m1, c1, x1, d1, v1, Fc = symbols('m1 c1 x1 d1 v1 Fc', real=True)\n",
    "dx_dt = v1\n",
    "dv_dt = (1/m1)*(-c1*x1 - d1*v1 + Fc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABEAAAAMCAYAAACEJVa/AAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA9ElEQVQoFZWS4Q2CQAyFOcMAJG6gG5iwgW6AK+gGOoMj4AYGN3AEEjeAEcQN8HuEngfiD15S2r62D3pH1LZtBBLshOXy4szIK+xg+ZTvmmm6qAi2WBM2kkukGHEbuIdxsXNuBVFiwh57ddH3cSZMldKr4aNioDmPxBRhGmy8jr4usx55IK4yLtxdBXV40X5gIDolsmDIsCOoaXob0fvlKP9JQxHt+Aw7OIOM/BZyU3EoUtMgoQ4I6NpTvmwg3JcHzvU72mBBIDFdq87rKj8GL9D55dTXqsVhA6SueDbCdeYMa1WPwTqe/ROwhs5MP5/WUXzHyg/ZX7BOp2+vswAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle v_{1}$"
      ],
      "text/plain": [
       "v₁"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dx_dt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\frac{Fc - c_{1} x_{1} - d_{1} v_{1}}{m_{1}}$"
      ],
      "text/plain": [
       "Fc - c₁⋅x₁ - d₁⋅v₁\n",
       "──────────────────\n",
       "        m₁        "
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dv_dt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Replace symbols by their values:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "dx_dt_example = dx_dt\n",
    "dv_dt_example = dv_dt.subs(m1,1.0).subs(c1,1.0).subs(d1,1.0).subs(Fc,0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABEAAAAMCAYAAACEJVa/AAAACXBIWXMAAA7EAAAOxAGVKw4bAAAA9ElEQVQoFZWS4Q2CQAyFOcMAJG6gG5iwgW6AK+gGOoMj4AYGN3AEEjeAEcQN8HuEngfiD15S2r62D3pH1LZtBBLshOXy4szIK+xg+ZTvmmm6qAi2WBM2kkukGHEbuIdxsXNuBVFiwh57ddH3cSZMldKr4aNioDmPxBRhGmy8jr4usx55IK4yLtxdBXV40X5gIDolsmDIsCOoaXob0fvlKP9JQxHt+Aw7OIOM/BZyU3EoUtMgoQ4I6NpTvmwg3JcHzvU72mBBIDFdq87rKj8GL9D55dTXqsVhA6SueDbCdeYMa1WPwTqe/ROwhs5MP5/WUXzHyg/ZX7BOp2+vswAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle v_{1}$"
      ],
      "text/plain": [
       "v₁"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dx_dt_example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAIMAAAARCAYAAAD+Mx8uAAAACXBIWXMAAA7EAAAOxAGVKw4bAAADNElEQVRoBe2Zi1EbMRCGuYwLyIQOnA6cUEGSDkJSAdABjCtgSAckFTBOB0AFBDqADnDcgfN/svZG6KzLXXzmHmRn1pJW0mn170O6c7ZcLne6QFmWnUmPR/FC/FZ8Id3uVL54ejZscIamWFb7LP4tfl3nmRp/y1ybw3zxvXhssr6X7E/caWxGUnAjktdiuB/iB/FYTLsyaf6hBmP0nzZJ9YXktGfidybvW9k7bJqMOBkLw3LuVM4MGnspnsV6SEYk1XpW/IwutbWXzmPzSkq2TR8TCnB3gFL9q95h/6b2vhVsNj4mNrGFT6M8Yl7yHI6eHT+W6OJyea+o/4YcUh/3izPJvjvBAH7qYMN2A3xo7olPxGA3Ee+KH0PM1C5Q25nhjdfIPD1UMHaQqd8M94hpONDXP4UygTMRcwT1lSpj4x3B4eMxYt/wxLc5cr/GQMQYtZoZAuXKLp27UhoPv/Hj91XGjkIUEA1ECJFwRF3kssqq2tvfUmz8rgiO02CHZE/2bpkSPLjgO0phNFIHi137cVWLA3lcE98AYqOG61tk8O1hrvXsbeOL2uHGmUNmcc7i9TrSvjhvU2cucwrUMhaxPlWxYd6p9g0GRgTElclUXlkHZQqjkZ/Qyusba8sA6GeGp25kEfHgdSTqMS5y83gba+nQ2v9UtolFrHBVbJhn+ATPACeyZS1q+86AsnitGT5U3hwk9GruBblzBIO5IA2R6mDj9q+AIStAIW4ryV9+u+AMXAjfr9GTc+8u8nrOwSfHkzbP5ehizfwhiCphAwZiCyh3NAq3HCf1HQf9SVyadgZTqLAgyoiXYj495ySlSflzyTGqI8aqwmvkwUqS/9pXTifw4/bCjecju1fZCjbCAOPjNM4JVMZvVQTQIgoqiYo0KorqS6TQuWaR1k2ha8l+qX3rje3ONckwZn6rVd2IO8tU/SgO8WbwITay2icaMxPzpxbfFnhu7bORec9F0nXb2IAzl+ux1jpWydvWoV/XBZ7ZQPJSyjSwdEBfOwUGjnmu/XHc/Kc1CMQYNX1MrFmyNVEyLbemUfcWfoLR4DKDvJ2jhqODzECdFHqjDJF/vlb7RVMKoz/OqujLla6SYAAAAABJRU5ErkJggg==\n",
      "text/latex": [
       "$\\displaystyle - 1.0 v_{1} - 1.0 x_{1}$"
      ],
      "text/plain": [
       "-1.0⋅v₁ - 1.0⋅x₁"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dv_dt_example"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The above equation is a linear ODE, can can be put in the form $\\dot{x} = Ax$, where $x$ is a vector of position and velocity, and $A$ is a 2x2 matrix."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[ 0.,  1.],\n",
       "        [-1., -1.]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A_00 = diff(dx_dt_example, x1)\n",
    "A_01 = diff(dx_dt_example, v1)\n",
    "A_10 = diff(dv_dt_example, x1)\n",
    "A_11 = diff(dv_dt_example, v1)\n",
    "A_num = np.matrix([[A_00, A_01], [A_10, A_11]]).astype(np.float64)\n",
    "A_num"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This matrix can be used for simulation, as shown below, or for computing the analytical solution of the system, as shown next."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 9904 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Explicit Euler solver\n",
    "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\n",
    "\n",
    "# Simulation step size, time range, and total number of steps to be taken\n",
    "h = 0.1\n",
    "t_range = np.arange(0.0,7.0,h)\n",
    "num_steps = np.size(t_range)\n",
    "\n",
    "# Initial condition\n",
    "x0 = np.matrix([[1.0],[0.0]]) \n",
    "\n",
    "# Simulation\n",
    "xs_e = Explicit_Euler().simulate(lambda x: A_num*x, x0, 0.0, h, num_steps)\n",
    "\n",
    "# Plotting\n",
    "plt.figure(1)\n",
    "plt.plot(t_range,[x[0,0] for x in xs_e], 'o', markersize=3, label='x_euler')\n",
    "plt.xlabel('t (s)')\n",
    "plt.ylabel('position (m)')\n",
    "\n",
    "plt.legend()\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The analytical solution of a linear IVP $\\dot{x} = Ax$, where $x_0$ is known, is given by \n",
    "$$x(t) = e^{tA}x_0$$\n",
    "where $e^{At}$ is the matrix exponencial.\n",
    "Given a simulation step size $h$, the above formula can be put in the iterative form\n",
    "$$x(t+h) = e^{hA}x(t)$$\n",
    "where $x(t)$ is given.\n",
    "The iterative form is more efficient for computation, as we only compute $e^{hA}$ once."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def analytical_solution_matrix(A, x0, h, max_steps):\n",
    "    x = x0\n",
    "    exphA = linalg.expm(h*A) # Computes matrix exponencial.\n",
    "    xs = [x]\n",
    "    for i in range(1, max_steps):\n",
    "        x = exphA * x\n",
    "        xs.append(x)\n",
    "    return xs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plotting the analytical solution against the simulation computed above:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 16856 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "xs_sol = analytical_solution_matrix(A_num, x0, h, num_steps)\n",
    "\n",
    "plt.figure(1)\n",
    "plt.plot(t_range,[x[0,0] for x in xs_e], 'o', markersize=3, label='x_euler')\n",
    "plt.plot(t_range,[x[0,0] for x in xs_sol], '-', markersize=3, label='x_solution')\n",
    "plt.xlabel('t (s)')\n",
    "plt.ylabel('position (m)')\n",
    "\n",
    "plt.legend()\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise: Implement the Midpoint Method"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Adapt the midpoint method introduced in the Example1_CruiseController notebook to work with the mass spring damper vector IVP."
   ]
  },
  {
   "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 x: A_num*x, x0, 0.0, h, num_steps)\n",
    "\n",
    "# Plotting\n",
    "plt.figure(1)\n",
    "plt.plot(t_range,[x[0,0] for x in xs_e], 'o', markersize=3, label='x_euler')\n",
    "plt.plot(t_range,[x[0,0] for x in xs_mid], 'o', markersize=3, label='x_midpoint')\n",
    "plt.plot(t_range,[x[0,0] for x in xs_sol], '-', markersize=3, label='x_solution')\n",
    "plt.xlabel('t (s)')\n",
    "plt.ylabel('position (m)')\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
}
