{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Transformations Tutorial #2: The Coordinate System Manager\n",
    "\n",
    "## Introduction\n",
    "\n",
    "This tutorial is about the [`CoordinateSystemManager` (CSM)](https://weldx.readthedocs.io/en/latest/_autosummary/weldx.CoordinateSystemManager.html#weldx.CoordinateSystemManager) class of the `weldx.transformations` package.\n",
    "The purpose of the CSM is to define and manage the relationships of different coordinate systems and their associated data in a tree-like data structure. \n",
    "It provides methods to transform a `LocalCoordinateSystem` (LCS) or its data to an arbitrary other coordinate system.\n",
    "\n",
    "This tutorial builds upon the [tutorial about coordinate systems](transformations_01_coordinate_systems.ipynb), but its content can still be understood without reading the other one first.\n",
    "\n",
    "## Imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "from copy import deepcopy\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import xarray as xr\n",
    "from IPython.display import display\n",
    "\n",
    "from weldx import Q_, CoordinateSystemManager, LocalCoordinateSystem, WXRotation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create a CSM and add coordinate systems\n",
    "\n",
    "The creation of `CoordinateSystemManager` requires the name of the root coordinate system as parameter. As optional second parameter, we can give the `CoordinateSystemManager` instance a name. If no name is provided, it will get a default name."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "csm = CoordinateSystemManager(\"root\", \"My CSM\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Further coordinate systems can be added with the `add_cs` method of the `CoordinateSystemManager`. \n",
    "It expects four parameters with the fourth being optional. \n",
    "1. The name of the new coordinate system. \n",
    "2. The name of an already existing coordinate system which serves as reference for the new one.\n",
    "   The position and orientation of the new coordinate system that will be passed in form of a `LocalCoordinateSystem` refer to      this reference system\n",
    "3. A `LocalCoordinateSystem` that describes the position and orientation of the new coordinate system in its reference system.\n",
    "\n",
    "In case you only have the inverse data, meaning the position and orientation of the reference system inside the new coordinate system, you can set the fourth parameter (`lsc_child_in_parent`) to `False` and use the corresponding data instead."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "lcs_specimen_in_root = LocalCoordinateSystem(coordinates=Q_([0, 1, -0.5], \"cm\"))\n",
    "lcs_specimen_in_thermocouple = LocalCoordinateSystem(coordinates=Q_([0, -0.5, 0], \"cm\"))\n",
    "\n",
    "csm.add_cs(\"specimen\", \"root\", lcs_specimen_in_root)\n",
    "csm.add_cs(\n",
    "    \"thermocouple\", \"specimen\", lcs_specimen_in_thermocouple, lsc_child_in_parent=False\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Additionally, the `CoordinateSystemManager` provides some functions that create the `LocalCoordinateSystem` internally. \n",
    "The method `create_cs` takes the name of the new coordinate system and its reference system as first two parameters. \n",
    "The remaining parameters and their default values are identical to the ones from the `LocalCoordinateSystem.__init__` method. \n",
    "Similarly, there are functions for each of the `LocalCoordinateSystem`s construction methods (`from_euler`, `from_xyz`, etc.). \n",
    "The naming is simply `create_cs_` plus the name of the corresponding function of the `LoocalCoordinateSystem`. \n",
    "For example `construct_cs_from_euler` ([link tofunction documentation](https://weldx.readthedocs.io/en/latest/_autosummary/weldx.CoordinateSystemManager.create_cs_from_euler.html#weldx.CoordinateSystemManager.create_cs_from_euler)). \n",
    "As with `add_cs`, the last parameter of all those methods is `lsc_child_in_parent` which can be set to `False` if the provided values represent the orientation and coordinates of the reference system in the new child system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "csm.create_cs_from_euler(\n",
    "    \"flange\",\n",
    "    \"root\",\n",
    "    sequence=\"x\",\n",
    "    angles=20,\n",
    "    degrees=True,\n",
    "    coordinates=Q_([-1.5, 1, 1], \"cm\"),\n",
    ")\n",
    "csm.create_cs(\"torch\", \"flange\", coordinates=Q_([0, 0, -0.25], \"cm\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Please consult the [API documentation of the CSM](https://weldx.readthedocs.io/en/latest/_autosummary/weldx.CoordinateSystemManager.html#weldx.CoordinateSystemManager) to get an overview over the different LCS creation methods and their parameters"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualizing the coordinate system managers structure\n",
    "\n",
    "The internal structure of the `CoordinateSystemManager` is a tree. We can visualize it using the `plot_graph` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "csm.plot_graph();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside a Jupyter notebook it is sufficient to place the name of a `CoordinateSystemManager` instance at the end of a code cell to plot its graph:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Getting coordinate systems and transformations to another reference system\n",
    "\n",
    "Getting a coordinate system in relation to any other reference system in form of a `LocalCoordinateSystem` class is quite easy. \n",
    "Simply call `get_cs` with two parameters:\n",
    "\n",
    "1. The name of the system that should be transformed.\n",
    "2. The name of the target reference system.\n",
    "\n",
    "Let's try this for some coordinate systems:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(csm.get_cs(\"specimen\", \"root\") == lcs_specimen_in_root)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we get a coordinate system we added directly to the CSM.\n",
    "Therefore, no transformation is needed and the equality check is passed.\n",
    "If we want to get a coordinate system inside of its parent system, we can omit the second parameter as proven below:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(csm.get_cs(\"specimen\", \"root\") == csm.get_cs(\"specimen\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "However, note that the returned coordinate system is not necessarily identical to the one that we used during the definition.\n",
    "If we were setting the `lsc_child_in_parent` during the addition of the coordinate system to `False`, we provided the inverse transformation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(csm.get_cs(\"thermocouple\") == lcs_specimen_in_thermocouple)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(csm.get_cs(\"specimen\", \"thermocouple\") == lcs_specimen_in_thermocouple)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's get the coordinate \"thermocouple\" coordinate system in reference to the \"root\" system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.get_cs(\"thermocouple\", \"root\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the coordinate systems along the transformation path (\"thermocouple\", \"specimen\", \"root\") are not rotated, the coordinates of the resulting coordinate system is simply the sum of the individual coordinates:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "coords_transformed = csm.get_cs(\"thermocouple\", \"root\").coordinates.data\n",
    "coords_sum = (\n",
    "    csm.get_cs(\"thermocouple\").coordinates + csm.get_cs(\"specimen\").coordinates\n",
    ").data\n",
    "\n",
    "np.all(coords_transformed == coords_sum)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, getting coordinate systems in reference to any of the other coordinate systems of the CSM is quite easy."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualizing the coordinate systems of the CSM\n",
    "\n",
    "You can visualize all coordinate systems of the CSM in relation to each other by using its `plot` function.\n",
    "The `backend` parameter defines which rendering engine should be used.\n",
    "We will use `\"mpl\"` to use matplotlib first:\n",
    "\n",
    "> HINT: Note that you need the weldx-widgets package installed to use this functionality. Either use `pip install weldx-widgets` or `conda install weldx_widgets -c conda-forge` to install it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.plot(backend=\"mpl\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "By default the root coordinate system is the reference system of the plot, but we can change it using the `reference_system` parameter as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.plot(reference_system=\"torch\", backend=\"mpl\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The plot function has many other options that can be used to adjust the appearance of the plot.\n",
    "Consult the [function documentation](https://weldx.readthedocs.io/en/latest/_autosummary/weldx.CoordinateSystemManager.plot.html#weldx.CoordinateSystemManager.plot) for further details.\n",
    "\n",
    "If you are running a jupyter notebook, you can use k3d as rendering backend.\n",
    "It has a much more powerful rendering engine suited for 3d plots that can handle a vast amount of data and provides an interactive rendering window.\n",
    "Furthermore, the `plot` function of the CSM creates some additional control surfaces if k3d is used.\n",
    "This gives you the opportunity to modify the plot directly using buttons, checkboxes, sliders, etc. instead of recreating it with a different set of function parameters: \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Renaming existing coordinate systems\n",
    "Existing coordinate system nodes can be renamed using the `relabel` method.\n",
    "It expects a dictionary that maps old names to new ones as input.\n",
    "\n",
    "*Note: it is not possible to rename nodes on CSM instances that contain subsystems of any form. Subsystems are CSM instances that are attached to another CSM using the `merge` method. A detailed description follows further below.*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.relabel({\"thermocouple\": \"TC\"})\n",
    "csm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Updating and deleting coordinate system transformations\n",
    "\n",
    "A coordinate system can be updated by simply calling `add_cs` again with the correct reference system and an updated `LocalCoordinateSystem` instance. \n",
    "For example, to update the torch system, one could do the following:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.add_cs(\"torch\", \"flange\", LocalCoordinateSystem(coordinates=Q_([0, 0, -2], \"mm\")))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets check if the values have changed"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.get_cs(\"torch\", \"flange\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, the system has been updated. \n",
    "\n",
    "If you want to remove a coordinate system, you have to call `delete_cs`. \n",
    "The function takes two parameters.\n",
    "\n",
    "1. name of the coordinate system you want to delete.\n",
    "2. a `bool` that controls if child coordinate systems should be deleted too or not.\n",
    "\n",
    "In case you set this parameter to `False` (default) and try to remove a coordinate system with children, an exception is raised. \n",
    "This is an extra layer of security to prevent unintentional deletion of multiple coordinate systems. \n",
    "Here is an example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_copy = deepcopy(csm)\n",
    "csm_copy"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We made a copy of the `CoordinateSystemManager`.\n",
    "Now we are going to delete the specimen system, which has no other systems attached to it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_copy.delete_cs(\"TC\")\n",
    "csm_copy"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That worked. Now lets try to remove the flange system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    csm_copy.delete_cs(\"flange\")\n",
    "except Exception as e:\n",
    "    print(f\"Something went wrong! The following exception was raised:\\n{e}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the torch system is a child of the flange system and we didn't tell the function to delete child systems as well, an exception was raised. Lets retry this, but this time we will tell the function that we want child systems to be removed too:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_copy.delete_cs(\"flange\", True)\n",
    "csm_copy"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, we were successful and only the \"root\" and \"specimen\" coordinate systems are left in the `CoordinateSystemManager` instance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Assignment and transformation of spatial data\n",
    "\n",
    "A coordinate system stored in the `CoordinateSystemManager` can also get spatial data assigned to it. \n",
    "For example, this might be a sensor positions or geometry data in form of a point clouds. \n",
    "In this case it becomes the data's reference system and all values refer to its coordinate origin. \n",
    "\n",
    "Data can be assigned to a coordinate system with the `assign_data` function. \n",
    "It expects two parameters.\n",
    "1. The actual data. This can either be an `SpatialData` instance or an `xarray.DataArray`. \n",
    "2. A name of the data and the third one the name of the coordinate system that the data belongs to. \n",
    "\n",
    "In the following lines we define and assign some data to the `CoordinateSystemManager`.\n",
    "Let's create some data first:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from weldx import SpatialData\n",
    "\n",
    "points_pc = Q_([[2, (i / 50) + 1, 0] for i in range(201)], \"mm\")\n",
    "point_cloud_in_root = xr.DataArray(\n",
    "    points_pc, dims=[\"n\", \"c\"], coords={\"c\": [\"x\", \"y\", \"z\"]}\n",
    ")\n",
    "\n",
    "points_geo = Q_([[-1, -1, -0.2], [1, -1, -0.2], [-1, 2, -0.2], [1, 2, -0.2]], \"cm\")\n",
    "triangles = [[0, 1, 2], [2, 3, 1]]\n",
    "specimen_geometry_in_specimen = SpatialData(points_geo, triangles)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `SpatialData` class we used above is mostly a container class for 3d data.\n",
    "Additionally to pure point data, it can also store connectivety data in form of triangles.\n",
    "Consult the [class documentation](https://weldx.readthedocs.io/en/latest/_autosummary/weldx.SpatialData.html#weldx.SpatialData) for further information.\n",
    "Now we add the data to the CSM:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.assign_data(point_cloud_in_root, \"point cloud\", \"root\")\n",
    "csm.assign_data(specimen_geometry_in_specimen, \"specimen geometry\", \"specimen\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the `get_data` function, the unmodified data can be retrieved from the `CoordinateSystemManager` using its name as first parameter. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert point_cloud_in_root.identical(csm.get_data(\"point cloud\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The optional second parameter can be used to get the data transformed into any of the `CoordinateSystemManager`s coordinate systems. You just need to specify the desired target coordinate systems name:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.get_data(\"specimen geometry\", \"root\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "By default, the `plot` method will also visualize attached data.\n",
    "Here is an interactive plot of the CSM with the newly added data.\n",
    "You can change the reference system using the control surfaces."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A nice feature of attached `SpatialData` instances is that they are visualized with closed surfaces if they contain triangle data.\n",
    "In contrast, `xarray.DataArray` instances are always rendered as point clouds."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is not necessary to attach data to a coordinate system to transform it to another one. The `CoordinateSystemManager` also provides the `transform_data` function for this purpose. It expects three parameters.\n",
    "\n",
    "1. Actual data and must be a `numpy.ndarray` or an `xarray.DataArray`.\n",
    "2. The source system name.\n",
    "3. The target systems' name."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.transform_data(points_geo, \"specimen\", \"root\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you compare the resulting coordinates to the plot of the the geometry in the root coordinate system, you will see that they are identical.\n",
    "\n",
    "## Time dependencies\n",
    "\n",
    "As described in the [previous tutorial about the `LocalCoordinateSystem` class](transformations_01_coordinate_systems.ipynb), the orientation of a coordinate system towards its reference system might vary in time.\n",
    "From the API side, time dependent coordinate systems are not treated any different than constant coordinate systems.\n",
    "However, the `get_cs` function needs to perform some time interpolations internally if the timestamps of all involved coordinate systems aren't identical.\n",
    "You might wonder which times the resulting interpolated `LocalCoordinateSystem` possesses.\n",
    "The default behavior is that its timestamps are the time union of all coordinate systems participating in the transformation.\n",
    "However, you can also provide the desired timestamps using the `time` parameter of the `get_cs` function. \n",
    "\n",
    "Let's define a new `CoordinateSystemManager` to demonstrate the different approaches:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_tdp = CoordinateSystemManager(\"root\")\n",
    "\n",
    "csm_tdp.create_cs(\n",
    "    \"parent\",\n",
    "    \"root\",\n",
    "    coordinates=Q_([[3, -3, 0], [0, -3, 0]], \"mm\"),\n",
    "    orientation=WXRotation.from_euler(\"z\", [0, -np.pi / 2]).as_matrix(),\n",
    "    time=Q_([1, 6], \"days\"),\n",
    ")\n",
    "\n",
    "csm_tdp.create_cs(\n",
    "    \"child\",\n",
    "    \"parent\",\n",
    "    coordinates=Q_([0, 6, 0], \"mm\"),\n",
    "    orientation=WXRotation.from_euler(\"z\", [0, np.pi / 2]).as_matrix(),\n",
    "    time=Q_([2, 5], \"days\"),\n",
    ")\n",
    "\n",
    "csm_tdp.create_cs(\n",
    "    \"child child\",\n",
    "    \"child\",\n",
    "    coordinates=Q_([6, 0, 0], \"mm\"),\n",
    "    orientation=WXRotation.from_euler(\"z\", [0, np.pi / 2]).as_matrix(),\n",
    "    time=Q_([3, 4], \"days\"),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we plot the CSM:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_tdp.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, the traces of the time dependent coordinate systems are drawn into the plot.\n",
    "You can hide them by unchecking the corresponding checkbox.\n",
    "Use the slider or the \"play\" button to cycle through the different time steps.\n",
    "Also try switching the reference systems and see what happens. \n",
    "\n",
    "Now we use `get_cs` with a specific `time` to transform all coordinate systems to the `\"root\"` system, create a new CSM with this data and plot it: "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "time = Q_([(i + 1) for i in range(144)], \"hours\")\n",
    "\n",
    "csm_tdp_2 = CoordinateSystemManager(\"root\")\n",
    "\n",
    "system_names = [\"parent\", \"child\", \"child child\"]\n",
    "\n",
    "for name in system_names:\n",
    "    lcs = csm_tdp.get_cs(name, \"root\", time=time)\n",
    "    csm_tdp_2.add_cs(name, \"root\", lcs)\n",
    "\n",
    "csm_tdp_2.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you take a look at the traces, you will notice that they looks much smoother now.\n",
    "Use the time controls and you will see that the data grid is now much denser.\n",
    "\n",
    "We can also tell the `get_cs` method to interpolate all coordinate systems to match the timestamps of one of the CSMs time dependent coordinate systems.\n",
    "Therefore, we simply provide the target systems name as `time` parameter:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_tdp_3 = CoordinateSystemManager(\"root\")\n",
    "\n",
    "system_names = [\"parent\", \"child\", \"child child\"]\n",
    "\n",
    "for name in system_names:\n",
    "    lcs = csm_tdp.get_cs(name, \"root\", time=\"parent\")\n",
    "    csm_tdp_2.add_cs(name, \"root\", lcs)\n",
    "\n",
    "csm_tdp_2.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous two examples we used the `get_cs` method to interpolate the coordinate system data and create a new CSM from it.\n",
    "The purpose was to showcase how `get_cs` works with time dependent coordinate systems.\n",
    "However, if you just want to get an interpolated copy of a CSM, it is much simpler to use its `interp_time` method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "time = time = Q_([(i + 1) for i in range(144)], \"hours\")\n",
    "\n",
    "csm_tdp_3 = csm_tdp.interp_time(time=time)\n",
    "\n",
    "csm_tdp_3.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also limit the time interpolation to certain coordinate systems by using the `affected_coordinate_systems` parameter.\n",
    "It expects a list of coordinate system names."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "time = Q_([3600, 7200], \"minutes\")\n",
    "\n",
    "csm_tdp_4 = csm_tdp.interp_time(\n",
    "    time=time, affected_coordinate_systems=[\"child\", \"child child\"]\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following comparisons show that it worked:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.all(csm_tdp_4.get_cs(\"parent\").time == time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.all(csm_tdp_4.get_cs(\"child\").time == time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.all(csm_tdp_4.get_cs(\"child child\").time == time)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Merging and unmerging\n",
    "\n",
    "Sometimes a larger hierarchy of coordinate systems can be subdivided into multiple smaller subsystems.\n",
    "Defining them individually might be more intuitive and easier.\n",
    "For this reason, the `CoordinateSystemManager` provides the possibility to merge an instance into another one.\n",
    "We will introduce this functionality with a short example.\n",
    "\n",
    "Consider an automated welding process, where the torch is moved by a robot arm.\n",
    "The robot provides two mount points for additional equipment.\n",
    "We know where the torch and the mount points are located in the moving robot head coordinate system.\n",
    "So we can define the following `CoordinateSystemManager` instance to describe the setup:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_robot = CoordinateSystemManager(\"robot head\", \"robot coordinate systems\")\n",
    "csm_robot.create_cs(\"torch\", \"robot head\", coordinates=Q_([0, 0, -2], \"mm\"))\n",
    "csm_robot.create_cs(\"mount point 1\", \"robot head\", coordinates=Q_([0, 1, -1], \"mm\"))\n",
    "csm_robot.create_cs(\"mount point 2\", \"robot head\", coordinates=Q_([0, -1, -1], \"mm\"))\n",
    "csm_robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As extra measurement equipment we have a laser scanner that we want to attach to the first mounting point. \n",
    "It uses its own coordinate system and all the gathered data refers to it. \n",
    "We know the coordinates of the scanners own mount point inside this system so that we can define:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scanner = CoordinateSystemManager(\"scanner\", \"scanner coordinate systems\")\n",
    "csm_scanner.create_cs(\"mount point 1\", \"scanner\", coordinates=Q_([0, 0, 2], \"mm\"))\n",
    "csm_scanner"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the coordinate system \"mount point 1\" was added to both `CoordinateSystemManager` instances. \n",
    "Merging requires that both instances share exactly one common coordinate system that serves as connecting node.\n",
    "\n",
    "Now we merge the scanner coordinate systems with the robot coordinate systems using the `merge` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_robot.merge(csm_scanner)\n",
    "csm_robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can see in the output above that the scanner coordinate systems were successfully merged into the `CoordinateSystemManager` instance containing the robot data. Let's define some additional `CoordinateSystemManager` instances:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_specimen = CoordinateSystemManager(\"specimen\", \"specimen coordinate systems\")\n",
    "csm_specimen.create_cs(\"thermocouple 1\", \"specimen\", coordinates=Q_([1, 1, 0], \"mm\"))\n",
    "csm_specimen.create_cs(\"thermocouple 2\", \"specimen\", coordinates=Q_([1, 4, 0], \"mm\"))\n",
    "csm_specimen"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_global = CoordinateSystemManager(\"root\", \"global coordinate systems\")\n",
    "csm_global.create_cs(\"specimen\", \"root\", coordinates=Q_([1, 2, 3], \"mm\"))\n",
    "csm_global.create_cs(\"robot head\", \"root\", coordinates=Q_([4, 5, 6], \"mm\"))\n",
    "csm_global"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we merge them all:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_global.merge(csm_robot)\n",
    "csm_global.merge(csm_specimen)\n",
    "csm_global"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "All coordinate systems are now collected in a single `CoordinateSystemManager`.\n",
    "We can extract the previously merged subsystems with the function `subsystems` property.\n",
    "It returns a list with the `CoordinateSystemManager` instances:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_list = csm_global.subsystems\n",
    "for sub_csm in csm_list:\n",
    "    print(sub_csm.name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Comparing the returned `CoordinateSystemManager` instances with the original ones shows that they are indeed identical:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for sub_csm in csm_list:\n",
    "    if sub_csm.name == \"robot coordinate systems\":\n",
    "        print(sub_csm == csm_robot)\n",
    "    else:\n",
    "        print(sub_csm == csm_specimen)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You might have noticed, that there is no `CoordinateSystemManager` for the scanner systems.\n",
    "The reason for this is that it is a subsystem of the robot coordinate systems and the `subsystems` property does not recursively extract nested subsystems.\n",
    "However, we can simply use `subsystems` a second time to get it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for sub_csm in csm_list:\n",
    "    if sub_csm.name == \"robot coordinate systems\":\n",
    "        print(sub_csm.subsystems[0].name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the `subsystems` property does not affect the state of the current `CoordinateSystemManager`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_global"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To remove all subsystems, you can call `remove_subsystems`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_global.remove_subsystems()\n",
    "csm_global"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Alternatively, if you want to decompose a `CoordinateSystemManager` instance into all its subsystems, you can use the `unmerge` function.\n",
    "It works exactly the same as the `subsystems` property with the difference that it also removes all subsystem data from the affected `CoordinateSystemManager` instance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Time dependent spatial data\n",
    "\n",
    ">This chapter covers an advanced topic and requires you to have a solid understanding of the previously discussed topics, especially the ones about time dependencies.\n",
    "Additionally, you should also be familiar with the `SpatialData` class.\n",
    "\n",
    "\n",
    "\n",
    "Time dependent spatial data is simply spatial data that gets another dimension that is associated with time.\n",
    "A possible way to utilize this would be for example to gather the different geometry states between multiple welding passes.\n",
    "\n",
    "However, in the context of the `CoordinateSystemManager`, it is used to construct a complex 3d geometry from multiple, time-dependent subsets.\n",
    "If you wonder what actual use-case this might have, think of a laser scanner that scans a geometry from multiple positions and angles.\n",
    "All the data is usually captured in the scanners own coordinate system.\n",
    "This means that all scans lie initially on top of each other.\n",
    "If we know the time-dependent coordinates and orientation of the scanner as well as the time at which each scan was taken, we can reconstruct the whole scanned object, given we know all the transformations to the scanned objects coordinate system.\n",
    "The `CoordinateSystemManager` does this automatically if you pass time-dependent `SpatialData` to it.\n",
    "\n",
    "To demonstrate how this is done, we will look at a small example.\n",
    "First we create the data of a 2d-scanner:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_scan_n = 20\n",
    "\n",
    "scan_width_in_mm = 4\n",
    "\n",
    "\n",
    "dx_scan = scan_width_in_mm / (num_scan_n - 1)\n",
    "scan_profile = np.array(\n",
    "    [\n",
    "        [\n",
    "            0,\n",
    "            i * dx_scan - scan_width_in_mm / 2,\n",
    "            np.sin(i * np.pi / (num_scan_n - 1)) - 2,\n",
    "        ]\n",
    "        for i in range(num_scan_n)\n",
    "    ]\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is just a simple, single sine-wave:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(scan_profile[:, 1], scan_profile[:, 2])\n",
    "plt.xlabel(\"y in mm\")\n",
    "plt.ylabel(\"z in mm\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now lets say we scanned 40 profiles over a period of 20 seconds.\n",
    "For simplicity, we reuse the same profile for every scan."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_scan_p = 40\n",
    "duration_scan_in_sec = 20\n",
    "dt_scan = duration_scan_in_sec / (num_scan_p - 1)\n",
    "time_p = Q_([i * dt_scan for i in range(num_scan_p)], \"s\")\n",
    "\n",
    "scan_time = Q_([i * dt_scan for i in range(num_scan_p)], \"s\")\n",
    "scan_data = Q_([scan_profile for _ in range(num_scan_p)], \"mm\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that we now have a 3 dimensional `scan_data` array with the outer dimension being time.\n",
    "The next one represents the individual points of a scan and the last one the 3 spatial coordinates:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "scan_data.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The content of the next cell is purely to get a better visualization and can be omitted.\n",
    "We create a list of triplets.\n",
    "Each of those tells the SpatialData the indices of the data points that form a triangle.\n",
    "This enables the `CoordinateSystemManager` to plot the spatial data as a closed surface instead of a point cloud."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "triangles = []\n",
    "for j in range(num_scan_p - 1):\n",
    "    for i in range(num_scan_n - 1):\n",
    "        offset = j * num_scan_n + i\n",
    "        triangles.append([offset, offset + num_scan_n, offset + 1])\n",
    "        triangles.append([offset + 1, offset + num_scan_n, offset + num_scan_n + 1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With all the data gathered, we can create our `SpatialData` class:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "scans = SpatialData(coordinates=scan_data, triangles=triangles, time=time_p)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we need a `CoordinateSystemManager` where we can attach the data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan = CoordinateSystemManager(\"base\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To get things a bit more interesting, let's say our specimen is placed on a table that rotates 180 degrees during the scan process.\n",
    "Note that we will use some unrealistically small distances so that the final plot is more compact for demonstration purposes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "table_rotation_degrees = 180\n",
    "num_timesteps_csm = 101\n",
    "\n",
    "dt_csm = duration_scan_in_sec / (num_timesteps_csm - 1)\n",
    "time_csm = Q_([i * dt_csm for i in range(num_timesteps_csm)], \"s\")\n",
    "\n",
    "\n",
    "deg_per_step = table_rotation_degrees / (num_timesteps_csm - 1)\n",
    "angles_table = [i * deg_per_step for i in range(num_timesteps_csm)]\n",
    "\n",
    "\n",
    "csm_scan.create_cs_from_euler(\n",
    "    \"table\",\n",
    "    \"base\",\n",
    "    sequence=\"z\",\n",
    "    angles=angles_table,\n",
    "    degrees=True,\n",
    "    coordinates=Q_([-2, -2, -2], \"mm\"),\n",
    "    time=time_csm,\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The specimen is placed at a certain offset to the tables center of rotation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan.create_cs(\"specimen\", \"table\", coordinates=Q_([-1, 3, 2], \"mm\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The scanner itself is mounted at a movable robot arm that has its own coordinate system which we call \"tcp\" (tool center point).\n",
    "During the scanning process, the robot arm performs a linear translation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_mm_per_step = -5 / (num_timesteps_csm - 1)\n",
    "coordinates_tcp = Q_(\n",
    "    [[3, i * tcp_mm_per_step + 12, 10] for i in range(num_timesteps_csm)], \"mm\"\n",
    ")\n",
    "\n",
    "csm_scan.create_cs(\"tcp\", \"base\", coordinates=coordinates_tcp, time=time_csm)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The scanners coordinate system has a fixed offset to the robot arms coordinate system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan.create_cs(\"scanner\", \"tcp\", coordinates=Q_([0, 0, 2], \"mm\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Our complete `CoordinateSystemManager` has the following structure:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that the coloured arrows represent time-dependent relations.\n",
    "\n",
    "Now let's recapitulate what we are going to do.\n",
    "We got a set of scan profiles of a specimen measured at different points in time in the scanner coordinate system.\n",
    "The object we scanned is static in the specimen coordinate system.\n",
    "So we tell the `CoordinateSystemManager` that the time-dependent data we want to add was measured in the scanner coordinate system but should be transformed into the specimen coordinate system.\n",
    "This is done as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan.assign_data(\n",
    "    scans, data_name=\"scans\", reference_system=\"scanner\", target_system=\"specimen\"\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For each time step of `SpatialData`, the `CoordinateSystemManager` now calculates the correct, time-dependent transformation from the scanner to the specimen coordinate system.\n",
    "Those transformations are then applied to the data before it is stored.\n",
    "Let's plot the `CoordinateSystemManager` and check if everything worked as expected."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm_scan.plot(backend=\"k3d\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To understand the plot, it is best to play the animation and see how everything moves in relation to each other.\n",
    "\n",
    "Due to the rotating table, we get a circular shape.\n",
    "Because the scanner was also performing a translation towards the center of rotation, we do also get a varying radius.\n",
    "Since we used the same profile for each time step, the resulting geometry has the same cross-section everywhere and it looks like the scanner is just following the peak of the geometry.\n",
    "\n",
    "Of cause, this is a very simplistic example, but it demonstrates how easy it is to reconstruct scan data with the `CoordinateSystemManager`, regardless of how complex the relative motions of the different involved coordinate systems are.\n",
    "Reconstructing real scan data wouldn't be any different."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "",
   "language": "python",
   "name": ""
  },
  "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.10.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}