{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7fefb950-9158-4c62-b593-cda353ff5db1",
   "metadata": {},
   "source": [
    "# Demo of `LaPD6KTransform`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1bef64d2-1541-4dec-ac10-ebcf4cffe4b2",
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63c23fe0-5407-40b9-a998-6f1581d6eb6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import sys\n",
    "\n",
    "plt.rcParams[\"figure.figsize\"] = [10.5, 0.56 * 10.5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9e25f18e-6ce0-48b2-82ac-27c69b006a29",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from bapsf_motion.transform import LaPD6KTransform\n",
    "except ModuleNotFoundError:\n",
    "    from pathlib import Path\n",
    "\n",
    "    HERE = Path().cwd()\n",
    "    BAPSF_MOTION = (HERE / \"..\" / \"..\" / \"..\" ).resolve()\n",
    "    sys.path.append(str(BAPSF_MOTION))\n",
    "    \n",
    "    from bapsf_motion.transform import LaPD6KTransform"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f79461eb-d3a0-48bc-9a1e-13c6e5fee6db",
   "metadata": {},
   "source": [
    "General input keyword arguments to use for the demo."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15dd3644-c856-46c2-9f6f-846956b435d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "input_kwargs = {\n",
    "    \"pivot_to_center\": 57.288,\n",
    "    \"pivot_to_drive\": 134.0,\n",
    "    \"pivot_to_feedthru\": 21.6,\n",
    "    \"probe_axis_offset\": 20.1,\n",
    "    \"droop_correct\": False,\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "af05b4fc-afe4-4a5e-81a5-d11d95f022a4",
   "metadata": {},
   "source": [
    "## Transform from Motion Space to Drive Space to Motion Space\n",
    "\n",
    "Let's show the transform can successfully convert from the motion space to the drive space, and back.\n",
    "\n",
    "Instantiate the transform class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "768e0851-57e5-4b44-9f0f-541741376aeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr = LaPD6KTransform((\"x\", \"y\"), **input_kwargs)\n",
    "tr.config"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f540f0d3-4ce5-4f63-885e-1ca1584142a9",
   "metadata": {},
   "source": [
    "Construct a set of points in the motion space to convert."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea80e434-af3a-42fe-b590-b4744f382ec5",
   "metadata": {},
   "outputs": [],
   "source": [
    "points = np.zeros((40, 2))\n",
    "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "points[0:10, 1] = 5 * np.ones(10)\n",
    "points[10:20, 0] = 5 * np.ones(10)\n",
    "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 1] = -5 * np.ones(10)\n",
    "points[30:40, 0] = -5 * np.ones(10)\n",
    "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "\n",
    "key_points = np.array(\n",
    "    [\n",
    "        [-5, 5],\n",
    "        [-5, -5],\n",
    "        [5, -5],\n",
    "        [5, 5],\n",
    "        [0, 0]\n",
    "    ],\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de26ee8e-3926-4f2a-910f-a800b7cdb152",
   "metadata": {},
   "source": "Calculate the drive space points `dpoints` and return to motion space points `mpoints`."
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d03325f-6e57-445c-9717-19fb98ea29d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "dpoints = tr(points, to_coords=\"drive\")\n",
    "mpoints = tr(dpoints, to_coords=\"motion_space\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4eaa3771-3571-4683-aae8-bd8d2f80b96e",
   "metadata": {},
   "source": [
    "Plot the transform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a0c8f222-15a8-45f5-b20e-ec475ba3d854",
   "metadata": {},
   "outputs": [],
   "source": [
    "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n",
    "figwidth = 1.4 * figwidth\n",
    "figheight = figheight\n",
    "fig, axs = plt.subplots(1, 3, figsize=[figwidth, figheight])\n",
    "\n",
    "axs[0].set_title(\"Motion Space\")\n",
    "axs[1].set_title(\"Drive Space\")\n",
    "axs[2].set_title(\"Motion Space Return\")\n",
    "\n",
    "for ii in range(3):\n",
    "    axs[ii].set_xlabel(\"X\")\n",
    "    axs[ii].set_ylabel(\"Y\")\n",
    "\n",
    "axs[0].fill(points[...,0], points[...,1])\n",
    "axs[1].fill(dpoints[...,0], dpoints[...,1])\n",
    "axs[2].fill(mpoints[...,0], mpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    key_points.tolist(),\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    dpt = tr(pt, to_coords=\"drive\")\n",
    "    mpt = tr(dpt, to_coords=\"motion_space\")\n",
    "    axs[0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n",
    "    axs[2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d95b6680-c8df-4be5-8b8e-a8915f553ad8",
   "metadata": {},
   "source": [
    "Are the returned motion space points \"identical\" to the starting points?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "76f1c97e-a944-4d8b-91a6-9cc328671b5f",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.allclose(points, mpoints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4449700-7100-4c29-946c-4d22cea7ee0f",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.max(np.abs(points - mpoints))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bd3dd84b-5a9a-48c1-b304-c62f84976b7a",
   "metadata": {},
   "source": [
    "## Transform from Drive Sapce to Motion Space to Drive Space\n",
    "\n",
    "Let's show the transform can successfully convert from the drive space to the motion space, and back.\n",
    "\n",
    "Using the same transform and initial points in the previous section, lets construct the motion space points `mpoints` and return to drive space points `dpoints`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e87c5d28-ec61-4a90-97cd-89db765fbd2c",
   "metadata": {},
   "outputs": [],
   "source": [
    "mpoints = tr(points, to_coords=\"motion_space\")\n",
    "dpoints = tr(mpoints, to_coords=\"drive\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3529a742-cc81-44b9-ac1d-e3cf9e3cbc9a",
   "metadata": {},
   "source": [
    "Plot the transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e61105e-3788-4edb-a54c-9a582f04f745",
   "metadata": {},
   "outputs": [],
   "source": [
    "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n",
    "figwidth = 1.4 * figwidth\n",
    "figheight = figheight\n",
    "fig, axs = plt.subplots(1, 3, figsize=[figwidth, figheight])\n",
    "\n",
    "axs[0].set_title(\"Drive Space\")\n",
    "axs[1].set_title(\"Motion Space\")\n",
    "axs[2].set_title(\"Drive Space Return\")\n",
    "\n",
    "for ii in range(3):\n",
    "    axs[ii].set_xlabel(\"X\")\n",
    "    axs[ii].set_ylabel(\"Y\")\n",
    "\n",
    "axs[0].fill(points[...,0], points[...,1])\n",
    "axs[1].fill(mpoints[...,0], mpoints[...,1])\n",
    "axs[2].fill(dpoints[...,0], dpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    key_points.tolist(),\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    mpt = tr(pt, to_coords=\"motion_space\")\n",
    "    dpt = tr(mpt, to_coords=\"drive\")\n",
    "    axs[0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[1].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n",
    "    axs[2].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4b0115f5-6e20-41d8-ae82-72452a1a831d",
   "metadata": {},
   "source": [
    "Are the returned drive space points \"identical\" to the starting points?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "054364ac-4e07-40f9-ada2-a3677c8c7404",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.allclose(points, dpoints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57e32b70-3829-4f29-93a8-5549b3e0daef",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.max(np.abs(points - dpoints))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a6ac8c55-eca5-44f7-9579-e7b61bdab6f0",
   "metadata": {},
   "source": [
    "## Transform Can Droop Correct\n",
    "\n",
    "The transform `LaPD6KTransform` also incorporates droop correction via the `LaPDXYDroopCorrect` class.\n",
    "\n",
    "Instantiate the transform with droop correction enabled."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1e2e4537-cb72-4d9a-b457-6f3160771261",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr = LaPD6KTransform(\n",
    "    (\"x\", \"y\"),\n",
    "    **{\n",
    "        **input_kwargs,\n",
    "        \"droop_correct\": True,\n",
    "        \"droop_scale\": 2.0,\n",
    "    },\n",
    ")\n",
    "tr.config"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ac8d5ed5-c6e1-4632-9806-63d51d57fc63",
   "metadata": {},
   "source": [
    "Construct a set of points for the transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1a4a8740-5b79-45cc-b67f-311d72caeb66",
   "metadata": {},
   "outputs": [],
   "source": [
    "points = np.zeros((40, 2))\n",
    "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "points[0:10, 1] = 5 * np.ones(10)\n",
    "points[10:20, 0] = 5 * np.ones(10)\n",
    "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 1] = -5 * np.ones(10)\n",
    "points[30:40, 0] = -5 * np.ones(10)\n",
    "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "\n",
    "key_points = np.array(\n",
    "    [\n",
    "        [-5, 5],\n",
    "        [-5, -5],\n",
    "        [5, -5],\n",
    "        [5, 5],\n",
    "        [0, 0]\n",
    "    ],\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3dc12ae7-0ef0-4eba-a652-e4980d76a61a",
   "metadata": {},
   "source": "Calculate the drive space points `dpoints` and return to motion space points`mpoints`."
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2b9dc74c-298b-46db-b06b-8fe81d29b936",
   "metadata": {},
   "outputs": [],
   "source": [
    "dpoints = tr(points, to_coords=\"drive\")\n",
    "mpoints = tr(dpoints, to_coords=\"motion_space\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "25f03021-a4f1-493c-b5f9-39b675857c49",
   "metadata": {},
   "source": [
    "Plot the transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0aafc667-d47e-4a3f-8f18-646b599bfc53",
   "metadata": {},
   "outputs": [],
   "source": [
    "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n",
    "figwidth = 1.4 * figwidth\n",
    "figheight = figheight\n",
    "fig, axs = plt.subplots(1, 3, figsize=[figwidth, figheight])\n",
    "\n",
    "axs[0].set_title(\"Motion Space\")\n",
    "axs[1].set_title(\"Drive Space\")\n",
    "axs[2].set_title(\"Motion Space Return\")\n",
    "\n",
    "for ii in range(3):\n",
    "    axs[ii].set_xlabel(\"X\")\n",
    "    axs[ii].set_ylabel(\"Y\")\n",
    "\n",
    "axs[0].fill(points[...,0], points[...,1])\n",
    "axs[1].fill(dpoints[...,0], dpoints[...,1])\n",
    "axs[2].fill(mpoints[...,0], mpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    key_points.tolist(),\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    dpt = tr(pt, to_coords=\"drive\")\n",
    "    mpt = tr(dpt, to_coords=\"motion_space\")\n",
    "    axs[0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n",
    "    axs[2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c7293afa-e931-499d-84ad-f7f3f7c696e4",
   "metadata": {},
   "source": [
    "Are the returned motion space points \"identical\" to the starting points?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8dac0d50-a14d-4319-8630-fa9c1a750f09",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.allclose(points, mpoints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4995cb9b-7c5e-4660-95c1-e1fbe2b73283",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.max(np.abs(points - mpoints))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8d84d8dd-8ec5-4c64-9696-3162096150f8",
   "metadata": {},
   "source": [
    "## Configure for West Side Deployment\n",
    "\n",
    "The default values for `LaPD6KTransform` is for an East side deployment on the LaPD.  However, the transform can be configured for a West side deployment by using a negative `pivot_to_center` and `[1, 1]` for the `mspace_polarity`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "82aa413b-6546-4cbc-9115-125ffcc107ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr = LaPD6KTransform(\n",
    "    (\"x\", \"y\"),\n",
    "    **{\n",
    "        **input_kwargs,\n",
    "        \"pivot_to_center\": -58.771,\n",
    "        \"mspace_polarity\": [1, 1],\n",
    "        \"droop_correct\": True,\n",
    "        \"droop_scale\": 2.0,\n",
    "    },\n",
    ")\n",
    "tr.config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d5853946-24e3-4286-aae6-aa48a59af280",
   "metadata": {},
   "outputs": [],
   "source": [
    "points = np.zeros((40, 2))\n",
    "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "points[0:10, 1] = 5 * np.ones(10)\n",
    "points[10:20, 0] = 5 * np.ones(10)\n",
    "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n",
    "points[20:30, 1] = -5 * np.ones(10)\n",
    "points[30:40, 0] = -5 * np.ones(10)\n",
    "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n",
    "\n",
    "key_points = np.array(\n",
    "    [\n",
    "        [-5, 5],\n",
    "        [-5, -5],\n",
    "        [5, -5],\n",
    "        [5, 5],\n",
    "        [0, 0]\n",
    "    ],\n",
    ")\n",
    "\n",
    "dpoints = tr(points, to_coords=\"drive\")\n",
    "mpoints = tr(dpoints, to_coords=\"motion_space\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0059a4f-f084-4d16-921c-6d2dd79fa3e5",
   "metadata": {},
   "source": [
    "Plot the transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "024c67c5-b442-40ee-8cd4-3c57a9c9e21a",
   "metadata": {},
   "outputs": [],
   "source": [
    "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n",
    "figwidth = 1.4 * figwidth\n",
    "figheight = figheight\n",
    "fig, axs = plt.subplots(1, 3, figsize=[figwidth, figheight])\n",
    "\n",
    "axs[0].set_title(\"Motion Space\")\n",
    "axs[1].set_title(\"Drive Space\")\n",
    "axs[2].set_title(\"Motion Space Return\")\n",
    "\n",
    "for ii in range(3):\n",
    "    axs[ii].set_xlabel(\"X\")\n",
    "    axs[ii].set_ylabel(\"Y\")\n",
    "\n",
    "axs[0].fill(points[...,0], points[...,1])\n",
    "axs[1].fill(dpoints[...,0], dpoints[...,1])\n",
    "axs[2].fill(mpoints[...,0], mpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    key_points.tolist(),\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    dpt = tr(pt, to_coords=\"drive\")\n",
    "    mpt = tr(dpt, to_coords=\"motion_space\")\n",
    "    axs[0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n",
    "    axs[2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4613643d-b83f-4f80-9c58-3053d48b43ad",
   "metadata": {},
   "source": [
    "Are the returned motion space points \"identical\" to the starting points?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62a2a57c-f919-4ae1-8fc3-1923c54d8c49",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.allclose(points, mpoints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "627126b3-419e-4a1b-8c28-7a45499709c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.max(np.abs(points - mpoints))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8165d9e8-797e-4f76-885b-fabd164081c4",
   "metadata": {
    "tags": []
   },
   "source": [
    "## The Algorithms\n",
    "\n",
    "To start we will use $(e_0, e_1)$ to represent the drive space coordinates and $(x, y)$ to represent the motion space coordinates.\n",
    "\n",
    "<figure>\n",
    "<img \n",
    "     src=\"LaPD6KTransform_space_relation_cartoon.png\"\n",
    "     style=\"width:100%; margin-left:auto; margin-right:auto; display:block\"\n",
    "     alt=\"top_level_cartoon\">\n",
    "<figcaption style=\"text-align:center\"> Top-Level Cartoon of the Drive and Motion Space Relationship </figcaption>\n",
    "</figure>\n",
    "\n",
    "**Note:**  The motion space x-axis points towards the the LaPD -X when the probe drive is deployed on the East side of the machine.  This is why the East side operation requires `mspace_polarity = [-1, 1]`, and the West side requires `mspace_polarity = [1, 1]`."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8c724e04-242d-4b3a-be03-f8c4dee2fffa",
   "metadata": {
    "tags": []
   },
   "source": [
    "### Algorithm: Drive to Motion Space\n",
    "\n",
    "The key parameter we need to determine to convert the drive space coordinates to the motion space coordinates is the angle $\\theta$, which is the angle the probe shaft makes with the horizontal.  Let's consider the following diagram...\n",
    "\n",
    "<figure class=center>\n",
    "<img \n",
    "     src=\"LaPD6KTransform_drive_space_overview.png\"\n",
    "     style=\"width:60%; margin-left:auto; margin-right:auto; display:block\"\n",
    "     alt=\"drive_overview\"\n",
    "     >\n",
    "<figcaption style=\"text-align:center\"> Drive Space Overview </figcaption>\n",
    "</figure>\n",
    "\n",
    "Here...\n",
    "\n",
    "- $d_o$ = `probe_axis_offset` which is the perpendicular distance from the probe axis to the pinion location on the horizontal arm of the 6K probe drive.\n",
    "- $R_A$ = `six_k_arm_length` which is the length of the vertical hanging arm of the 6K probe drive.\n",
    "- $\\beta$ is the angular drop of the pinion location from the probe drive shaft with respect to the ball valve\n",
    "\n",
    "  $$\n",
    "  tan\\,\\beta = \\frac{d_o}{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive}}=\\frac{\\texttt{probe}\\_\\texttt{axis}\\_\\texttt{offset}}{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive}}\n",
    "  $$\n",
    "\n",
    "- $R_P$ = `pivot_to_drive_pinion` the radial distance of the probe drive pinion from the ball valve pivot\n",
    "  \n",
    "  $$\n",
    "  R_P^2 = d_o^2 + \\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive}^2\n",
    "  $$\n",
    "  \n",
    "- The vertical pinoin location above the horizontal is given by $R_A - d_o + e_1$, assuming $e_1=0$ when the probe shaft is horizontal.\n",
    "- $\\gamma$ is the angle the vertical pinion makes with respect to the ball valve pivot and the horizontal\n",
    "\n",
    "  $$\n",
    "  \\tan\\,\\gamma = \\frac{R_A - d_o + e_1}{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive}}\n",
    "  $$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "979114a2-267d-4142-a6ae-d1ad13c480eb",
   "metadata": {},
   "source": [
    "Now adopt a reference frame where the line intersecting the ball valve pivot and probe drive vertical (grey dashed above) is rotated to the horizontal and the ball valve is the origin.  In this reference frame will use a coordinate system $(s_0, s_1)$.  In this system the pinion point is located at the intersection of two circles:\n",
    "\n",
    "1. The circle about the ball valve pivot of radius $R_P$.\n",
    "    \n",
    "    $$\n",
    "    R_P^2 = s_0^2 + s_1^2\n",
    "    $$\n",
    "    \n",
    "2. The circle about the vertical pinion of radius $R_A$.\n",
    "\n",
    "    $$\n",
    "    R_A^2 = (s_0 + L)^2 + s_1^2\\\\\n",
    "    \\text{where}\\; L^2 = (R_A - d_o + e_1)^2 + \\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive}^2\n",
    "    $$\n",
    "\n",
    "Solving this system of equations we can calculate the location of the pinion and, thus, the angle $\\phi$ depicted in the Drive Space Overview figure.\n",
    "\n",
    "$$\n",
    "\\tan^2 \\phi = \\left(\\frac{2\\,L\\,R_P}{R_A^2-R_P^2-L^2}\\right)^2 - 1\n",
    "$$\n",
    "\n",
    "Knowing $\\phi$, the signed angle $\\theta$ can be expressed as\n",
    "\n",
    "$$\n",
    "\\theta = \\gamma +|\\beta|-|\\phi|\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ff994b3b-3eca-498d-8b60-38129b83970c",
   "metadata": {},
   "source": [
    "Taking $\\theta$ and the radial projection of the probe into the motion space as $r=D_C + e_0$, where $D_C$ is the distans from the ball valve pivot to the motion space origin `pivot_to_center`, then the motion space coordinates can be expressed as\n",
    "\n",
    "$$\n",
    "x = (\\cos\\theta) \\, e_0 + D_C \\,(\\cos\\theta-1)\\\\\n",
    "y = (-\\sin\\theta)\\, e_0 - D_C \\,\\sin\\theta\n",
    "$$\n",
    "\n",
    "and expressed as the `_matrix_to_motion_space`\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "    x \\\\ y \\\\ 1\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "    \\cos\\theta & 0 & D_C \\, (\\cos\\theta - 1)\\\\\n",
    "    -\\sin\\theta & 0 & -D_C \\, \\sin\\theta\\\\\n",
    "    0 & 0 & 1\\\\\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "    e_0 \\\\ e_1 \\\\ 1\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9a2d6321-c59d-4771-9676-26600d4acf33",
   "metadata": {},
   "source": "Obviously this not a perfectly clean expression since $\\theta$ depends on $e_1$.  However, this is the expression that must be used to work with the archatecture designed int `BaseTransform`."
  },
  {
   "cell_type": "markdown",
   "id": "1c8d4e25-6b31-4acd-8070-610c9b87d829",
   "metadata": {},
   "source": [
    "### Algorithm: Motion to Drive Space\n",
    "\n",
    "In order to convert from the motion space to the drive space the key parameter to determine is the location of the probe drive pinion.  Again this boils down to determining the angle $\\theta$, since the pinion is always a distance $R_P$ from the ball valve pivot and at an angle of $\\theta - |\\beta|$.\n",
    "\n",
    "Knowing the motion space coordinates $(x, y)$ the angle $\\theta$ can be written as..\n",
    "\n",
    "$$\n",
    "\\tan\\theta = -\\frac{y}{D_C + x}\n",
    "$$\n",
    "\n",
    "Then the probe drive pinion is located at the following position with the ball valve coordinate system $(s_0, s_1)$\n",
    "\n",
    "$$\n",
    "s_0 = -R_P \\cos(\\theta - |\\beta|)\\\\\n",
    "s_1 = R_P \\sin(\\theta - |\\beta|)\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d24296ce-b36f-47f9-afa0-a8a25cedf8f4",
   "metadata": {},
   "source": [
    "Now we can determine the angle $\\alpha$ in which the probe drive arm leans forward.\n",
    "\n",
    "$$\n",
    "\\sin\\alpha = \\frac{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive} + s_0}{R_A}\n",
    "= \\frac{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive} - R_P \\cos(\\theta - |\\beta|)}{R_A}\n",
    "$$\n",
    "$$\n",
    "\\cos\\alpha = \\frac{R_A - d_o + e_1 - s_1}{R_A}\n",
    "= \\frac{R_A - d_o + e_1 - R_P \\sin(\\theta - |\\beta|)}{R_A}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e9ae34a6-c658-415c-8950-5c28357b497a",
   "metadata": {},
   "source": [
    "Now we can cast the drive space coordinates as\n",
    "\n",
    "$$\n",
    "e_0 = \\frac{1}{\\cos\\theta}x + D_C\\left(\\frac{1}{\\cos\\theta}-1\\right)\\\\\n",
    "e_1 = R_A (\\cos\\alpha - 1) + d_o + R_P \\sin(\\theta - |\\beta|)\n",
    "$$\n",
    "\n",
    "where\n",
    "\n",
    "$$\n",
    "\\sin\\alpha = \\frac{\\texttt{pivot}\\_\\texttt{to}\\_\\texttt{drive} - R_P \\cos(\\theta - |\\beta|)}{R_A}\\\\\n",
    "\\tan\\theta = -\\frac{y}{D_C + x}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "70dde653-1b6a-4879-a5b3-daf15d212968",
   "metadata": {},
   "source": [
    "The does yield a rather ugly, but functional, transformation matrix of\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "    e_0 \\\\ e_1 \\\\ 1\n",
    "\\end{bmatrix}\n",
    "=\n",
    "\\begin{bmatrix}\n",
    "    \\frac{1}{\\cos\\theta} & 0 & D_C \\, \\left(\\frac{1}{\\cos\\theta} - 1\\right)\\\\\n",
    "    0 & 0 & R_A (\\cos\\alpha - 1) + d_o + R_P \\sin(\\theta - |\\beta|)\\\\\n",
    "    0 & 0 & 1\\\\\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "    x \\\\ y \\\\ 1\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  }
 ],
 "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.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}