{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# An example of searching through a database of segments for a specific condition, and plotting the results.\n",
    "\n",
    "segments = [\n",
    "  \"c3d1ccb52f5f9d65|2023-07-22--01-23-20/6:10\",\n",
    "]\n",
    "platform = \"SUBARU OUTBACK 6TH GEN\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import copy\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "from opendbc.can.parser import CANParser\n",
    "\n",
    "from openpilot.selfdrive.car.subaru.values import CanBus, DBC\n",
    "from openpilot.tools.lib.logreader import LogReader\n",
    "\n",
    "\"\"\"\n",
    "In this example, we search for positive transitions of Steer_Warning, which indicate that the EPS\n",
    "has stopped responding to our messages. This analysis would allow you to find the cause of these\n",
    "steer warnings and potentially work around them.\n",
    "\"\"\"\n",
    "\n",
    "for segment in segments:\n",
    "  lr = LogReader(segment)\n",
    "\n",
    "  can_msgs = [msg for msg in lr if msg.which() == \"can\"]\n",
    "\n",
    "  messages = [\n",
    "    (\"Steering_Torque\", 50)\n",
    "  ]\n",
    "\n",
    "  cp = CANParser(DBC[platform][\"pt\"], messages, CanBus.main)\n",
    "\n",
    "  steering_torque_history = []\n",
    "  examples = []\n",
    "\n",
    "  for msg in can_msgs:\n",
    "    cp.update_strings([msg.as_builder().to_bytes()])\n",
    "    steering_torque_history.append(copy.copy(cp.vl[\"Steering_Torque\"]))\n",
    "  \n",
    "  steer_warning_last = False\n",
    "  for i, steering_torque_msg in enumerate(steering_torque_history):\n",
    "    steer_warning = steering_torque_msg[\"Steer_Warning\"]\n",
    "\n",
    "    steer_angle = steering_torque_msg[\"Steering_Angle\"]\n",
    "\n",
    "    if steer_warning and not steer_warning_last: # positive transition of \"Steer_Warning\"\n",
    "      examples.append(i)\n",
    "\n",
    "    steer_warning_last = steer_warning\n",
    "\n",
    "  FRAME_DELTA = 100 # plot this many frames around the positive transition\n",
    "\n",
    "  for example in examples:\n",
    "    fig, axs = plt.subplots(2)\n",
    "\n",
    "    min_frame = int(example-FRAME_DELTA/2)\n",
    "    max_frame = int(example+FRAME_DELTA/2)\n",
    "\n",
    "    steering_angle_history = [msg[\"Steering_Angle\"] for msg in steering_torque_history[min_frame:max_frame]]\n",
    "    steering_warning_history = [msg[\"Steer_Warning\"] for msg in steering_torque_history[min_frame:max_frame]]\n",
    "\n",
    "    xs = np.arange(-FRAME_DELTA/2, FRAME_DELTA/2)\n",
    "\n",
    "    axs[0].plot(xs, steering_angle_history)\n",
    "    axs[0].set_ylabel(\"Steering Angle (deg)\")\n",
    "    axs[1].plot(xs, steering_warning_history)\n",
    "    axs[1].set_ylabel(\"Steer Warning\")\n",
    "\n",
    "    plt.show()\n"
   ]
  }
 ],
 "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.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}