{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "import json\n",
    "import os\n",
    "import pickle\n",
    "import sys\n",
    "\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from matplotlib import pyplot as plt\n",
    "\n",
    "from clang_interop.cl_types import ClContext\n",
    "from clang_interop.process_clang_output import process_clang_output\n",
    "\n",
    "sys.path.append(\"../autoware/build/tracetools_read/\")\n",
    "sys.path.append(\"../autoware/build/tracetools_analysis/\")\n",
    "from tracetools_read.trace import *\n",
    "from tracetools_analysis.loading import load_file\n",
    "from tracetools_analysis.processor.ros2 import Ros2Handler\n",
    "from tracetools_analysis.utils.ros2 import Ros2DataModelUtil\n",
    "\n",
    "from dataclasses import dataclass\n",
    "from typing import List, Dict, Set, Tuple\n",
    "\n",
    "from tracing_interop.tr_types import TrTimer, TrTopic, TrPublisher, TrPublishInstance, TrCallbackInstance, \\\n",
    "TrCallbackSymbol, TrCallbackObject, TrSubscriptionObject, TrContext\n",
    "from misc.utils import ProgressPrinter, cached"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "TR_PATH = os.path.expanduser(\"data/trace-awsim-x86/ust\")\n",
    "CL_PATH = os.path.expanduser(\"~/Projects/llvm-project/clang-tools-extra/ros2-internal-dependency-checker/output\")"
   ]
  },
  {
   "cell_type": "markdown",
   "source": [
    "# Organize Trace Data"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%% md\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "def _load_traces():\n",
    "    file = load_file(TR_PATH)\n",
    "    handler = Ros2Handler.process(file)\n",
    "    util = Ros2DataModelUtil(handler)\n",
    "    return TrContext(util, handler)\n",
    "\n",
    "\n",
    "_tracing_context = cached(\"tr_objects\", _load_traces, [TR_PATH])\n",
    "_tr_globals = [\"nodes\", \"publishers\", \"subscriptions\", \"timers\", \"timer_node_links\", \"subscription_objects\",\n",
    "               \"callback_objects\", \"callback_symbols\", \"publish_instances\", \"callback_instances\", \"topics\"]\n",
    "\n",
    "# Help the IDE recognize those identifiers\n",
    "nodes = publishers = subscriptions = timers = timer_node_links = subscription_objects = callback_objects = callback_symbols = publish_instances = callback_instances = topics = None\n",
    "\n",
    "for name in _tr_globals:\n",
    "    globals()[name] = getattr(_tracing_context, name)\n",
    "\n",
    "print(\"Done.\")\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "# ROS2 Tracing & Clang Matching"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%% md\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "def _load_cl_objects():\n",
    "    return process_clang_output(CL_PATH)\n",
    "\n",
    "\n",
    "_cl_context: ClContext = cached(\"cl_objects\", _load_cl_objects, [CL_PATH])"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "from matching.subscriptions import cl_deps_to_tr_deps, match\n",
    "\n",
    "matches, tr_unmatched, cl_unmatched = match(_tracing_context, _cl_context)\n",
    "tr_internal_deps = cl_deps_to_tr_deps(matches, _tracing_context, _cl_context)\n",
    "tr_cl_matches = {tup[1]: tup[0] for tup in matches}\n",
    "\n",
    "print(len(tr_internal_deps), sum(map(len, tr_internal_deps.values())))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "# E2E Latency Calculation"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%% md\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "from latency_graph import latency_graph as lg\n",
    "\n",
    "#lat_graph = lg.LatencyGraph(_tracing_context)\n",
    "\n",
    "import pickle\n",
    "\n",
    "#with open(\"lat_graph.pkl\", \"wb\") as f:\n",
    "#    pickle.dump(lat_graph, f)\n",
    "with open(\"lat_graph.pkl\", \"rb\") as f:\n",
    "    lat_graph = pickle.load(f)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "from matching.subscriptions import sanitize\n",
    "from typing import Iterable, Sized\n",
    "from tracing_interop.tr_types import TrNode, TrCallbackObject, TrCallbackSymbol, TrSubscriptionObject\n",
    "\n",
    "#################################################\n",
    "# Plot DFG\n",
    "#################################################\n",
    "\n",
    "# Compare with: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/node-diagram/\n",
    "node_colors = {\n",
    "        \"sensing\": {\"fill\": \"#e1d5e7\", \"stroke\": \"#9673a6\"},\n",
    "        \"localization\": {\"fill\": \"#dae8fc\", \"stroke\": \"#6c8ebf\"},\n",
    "        \"perception\": {\"fill\": \"#d5e8d4\", \"stroke\": \"#82b366\"},\n",
    "        \"planning\": {\"fill\": \"#fff2cc\", \"stroke\": \"#d6b656\"},\n",
    "        \"control\": {\"fill\": \"#ffe6cc\", \"stroke\": \"#d79b00\"},\n",
    "        \"system\": {\"fill\": \"#f8cecc\", \"stroke\": \"#b85450\"},\n",
    "        \"vehicle_interface\": {\"fill\": \"#b0e3e6\", \"stroke\": \"#0e8088\"},\n",
    "        None: {\"fill\": \"#f5f5f5\", \"stroke\": \"#666666\"}\n",
    "}\n",
    "\n",
    "node_namespace_mapping = {\n",
    "        'perception': 'perception',\n",
    "        'sensing': 'sensing',\n",
    "        'planning': 'planning',\n",
    "        'control': 'control',\n",
    "        'awapi': None,\n",
    "        'autoware_api': None,\n",
    "        'map': None,\n",
    "        'system': 'system',\n",
    "        'localization': 'localization',\n",
    "        'robot_state_publisher': None,\n",
    "        'aggregator_node': None,\n",
    "        'pointcloud_container': 'sensing',\n",
    "}\n",
    "\n",
    "import graphviz as gv\n",
    "\n",
    "g = gv.Digraph('G', filename=\"latency_graph.gv\",\n",
    "               node_attr={'shape': 'plain'},\n",
    "               graph_attr={'pack': '1'})\n",
    "g.graph_attr['rankdir'] = 'LR'\n",
    "\n",
    "def plot_hierarchy(gv_parent, lg_node: lg.LGHierarchyLevel, **subgraph_kwargs):\n",
    "    if lg_node.name == \"[NONE]\":\n",
    "        return\n",
    "\n",
    "    print(f\"{'  ' * lg_node.full_name.count('/')}Processing {lg_node.name}: {len(lg_node.callbacks)}\")\n",
    "    with gv_parent.subgraph(name=f\"cluster_{lg_node.full_name.replace('/', '__')}\", **subgraph_kwargs) as c:\n",
    "        c.attr(label=lg_node.name)\n",
    "        for cb in lg_node.callbacks:\n",
    "            if isinstance(cb, lg.LGTrCallback):\n",
    "                tr_cb = cb.cb\n",
    "                try:\n",
    "                    sym = _tracing_context.callback_symbols.get(tr_cb.callback_object)\n",
    "                    pretty_sym = repr(sanitize(sym.symbol)).replace(\"&\", \"&amp;\").replace(\"<\", \"&lt;\").replace(\">\", \"&gt;\")\n",
    "                except KeyError:\n",
    "                    pretty_sym = cb.name\n",
    "                except TypeError:\n",
    "                    pretty_sym = cb.name\n",
    "            else:\n",
    "                pretty_sym = cb.name\n",
    "            c.node(cb.id(),\n",
    "                   f'<<table BORDER=\"0\" CELLBORDER=\"1\" CELLSPACING=\"0\"><tr><td port=\"in\"></td><td>{pretty_sym}</td><td port=\"out\"></td></tr></table>>')\n",
    "\n",
    "        for ch in lg_node.children:\n",
    "            plot_hierarchy(c, ch, **subgraph_kwargs)\n",
    "\n",
    "def plot_lg(graph: lg.LatencyGraph):\n",
    "    for top_level_node in graph.top_node.children:\n",
    "        colors = node_colors[node_namespace_mapping.get(top_level_node.name)]\n",
    "        plot_hierarchy(g, top_level_node, graph_attr={'bgcolor': colors[\"fill\"], 'pencolor': colors[\"stroke\"]})\n",
    "\n",
    "    for edge in graph.edges:\n",
    "        g.edge(f\"{edge.start.id()}:out\", f\"{edge.end.id()}:in\")\n",
    "\n",
    "plot_lg(lat_graph)\n",
    "\n",
    "g.save(\"latency_graph.gv\")\n",
    "g.render(\"latency_graph.svg\")\n",
    "\n",
    "g"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "##################################################\n",
    "# Compute in/out topics for hierarchy level X\n",
    "##################################################\n",
    "\n",
    "HIER_LEVEL = 2\n",
    "\n",
    "def get_nodes_on_level(lat_graph: lg.LatencyGraph):\n",
    "    def _traverse_node(node: lg.LGHierarchyLevel, cur_lvl=0):\n",
    "        if cur_lvl == HIER_LEVEL:\n",
    "            return [node]\n",
    "\n",
    "        if not node.children and cur_lvl < HIER_LEVEL:\n",
    "            return [node]\n",
    "\n",
    "        collected_nodes = []\n",
    "        for ch in node.children:\n",
    "            collected_nodes += _traverse_node(ch, cur_lvl + 1)\n",
    "        return collected_nodes\n",
    "\n",
    "    return _traverse_node(lat_graph.top_node)\n",
    "\n",
    "lvl_nodes = get_nodes_on_level(lat_graph)\n",
    "lvl_nodes = [n for n in lvl_nodes if \"transform_listener_impl\" not in n.full_name]\n",
    "\n",
    "print(', '.join(map(lambda n: n.full_name, lvl_nodes)))\n",
    "\n",
    "def _collect_callbacks(n: lg.LGHierarchyLevel):\n",
    "    callbacks = []\n",
    "    callbacks += n.callbacks\n",
    "    for ch in n.children:\n",
    "        callbacks += _collect_callbacks(ch)\n",
    "    return callbacks\n",
    "\n",
    "\n",
    "cb_to_node_map = {}\n",
    "for n in lvl_nodes:\n",
    "    cbs = _collect_callbacks(n)\n",
    "    for cb in cbs:\n",
    "        cb_to_node_map[cb.id()] = n\n",
    "\n",
    "\n",
    "edges_between_nodes = {}\n",
    "for edge in lat_graph.edges:\n",
    "    from_node = cb_to_node_map.get(edge.start.id())\n",
    "    to_node = cb_to_node_map.get(edge.end.id())\n",
    "\n",
    "    if from_node is None or to_node is None:\n",
    "        continue\n",
    "\n",
    "    if from_node.full_name == to_node.full_name:\n",
    "        continue\n",
    "\n",
    "    k = (from_node.full_name, to_node.full_name)\n",
    "\n",
    "    if k not in edges_between_nodes:\n",
    "        edges_between_nodes[k] = 0\n",
    "\n",
    "    edges_between_nodes[k] += 1\n",
    "\n",
    "g = gv.Digraph('G', filename=\"latency_graph.gv\",\n",
    "               node_attr={'shape': 'plain'},\n",
    "               graph_attr={'pack': '1'})\n",
    "g.graph_attr['rankdir'] = 'LR'\n",
    "\n",
    "for n in lvl_nodes:\n",
    "    colors = node_colors[node_namespace_mapping.get(n.full_name.strip(\"/\").split(\"/\")[0])]\n",
    "    g.node(n.full_name, label=n.full_name, fillcolor=colors[\"fill\"], color=colors[\"stroke\"], shape=\"box\", style=\"filled\")\n",
    "\n",
    "for (src_name, dst_name), cnt in edges_between_nodes.items():\n",
    "    print(src_name, dst_name, cnt)\n",
    "    g.edge(src_name, dst_name, weight=str(cnt))\n",
    "\n",
    "g.save(\"level_graph.gv\")\n",
    "g.render(\"level_graph.svg\")\n",
    "\n",
    "g"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}