{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Double Mass Spring Damper Co-simulation Example"
   ]
  },
  {
   "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",
    "from scipy.linalg import expm\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\n",
    "\n",
    "# Import local code\n",
    "import sys\n",
    "sys.path.append(\"C:\\\\srcctrl\\\\2019.SummerSim.Paper\\\\experiments\\\\PowerWindow\\\\FaultInjectionCosim\")\n",
    "from fmifaultinjection.core import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MSD1(VirtualFMU):\n",
    "    def __init__(self, instanceName):\n",
    "        ref = 0\n",
    "        self.x = ref;\n",
    "        ref += 1\n",
    "        self.v = ref;\n",
    "        ref += 1\n",
    "        self.m = ref;\n",
    "        ref += 1\n",
    "        self.c = ref;\n",
    "        ref += 1\n",
    "        self.cf = ref;\n",
    "        ref += 1\n",
    "        self.fe = ref;\n",
    "        ref += 1\n",
    "\n",
    "        super().__init__(instanceName, ref)\n",
    "\n",
    "    def reset(self):\n",
    "        super().reset()\n",
    "\n",
    "        self.state[self.x] = 1.0\n",
    "        self.state[self.v] = 0.0\n",
    "        self.state[self.m] = 1.0\n",
    "        self.state[self.c] = 1.0\n",
    "        self.state[self.cf] = 1.0\n",
    "        self.state[self.fe] = 0.0\n",
    "\n",
    "    def doStep(self, currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPoint=fmi2True):\n",
    "        der_x = self.state[self.v]\n",
    "        der_v = (1.0/self.state[self.m])*(- self.state[self.c]*self.state[self.x]\n",
    "                                            - self.state[self.cf] * self.state[self.v] \n",
    "                                            + self.state[self.fe])\n",
    "        \n",
    "        self.state[self.x] = self.state[self.x] + der_x * communicationStepSize\n",
    "        self.state[self.v] = self.state[self.v] + der_v * communicationStepSize\n",
    "\n",
    "\n",
    "class MSD2(VirtualFMU):\n",
    "    def __init__(self, instanceName):\n",
    "        ref = 0\n",
    "        self.x = ref;\n",
    "        ref += 1\n",
    "        self.v = ref;\n",
    "        ref += 1\n",
    "        self.m = ref;\n",
    "        ref += 1\n",
    "        self.c = ref;\n",
    "        ref += 1\n",
    "        self.cf = ref;\n",
    "        ref += 1\n",
    "        self.ce = ref;\n",
    "        ref += 1\n",
    "        self.cef = ref;\n",
    "        ref += 1\n",
    "        self.fe = ref;\n",
    "        ref += 1\n",
    "        self.xe = ref;\n",
    "        ref += 1\n",
    "        self.ve = ref;\n",
    "        ref += 1\n",
    "\n",
    "        super().__init__(instanceName, ref)\n",
    "\n",
    "    def reset(self):\n",
    "        super().reset()\n",
    "\n",
    "        self.state[self.x] = 0.0\n",
    "        self.state[self.v] = 0.0\n",
    "        self.state[self.m] = 1.0\n",
    "        self.state[self.c] = 1.0\n",
    "        self.state[self.cf] = 1.0\n",
    "        self.state[self.ce] = 1.0\n",
    "        self.state[self.cef] = 1.0\n",
    "        self.state[self.fe] = 0.0\n",
    "        self.state[self.xe] = 0.0\n",
    "        self.state[self.ve] = 0.0\n",
    "\n",
    "    def doStep(self, currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPoint=fmi2True):\n",
    "        self.state[self.fe] = self.state[self.ce]*(self.state[self.x]-self.state[self.xe]) \\\n",
    "                                + self.state[self.cef]*(self.state[self.v] - self.state[self.ve])\n",
    "        \n",
    "        der_x = self.state[self.v]\n",
    "        der_v = (1.0/self.state[self.m])*(- self.state[self.c]*self.state[self.x]\n",
    "                                            - self.state[self.cf] * self.state[self.v] \n",
    "                                            - self.state[self.fe])\n",
    "        \n",
    "        self.state[self.x] = self.state[self.x] + der_x * communicationStepSize\n",
    "        self.state[self.v] = self.state[self.v] + der_v * communicationStepSize\n",
    "        \n",
    "\n",
    "class CoupledMSDReference(VirtualFMU):\n",
    "    def __init__(self, instanceName):\n",
    "        ref = 0\n",
    "        self.x1 = ref;\n",
    "        ref += 1\n",
    "        self.v1 = ref;\n",
    "        ref += 1\n",
    "        self.m1 = ref;\n",
    "        ref += 1\n",
    "        self.c1 = ref;\n",
    "        ref += 1\n",
    "        self.cf1 = ref;\n",
    "        ref += 1\n",
    "        \n",
    "        self.x2 = ref;\n",
    "        ref += 1\n",
    "        self.v2 = ref;\n",
    "        ref += 1\n",
    "        self.m2 = ref;\n",
    "        ref += 1\n",
    "        self.c2 = ref;\n",
    "        ref += 1\n",
    "        self.cf2 = ref;\n",
    "        ref += 1\n",
    "        self.ce = ref;\n",
    "        ref += 1\n",
    "        self.cef = ref;\n",
    "        ref += 1\n",
    "        \n",
    "        super().__init__(instanceName, ref)\n",
    "\n",
    "    def reset(self):\n",
    "        super().reset()\n",
    "\n",
    "        self.state[self.x1] = 1.0\n",
    "        self.state[self.v1] = 0.0\n",
    "        self.state[self.m1] = 1.0\n",
    "        self.state[self.c1] = 1.0\n",
    "        self.state[self.cf1] = 1.0\n",
    "        \n",
    "        self.state[self.x2] = 0.0\n",
    "        self.state[self.v2] = 0.0\n",
    "        self.state[self.m2] = 1.0\n",
    "        self.state[self.c2] = 1.0\n",
    "        self.state[self.cf2] = 1.0\n",
    "        self.state[self.ce] = 1.0\n",
    "        self.state[self.cef] = 1.0\n",
    "        \n",
    "    def doStep(self, currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPoint=fmi2True):\n",
    "        \n",
    "        m1 = self.state[self.m1]\n",
    "        c1 = self.state[self.c1]\n",
    "        cf1 = self.state[self.cf1]\n",
    "        \n",
    "        m2 = self.state[self.m2]\n",
    "        ce = self.state[self.ce]\n",
    "        cef = self.state[self.cef]\n",
    "        c2 = self.state[self.c2]\n",
    "        cf2 = self.state[self.cf2]\n",
    "        \n",
    "        A = np.matrix([ \n",
    "                [0,                     1,                      0,                    0                     ], \n",
    "                [(-(1/m1)*(c1+ce)),     (-(1/m1)*(cf1+cef)),      ((1/m1)*cef),           ((1/m1)*cef)           ], \n",
    "                [0,                     0,                      0,                    1                     ],\n",
    "                [((1/m2)*ce),           ((1/m2)*cef),            (-(1/m2)*(c2+ce)),    (-(1/m2)*(cf2+cef))    ]\n",
    "              ])\n",
    "        \n",
    "        exphA = expm(communicationStepSize*A)\n",
    "\n",
    "        x = np.matrix([ \n",
    "                    [self.state[self.x1]], #x1\n",
    "                    [self.state[self.v1]], #v1\n",
    "                    [self.state[self.x2]], #x2\n",
    "                    [self.state[self.v2]]  #v2\n",
    "                  ])\n",
    "        \n",
    "        xn = exphA*x\n",
    "        \n",
    "        self.state[self.x1] = xn[0,0]\n",
    "        self.state[self.v1] = xn[1,0]\n",
    "        self.state[self.x2] = xn[2,0]\n",
    "        self.state[self.v2] = xn[3,0]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "msd1 = MSD1(\"msd1\")\n",
    "msd1.instantiate()\n",
    "msd2 = MSD2(\"msd2\")\n",
    "msd2.instantiate()\n",
    "solution = CoupledMSDReference(\"solution\")\n",
    "solution.instantiate()\n",
    "\n",
    "msd1_out = Connection(value_type=VarType.REAL,\n",
    "                    signal_type=SignalType.CONTINUOUS,\n",
    "                    source_fmu=msd1,\n",
    "                    target_fmu=msd2,\n",
    "                    source_vr=[msd1.x,msd1.v],\n",
    "                    target_vr=[msd2.xe,msd2.ve])\n",
    "msd1_in = Connection(value_type=VarType.REAL,\n",
    "                    signal_type=SignalType.CONTINUOUS,\n",
    "                    source_fmu=msd2,\n",
    "                    target_fmu=msd1,\n",
    "                    source_vr=[msd2.fe],\n",
    "                    target_vr=[msd1.fe])\n",
    "msd2_out = OutputConnection(value_type=VarType.REAL,\n",
    "                    signal_type=SignalType.CONTINUOUS,\n",
    "                    source_fmu=msd2,\n",
    "                    source_vr=[msd2.x,msd2.v])\n",
    "solution_out = OutputConnection(value_type=VarType.REAL,\n",
    "                    signal_type=SignalType.CONTINUOUS,\n",
    "                    source_fmu=solution,\n",
    "                    source_vr=[solution.x1,solution.v1,solution.x2,solution.v2])\n",
    "\n",
    "connections = [msd1_out, msd1_in]\n",
    "out_connections = [msd1_out, msd1_in, msd2_out, solution_out]\n",
    "scenario = CosimScenario(\n",
    "            fmus=[msd1, msd2,solution],\n",
    "            connections=connections,\n",
    "            step_size=0.01,\n",
    "            print_interval=0.1,\n",
    "            stop_time = 10.0,\n",
    "            outputs=out_connections)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "runner = CosimRunner()\n",
    "\n",
    "results = runner.run_cosim(scenario, lambda t: None)\n",
    "\n",
    "for f in scenario.fmus:\n",
    "    f.terminate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 18548 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.xlabel(\"time\")\n",
    "plt.ylabel('position (m)')\n",
    "plt.plot(results.timestamps, results.signals[msd1.instanceName][msd1.x], 'o', label=\"x1\")\n",
    "plt.plot(results.timestamps, results.signals[msd2.instanceName][msd2.x], 'o', label=\"x2\")\n",
    "plt.plot(results.timestamps, results.signals[solution.instanceName][solution.x1], '-', label=\"x1_sol\")\n",
    "plt.plot(results.timestamps, results.signals[solution.instanceName][solution.x2], '-', label=\"x2_sol\")\n",
    "plt.legend()\n",
    "\n",
    "pp = PdfPages('double_msd_example_trace.pdf')\n",
    "pp.savefig()\n",
    "pp.close()\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
