{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7fefb950-9158-4c62-b593-cda353ff5db1",
   "metadata": {},
   "source": [
    "# Demo of `IdentityTransform`"
   ]
  },
  {
   "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 IdentityTransform\n",
    "except (ModuleNotFoundError, ImportError):\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 IdentityTransform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "120c8907-672f-4915-aa03-506dd91b1d18",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr = IdentityTransform(\n",
    "    (\"x\", \"y\"),\n",
    ")"
   ]
  },
  {
   "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 = 2.0 * figheight\n",
    "fig, axs = plt.subplots(2, 3, figsize=[figwidth, figheight])\n",
    "\n",
    "axs[0,0].set_xlabel(\"MSpace X\")\n",
    "axs[0,0].set_ylabel(\"MSpace Y\")\n",
    "axs[0,1].set_xlabel(\"Drive X\")\n",
    "axs[0,1].set_ylabel(\"Drive Y\")\n",
    "axs[0,2].set_xlabel(\"MSpace X\")\n",
    "axs[0,2].set_ylabel(\"MSpace Y\")\n",
    "\n",
    "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",
    "dpoints = tr(points, to_coords=\"drive\")\n",
    "mpoints = tr(dpoints, to_coords=\"motion_space\")\n",
    "\n",
    "axs[0,0].fill(points[...,0], points[...,1])\n",
    "axs[0,1].fill(dpoints[...,0], dpoints[...,1])\n",
    "axs[0,2].fill(mpoints[...,0], mpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    [\n",
    "        [-5, 5],\n",
    "        [-5, -5],\n",
    "        [5, -5],\n",
    "        [5, 5],\n",
    "        [0, 0]\n",
    "    ],\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    dpt = tr(pt, to_coords=\"drive\")\n",
    "    mpt = tr(dpt, to_coords=\"motion_space\")\n",
    "    print(pt, dpt, mpt)\n",
    "    axs[0,0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[0,1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n",
    "    axs[0,2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n",
    "\n",
    "##\n",
    "\n",
    "axs[1,0].set_xlabel(\"Drive X\")\n",
    "axs[1,0].set_ylabel(\"Drive Y\")\n",
    "axs[1,1].set_xlabel(\"MSpace X\")\n",
    "axs[1,1].set_ylabel(\"MSpace Y\")\n",
    "axs[1,2].set_xlabel(\"Drive X\")\n",
    "axs[1,2].set_ylabel(\"Drive Y\")\n",
    "\n",
    "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",
    "mpoints = tr(points, to_coords=\"motion_space\")\n",
    "dpoints = tr(mpoints, to_coords=\"drive\")\n",
    "\n",
    "axs[1,0].fill(points[...,0], points[...,1])\n",
    "axs[1,1].fill(mpoints[...,0], mpoints[...,1])\n",
    "axs[1,2].fill(dpoints[...,0], dpoints[...,1])\n",
    "\n",
    "for pt, color in zip(\n",
    "    [\n",
    "        [-5, 5],\n",
    "        [-5, -5],\n",
    "        [5, -5],\n",
    "        [5, 5],\n",
    "        [0, 0]\n",
    "    ],\n",
    "    [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n",
    "):\n",
    "    mpt = tr(pt, to_coords=\"motion_space\")\n",
    "    dpt = tr(mpt, to_coords=\"drive\")\n",
    "    axs[1,0].plot(pt[0], pt[1], 'o', color=color)\n",
    "    axs[1,1].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n",
    "    axs[1,2].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n",
    "    print(f\"X = {pt[0]}  Δ = {dpt[...,0] - pt[0]} || Y = {pt[1]}  Δ = {dpt[...,1] - pt[1]}\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3c7e7e77-c0d0-4df0-bbdf-a0729792c490",
   "metadata": {},
   "source": [
    "### Test Transforming `drive -> motion space -> drive`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a277810b-3553-4cdf-9fc9-dc0efab86cfc",
   "metadata": {},
   "outputs": [],
   "source": [
    "points = np.array([[5, 5], [5, 5]])\n",
    "mpoints = tr(points, to_coords=\"motion_space\")\n",
    "dpoints = tr(mpoints, to_coords=\"drive\")\n",
    "\n",
    "(\n",
    "    np.isclose(dpoints, points),\n",
    "    np.allclose(dpoints, points),\n",
    "    np.allclose(dpoints[...,0], points[...,0]),\n",
    "    np.allclose(dpoints[...,1], points[...,1]),\n",
    "    np.min(dpoints - points),\n",
    "    np.max(dpoints - points),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "be2cbc76-fd22-48d3-aeeb-b1650fa93f9a",
   "metadata": {},
   "source": [
    "### Test Transforming `motion space -> drive -> motion space`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8782f090-6ecb-4d25-8944-9cd1853be826",
   "metadata": {},
   "outputs": [],
   "source": [
    "dpoints = tr(points, to_coords=\"drive\")\n",
    "mpoints = tr(dpoints, to_coords=\"motion_space\")\n",
    "\n",
    "(\n",
    "    np.allclose(mpoints, points),\n",
    "    np.allclose(mpoints[...,0], points[...,0]),\n",
    "    np.allclose(mpoints[...,1], points[...,1]),\n",
    "    np.min(mpoints - points),\n",
    "    np.max(mpoints - points),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dea6120b-b00c-4828-83e8-68eff644a5e8",
   "metadata": {},
   "source": [
    "## Prototyping"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "48b03845-2742-46f7-91d4-6dfa28ef3b0c",
   "metadata": {},
   "outputs": [],
   "source": [
    "pts = [\n",
    "    [-5, 5],\n",
    "    [-5, -5],\n",
    "    [5, -5],\n",
    "    [5, 5],\n",
    "    [0, 0]\n",
    "]\n",
    "# pts = [[-5, 5]]\n",
    "\n",
    "pts = tr._condition_points(pts)\n",
    "matrix = tr.matrix(pts, to_coords=\"mspace\")\n",
    "pts = np.concatenate(\n",
    "    (pts, np.ones((pts.shape[0], 1))),\n",
    "    axis=1,\n",
    ")\n",
    "results = np.einsum(\"kmn,kn->km\", matrix, pts)[:-1,...]\n",
    "ii = 1\n",
    "# pts[ii, ...]\n",
    "(pts[ii,...], results[ii,...])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "875c4699-4202-46ef-8a9f-a70f09578a78",
   "metadata": {},
   "outputs": [],
   "source": [
    "matrix[ii, ...]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e3b27182-50e0-40a4-a575-ee61226774b9",
   "metadata": {},
   "outputs": [],
   "source": [
    "(\n",
    "    pts[ii, :-1],\n",
    "    tr(pts[ii, :-1], to_coords=\"mspace\"),\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5efa5851-deb7-49f6-9c27-bdf66f6e5bd0",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr(pts[ii, :-1], to_coords=\"mspace\")"
   ]
  }
 ],
 "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
}