{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0e827775",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "# Data Logging\n",
    "\n",
    "This notebook explores **kdflex**'s built in data logging capabilities using a 2-link pendulum example multibody. We start with generic non-simulation data logging to H5 files using {py:class}`Karana.KUtils.H5Writer`. Then we will demonstrate automatically logging sim data using the {py:class}`Karana.Models.DataLogger`. This requires going through the typical process of setting up a multibody and running the simulation.\n",
    "\n",
    "Requirements:\n",
    "- [2-link Pendulum](../example_2_link_pendulum/notebook.ipynb)\n",
    "\n",
    "In this tutorial we will:\n",
    "- Setup the PacketTables\n",
    "- Log using H5Writer\n",
    "- Read the H5 file using H5py  \n",
    "- Setup a 2-link pendulum\n",
    "- Attach a DataLogger model\n",
    "- Run the simulation\n",
    "- Read logging output\n",
    "- Clean up the simulation\n",
    "\n",
    "For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page).\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7e2551c",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import atexit\n",
    "from typing import cast\n",
    "from math import pi\n",
    "import Karana.Core as kc\n",
    "import Karana.Math as km\n",
    "import Karana.Integrators as ki\n",
    "import Karana.Frame as kf\n",
    "import Karana.Dynamics as kd\n",
    "import Karana.Scene as ks\n",
    "import Karana.KUtils as ku\n",
    "import Karana.Models as kmdl"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "77fb48de",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "# Generic Non-Simulation Data Logging\n",
    "\n",
    "We don't necessarily need to be running a simulation to use **kdflex**'s logging capabilities. The following cells demonstrate how to use **kdflex**'s data logging capabilities without running a simulation. See later in the notebook for how to log data from a simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "235cfeb0",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup the PacketTables\n",
    "\n",
    "PacketTables are the tables that store data in the H5 log file.We use {py:class}`Karana.KUtils.PacketTableConfig` to define the columns and associated update functions for a PacketTable."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "862b6a5d",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "#  Here we create two packet tables configs which we will use to create the tables to store data in\n",
    "#  We use two tables here. This tests whether we can use a group that already exists, i.e., mygroup will\n",
    "# already be created by c and should be re-used by c2.\n",
    "c_gen = ku.PacketTableConfig(\"mygroup/inner_group/data\")\n",
    "c2_gen = ku.PacketTableConfig(\"mygroup/data\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0b730f56",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "For our case we next create functions for tracking example values. Each time these functions are called the data updates, simulating what our values might look like in a simulation. This is just to simulate what some sort of value we are interested in logging will look like. These functions are then added to the packet table configs."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "1db3fc3c",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# float\n",
    "t = 0.0\n",
    "\n",
    "\n",
    "def time():\n",
    "    \"\"\"Return current time.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    float\n",
    "        The current time.\n",
    "    \"\"\"\n",
    "    global t\n",
    "    t += 0.1\n",
    "    return t\n",
    "\n",
    "\n",
    "# adding float data to be recorded in the table\n",
    "c_gen.addDataFloat(\"time\", time)\n",
    "\n",
    "# integer\n",
    "i = 1\n",
    "\n",
    "\n",
    "def int_f() -> int:\n",
    "    \"\"\"Return an integer that increases by one each time this is called.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    int\n",
    "        The current value of the integer.\n",
    "    \"\"\"\n",
    "    global i\n",
    "    i += 1\n",
    "    return i\n",
    "\n",
    "\n",
    "# adding integer data to be recorded in the table\n",
    "c_gen.addDataInt(\"int\", int_f)\n",
    "\n",
    "# dynamic vector\n",
    "v = np.zeros(2)\n",
    "\n",
    "\n",
    "def vec():\n",
    "    \"\"\"Return a vector.\n",
    "\n",
    "    The first component increases by 1 and the second component by 2 each time this is called.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    NDArray[np.float64]\n",
    "        The current vector value.\n",
    "    \"\"\"\n",
    "    global v\n",
    "    v[0] += 1.0\n",
    "    v[1] += 2.0\n",
    "    return v\n",
    "\n",
    "\n",
    "# adding a dynamic vector to be recorded in the table\n",
    "c_gen.addData(\"vec\", vec, 2, as_scalars=True)\n",
    "\n",
    "# dynamic matrix\n",
    "m = np.zeros((3, 2))\n",
    "\n",
    "\n",
    "def mat():\n",
    "    \"\"\"Return a matrix.\n",
    "\n",
    "    The first component increases by 1 each time this is called.\n",
    "\n",
    "    Returns\n",
    "    -------\n",
    "    NDArray[np.float64]\n",
    "        The current matrix value.\n",
    "    \"\"\"\n",
    "    global m\n",
    "    m[0, 0] += 1.0\n",
    "    return m\n",
    "\n",
    "\n",
    "# adding a dynamic matrix to be recorded in both table configs\n",
    "c_gen.addData(\"mat\", mat, 3, 2)\n",
    "c2_gen.addData(\"mat\", mat, 3, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "36193966",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Log Using H5Writer\n",
    "\n",
    "We now create the {py:class}`Karana.KUtils.H5Writer` and specify a file to output data to. We create the packet tables from the packet table config and then log the data by calling {py:meth}`Karana.KUtils.H5Writer.log`. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "4b9ad221",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# create logger and tables\n",
    "h5 = ku.H5Writer(\"example.h5\")\n",
    "h5.createTable(c_gen)\n",
    "h5.createTable(c2_gen)\n",
    "\n",
    "# log data three times\n",
    "h5.log()\n",
    "h5.log()\n",
    "h5.log()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b915e036",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Below we demonstrate table naming and how to deactivate an active table. Logging will only add data to active tables."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "48607563",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Active table names: mygroup.inner_group.data, mygroup.data\n",
      "Active table names: mygroup.data\n"
     ]
    }
   ],
   "source": [
    "# check active table names\n",
    "print(f\"Active table names: {', '.join(h5.getActiveTableNames())}\")\n",
    "\n",
    "# deactivate table\n",
    "h5.deactivateTable(\"mygroup/inner_group/data\")\n",
    "\n",
    "# check active table names\n",
    "print(f\"Active table names: {', '.join(h5.getActiveTableNames())}\")\n",
    "\n",
    "# log only active tables\n",
    "h5.log()\n",
    "\n",
    "# logging an inactive table via logTable\n",
    "h5.deactivateTable(\"mygroup/data\")\n",
    "h5.logTable(\"mygroup/data\")\n",
    "\n",
    "# activate a new table\n",
    "h5.activateTable(\"mygroup/data\")\n",
    "h5.log()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "691591e7",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Read the H5 File Using h5py\n",
    "\n",
    "Let's view the data we just logged. In order to do this we need to delete the H5 writer so that it releases its lock on the HDF5 file. We also need to delete the corresponding PacketTableConfig in order to properly delete the H5 writer.\n",
    "\n",
    "We then specify the output file we want to read, and print out the keys for the data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "abcb4507",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<KeysViewHDF5 ['mygroup']>"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import h5py\n",
    "\n",
    "del c_gen, c2_gen, h5\n",
    "\n",
    "# load the file\n",
    "f = h5py.File(\"example.h5\", \"r\")\n",
    "f.keys()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "50712054",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "The HDF5 is structured in data groups which we specified earlier. The following cells depict how to parse and unpack this data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "bd659a91",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<KeysViewHDF5 ['data', 'inner_group']>\n",
      "[('mat', '<f8', (3, 2))]\n",
      "<KeysViewHDF5 ['data']>\n",
      "Data types logged in the inner group: [('time', '<f8'), ('int', '<i4'), ('vec_0', '<f8'), ('vec_1', '<f8'), ('mat', '<f8', (3, 2))]\n"
     ]
    }
   ],
   "source": [
    "# Data can be accessed like a dictionary using keys.\n",
    "# The following print statements illustrate the\n",
    "# structure of the hdf5 file and how to access the data\n",
    "\n",
    "# Show keys for mygroup\n",
    "print(f[\"mygroup\"].keys())\n",
    "print(f[\"mygroup\"][\"data\"].dtype)\n",
    "\n",
    "matrix_data = f[\"mygroup\"][\"data\"][\"mat\"][:]\n",
    "\n",
    "# Show keys for inner_group\n",
    "print(f[\"mygroup\"][\"inner_group\"].keys())\n",
    "dtype = f[\"mygroup\"][\"inner_group\"][\"data\"].dtype\n",
    "print(f\"Data types logged in the inner group: {dtype}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "8632dc5d",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Time data: [0.1 0.2 0.3]\n",
      "Integer data: [2 3 4]\n",
      "Vector element 0 data: [1. 2. 3.]\n",
      "Vector element 1 data: [2. 4. 6.]\n",
      "Matrix data: [[[2. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]\n",
      "\n",
      " [[4. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]\n",
      "\n",
      " [[6. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]\n",
      "\n",
      " [[7. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]\n",
      "\n",
      " [[8. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]\n",
      "\n",
      " [[9. 0.]\n",
      "  [0. 0.]\n",
      "  [0. 0.]]]\n"
     ]
    }
   ],
   "source": [
    "# Print out logged data\n",
    "time_data = f[\"mygroup\"][\"inner_group\"][\"data\"][\"time\"][:]\n",
    "integer_data = f[\"mygroup\"][\"inner_group\"][\"data\"][\"int\"][:]\n",
    "vec_0_data = f[\"mygroup\"][\"inner_group\"][\"data\"][\"vec_0\"][:]\n",
    "vec_1_data = f[\"mygroup\"][\"inner_group\"][\"data\"][\"vec_1\"][:]\n",
    "mat_data = f[\"mygroup\"][\"data\"][\"mat\"][:]\n",
    "print(f\"Time data: {time_data}\")\n",
    "print(f\"Integer data: {integer_data}\")\n",
    "print(f\"Vector element 0 data: {vec_0_data}\")\n",
    "print(f\"Vector element 1 data: {vec_1_data}\")\n",
    "print(f\"Matrix data: {mat_data}\")\n",
    "\n",
    "matrix_data_2 = f[\"mygroup\"][\"inner_group\"][\"data\"][\"mat\"][:]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d4d36fb9",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "# Simulation Data Logging\n",
    "\n",
    "Now we demonstrate automatically logging sim data using a DataLogger model. To do so, we setup a simulation, configure our writer, attach it to our state propagator, and run the simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bde08ea",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup a 2-link Pendulum\n",
    "\n",
    "We procedurally generate a 2-link pendulum multibody. It is a simpler way of creating the pendulum but has the same functionality as manual creation. You can read more about procedurally generating a multibody in our $n$-link pendulum [tutorial](../example_n_link_pendulum/notebook.ipynb)\n",
    "\n",
    "See [Multibody](treembody_sec) or [Frames](frames_layer_sec) for more information relating to this step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a5b75454",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "fc = kf.FrameContainer(\"root\")\n",
    "\n",
    "\n",
    "def createMbody(n_links: int):\n",
    "    \"\"\"Create the multibody.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    n_links : int\n",
    "        The number of pendulum links to use.\n",
    "    \"\"\"\n",
    "    mb = kd.Multibody(\"mb\", fc)\n",
    "\n",
    "    mat_info = ks.PhysicalMaterialInfo()\n",
    "    mat_info.color = ks.Color.FIREBRICK\n",
    "    brown = ks.PhysicalMaterial(mat_info)\n",
    "\n",
    "    sp_body = ks.ScenePartSpec()\n",
    "    sp_body.name = \"sp_body\"\n",
    "    sp_body.geometry = ks.BoxGeometry(0.05, 0.05, 1)\n",
    "    sp_body.material = brown\n",
    "    sp_body.transform = km.HomTran([0.0, 0.0, 0.0])\n",
    "    sp_body.scale = [1, 1, 1]\n",
    "\n",
    "    sp_pivot = ks.ScenePartSpec()\n",
    "    sp_pivot.name = \"sp_pivot\"\n",
    "    sp_pivot.geometry = ks.CylinderGeometry(0.1, 0.1)\n",
    "    sp_pivot.material = brown\n",
    "    sp_pivot.transform = km.HomTran([0.0, 0.0, -0.5])\n",
    "    sp_pivot.scale = [1, 1, 1]\n",
    "\n",
    "    params = kd.PhysicalBodyParams(\n",
    "        spI=km.SpatialInertia(2.0, np.zeros(3), np.diag([3, 2, 1])),\n",
    "        axes=[np.array([0.0, 1.0, 0.0])],\n",
    "        body_to_joint_transform=km.HomTran(np.array([0, 0, 0.5])),\n",
    "        inb_to_joint_transform=km.HomTran(np.array([0, 0, -0.5])),\n",
    "        scene_part_specs=[sp_body, sp_pivot],\n",
    "    )\n",
    "    kd.PhysicalBody.addSerialChain(\n",
    "        \"link\",\n",
    "        n_links,\n",
    "        cast(kd.PhysicalBody, mb.virtualRoot()),\n",
    "        htype=kd.HingeType.REVOLUTE,\n",
    "        params=params,\n",
    "    )\n",
    "\n",
    "    # finalize and verify multibody\n",
    "    mb.ensureHealthy()\n",
    "    mb.resetData()\n",
    "    assert kc.allReady()\n",
    "\n",
    "    return mb"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3ebe427e",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "In this cell we create the multibody by calling the createMbody we defined in the previous cell. We also obtain the two pendulum bodies and assign them to variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "d4a29bd7",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# initialization\n",
    "n_links = 2\n",
    "mb = createMbody(n_links)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c9258a5a",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Next we setup **kdflex**'s graphics by calling the setupGraphics helper method on the multibody. This method takes care of setting up the graphics environment. \n",
    "\n",
    "See [Visualization](visualization_sec) and [Scene Layer](scene_layer_sec) for more information relating to this section."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "d8e44d9b",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <div style=\"height:300px; resize:vertical; overflow: auto;\">\n",
       "          <iframe src=\"http://localhost:43575\" style=\"width:100%; height:100%; border:0; display:block;\"></iframe>\n",
       "        </div>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.0)\n",
    "# position the viewpoint camera of the visualization\n",
    "web_scene.defaultCamera().pointCameraAt(\n",
    "    [0.0, 5.0 + n_links / 2, -n_links / 2], [0, 0, -n_links / 2], [0, 0, 1]\n",
    ")\n",
    "\n",
    "proxy_scene = mb.getScene()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e2ffa804",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Now, we setup the {py:class}`Karana.Dynamics.StatePropagator` and register our models as well. \n",
    "\n",
    "See [Models](system_level_models_sec) for more concepts and information."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b9270355",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# Set up state propagator and select integrator type: rk4 or cvode\n",
    "sp = kd.StatePropagator(mb, integrator_type=ki.IntegratorType.RK4)\n",
    "integrator = sp.getIntegrator()\n",
    "\n",
    "# add a gravitational model to the state propagator\n",
    "ug = kmdl.Gravity(\"grav_model\", sp, kmdl.UniformGravity(\"uniform_gravity\"), mb)\n",
    "ug.getGravityInterface().setGravity(np.array([0, 0, -9.81]), 0.0, kmdl.OutputUpdateType.PRE_HOP)\n",
    "del ug\n",
    "\n",
    "# Makes sure the visualization scene is updated after each state change.\n",
    "kmdl.UpdateProxyScene(\"update_proxy_scene\", sp, proxy_scene)\n",
    "\n",
    "# sync the simulation time with real-time.\n",
    "kmdl.SyncRealTime(\"sync_real_time\", sp, 1.0)\n",
    "\n",
    "\n",
    "def post_step_fn(t, x):\n",
    "    \"\"\"Print out the time and state.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    t : float\n",
    "        The current time.\n",
    "    x : NDArray[np.float64]\n",
    "       The current state.\n",
    "    \"\"\"\n",
    "    print(f\"t = {float(integrator.getTime()) / 1e9}s; x = {integrator.getX()}\")\n",
    "\n",
    "\n",
    "sp.fns.post_hop_fns[\"update_and_info\"] = post_step_fn"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "62eec191",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Before we begin our simulation, we set the initial state for our multibody and statepropagator. \n",
    "\n",
    "When accessing or modifying [generalized coordinates](coords_sec) for a subhinge, it is recommended to directly set the subhinge's values rather than for the entire multibody in order to avoid ambiguity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "ca79d2ba",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# Modify the initial multibody state.\n",
    "# Here we will set the first pendulum position to pi/8 radians and its velocity to 0.0\n",
    "bd1 = mb.getBody(\"link_0\")\n",
    "bd1.parentHinge().subhinge(0).setQ([pi / 8])\n",
    "bd1.parentHinge().subhinge(0).setU([0.0])\n",
    "\n",
    "# Initialize state. Note that in kdflex there are two states, one for multibody and one for integrator.\n",
    "# Here we get the multibody state vector we updated above and then feed it to the integrator.\n",
    "t_init = np.timedelta64(0, \"ns\")\n",
    "x_init = sp.assembleState()\n",
    "sp.setTime(t_init)\n",
    "sp.setState(x_init)\n",
    "\n",
    "# sync graphics\n",
    "proxy_scene.update()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b8a0330",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "To execute our callback functions every 0.1 seconds, we create a timed event and register it to our state propagator here. See [timed events](timed_events_sec) for more information."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "d5e4c74d",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "h = np.timedelta64(int(1e8), \"ns\")\n",
    "t = kd.TimedEvent(\"hop_size\", h, lambda _: None, False)\n",
    "t.period = h\n",
    "\n",
    "# register the timed event\n",
    "sp.registerTimedEvent(t)\n",
    "del t"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "32e1a3c3",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Attach a DataLogger Model\n",
    "\n",
    "First, we setup our H5Writer instance and define the functions that we use to log data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "66b939ac",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# create packet table configs and add data\n",
    "\n",
    "# adding time\n",
    "c1 = ku.PacketTableConfig(\"time_1\")\n",
    "c1.addDataFloat(\"t\", lambda: float(sp.getTime()) / 1e9)\n",
    "c2 = ku.PacketTableConfig(\"time_2\")\n",
    "c2.addDataFloat(\"t\", lambda: float(sp.getTime()) / 1e9)\n",
    "\n",
    "# adding state\n",
    "c3 = ku.PacketTableConfig(\"state\")\n",
    "c3.addData(\"state\", lambda: np.array(integrator.getX()), n_links * 2)\n",
    "\n",
    "# adding relative velocity between body frames\n",
    "c4 = ku.PacketTableConfig(\"rel_vel\")\n",
    "bd1 = mb.getBody(\"link_0\")\n",
    "bd2 = mb.getBody(\"link_1\")\n",
    "pendulum_end_to_end = bd1.frameToFrame(bd2)\n",
    "c4.addData(\"rel_vel\", lambda: np.array(pendulum_end_to_end.relSpVel().getv()), 3)\n",
    "\n",
    "# create the H5 write and tables\n",
    "h5_writer = ku.H5Writer(\"log.h5\")\n",
    "h5_writer.createTable(c1)\n",
    "h5_writer.createTable(c2)\n",
    "h5_writer.createTable(c3)\n",
    "h5_writer.createTable(c4)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e1d554e0",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Now we create the {py:class}`Karana.Models.DataLogger` using the H5Writer and set its data logging frequency. We then verify the DataLogger and H5Writer is ready and register the DataLogger to the state propagator."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "8896de51",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Data Logger is Finalized: True\n"
     ]
    }
   ],
   "source": [
    "dl = kmdl.DataLogger(\"wsm\", sp, h5_writer)\n",
    "dl.params.log_first_step = True\n",
    "dl.setPeriod(0.1)\n",
    "\n",
    "print(f\"Data Logger is Finalized: {dl.isReady()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3fcb76cc",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Run the Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "06df6bab",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "t = 0.1s; x = [ 0.38085961  0.00907525 -0.23551005  0.17987599]\n",
      "t = 0.2s; x = [ 0.34610386  0.03532941 -0.45583893  0.34039766]\n",
      "t = 0.3s; x = [ 0.29068861  0.07587749 -0.64636777  0.46275047]\n",
      "t = 0.4s; x = [ 0.21827047  0.1260333  -0.79389321  0.5300487 ]\n",
      "t = 0.5s; x = [ 0.13370607  0.17961689 -0.8878367   0.52975609]\n",
      "t = 0.6s; x = [ 0.04272369  0.22952563 -0.92153824  0.45633623]\n",
      "t = 0.7s; x = [-0.04851943  0.26852565 -0.89317787  0.31280251]\n",
      "t = 0.8s; x = [-0.13393852  0.29010315 -0.80599992  0.11031439]\n",
      "t = 0.9s; x = [-0.2080092   0.28918536 -0.66784444 -0.13388806]\n",
      "t = 1.0s; x = [-0.26618358  0.26261765 -0.49022784 -0.39909194]\n",
      "t = 1.1s; x = [-0.30520321  0.20939939 -0.28723186 -0.66320243]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<SpStatusEnum.REACHED_END_TIME: 1>"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# This should log both times for 0.0, 0.1, and 0.2\n",
    "sp.advanceTo(0.2)\n",
    "\n",
    "# This should log only time_1 for 0.3\n",
    "h5_writer.deactivateTable(\"time_2\")\n",
    "sp.advanceTo(0.3)\n",
    "\n",
    "# This should log nothing for time up to 1.0\n",
    "sp.unregisterModel(dl)\n",
    "sp.advanceTo(1.0)\n",
    "\n",
    "# This should log only at 1.1, not 1.0\n",
    "h5_writer.activateTable(\"time_2\")\n",
    "dl.params.log_first_step = False\n",
    "sp.registerModel(dl)\n",
    "sp.advanceTo(1.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "58c0df01",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Read Logging Output\n",
    "\n",
    "Let's now view the results from the simulation. Make sure to delete the H5 writer and PacketTableConfigs to unlock the file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "a2954ed4",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<KeysViewHDF5 ['rel_vel', 'state', 'time_1', 'time_2']>"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "del c1, c2, c3, c4, h5_writer, integrator, dl\n",
    "kc.discard(sp)\n",
    "\n",
    "# load the file\n",
    "file = h5py.File(\"log.h5\", \"r\")\n",
    "file.keys()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "ea0a4a9e",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "State Vector: [([ 0.39269908,  0.        ,  0.        ,  0.        ],)\n",
      " ([ 0.38085961,  0.00907525, -0.23551005,  0.17987599],)\n",
      " ([ 0.34610386,  0.03532941, -0.45583893,  0.34039766],)\n",
      " ([ 0.29068861,  0.07587749, -0.64636777,  0.46275047],)\n",
      " ([-0.30520321,  0.20939939, -0.28723186, -0.66320243],)]\n",
      "Time1 Data: [(0. ,) (0.1,) (0.2,) (0.3,) (1.1,)]\n",
      "Time2 Data: [(0. ,) (0.1,) (0.2,) (1.1,)]\n",
      "Relative Velocity Data: [([ 0.        ,  0.        ,  0.        ],)\n",
      " ([-0.08993429,  0.        ,  0.0008162 ],)\n",
      " ([-0.17009262,  0.        ,  0.00601177],)\n",
      " ([-0.23070949,  0.        ,  0.01753933],)\n",
      " ([ 0.3243577 ,  0.        , -0.06893075],)]\n"
     ]
    }
   ],
   "source": [
    "# Output state data\n",
    "state_data = file[\"state\"][:]\n",
    "print(f\"State Vector: {state_data}\")\n",
    "\n",
    "# Output time data\n",
    "time_data = file[\"time_1\"][:]\n",
    "print(f\"Time1 Data: {time_data}\")\n",
    "\n",
    "# Output time data 2\n",
    "time_data_2 = file[\"time_2\"][:]\n",
    "print(f\"Time2 Data: {time_data_2}\")\n",
    "\n",
    "# Output relative velocity data\n",
    "rel_vel_data = file[\"rel_vel\"][:]\n",
    "print(f\"Relative Velocity Data: {rel_vel_data}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b0577ff9",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Clean Up the Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "36916c8e",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<function __main__.cleanup()>"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def cleanup():\n",
    "    \"\"\"Cleanup the simulation.\"\"\"\n",
    "    global bd1, bd2, web_scene, proxy_scene, pendulum_end_to_end\n",
    "    del bd1, bd2, web_scene, proxy_scene, pendulum_end_to_end\n",
    "\n",
    "    cleanup_graphics()\n",
    "\n",
    "    kc.discard(mb)\n",
    "    kc.discard(fc)\n",
    "\n",
    "\n",
    "atexit.register(cleanup)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "610e9004",
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Summary\n",
    "\n",
    "You are now able to store both non-simulation and simulation data to a H5 file and read its contents back later! We covered setting up PacketTableConfigs and attaching logging functions, creating our H5Writer, and registering a DataLogger model."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.3"
  },
  "name": "notebook.ipynb"
 },
 "nbformat": 4,
 "nbformat_minor": 5
}