{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "thanks to -> https://stackoverflow.com/questions/49558464/shrink-polygon-using-corner-coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.8500000000000001, 6.85, -1.3744678440936948, 11.065450849718747)"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 15560 characters]\n",
      "text/plain": [
       "<Figure size 1080x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "\n",
    "def shrink_polygon(coords, shrink_value_x, shrink_value_y):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    def det(a, b):\n",
    "        return a[0] * b[1] - a[1] * b[0]\n",
    "\n",
    "    def line_intersection(line1, line2):\n",
    "        xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])\n",
    "        ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])  # Typo was here\n",
    "\n",
    "        div = det(xdiff, ydiff)\n",
    "        if div == 0:\n",
    "            raise Exception(\"lines do not intersect\")\n",
    "\n",
    "        d = (det(*line1), det(*line2))\n",
    "        x = det(d, xdiff) / div\n",
    "        y = det(d, ydiff) / div\n",
    "        return x, y\n",
    "\n",
    "    # how much the coordinates are moved as an absolute value\n",
    "    # shrink_value_x = 2\n",
    "    # shrink_value_y = 2\n",
    "\n",
    "    # coords must be clockwise\n",
    "    # coords = [(0, 0), (0, 100), (20, 100), (30, 60), (40, 100), (60, 100), (60, 0), (40, 10), (40, 40), (20, 40), (20, 10)]\n",
    "\n",
    "    lines = [[coords[i - 1], coords[i]] for i in range(len(coords))]\n",
    "\n",
    "    new_lines = []\n",
    "    for i in lines:\n",
    "        dx = i[1][0] - i[0][0]\n",
    "        dy = i[1][1] - i[0][1]\n",
    "\n",
    "        # this is to take into account slopes\n",
    "        factor = 1 / (dx * dx + dy * dy) ** 0.5\n",
    "        new_dx = dy * shrink_value_x * factor\n",
    "        new_dy = dx * shrink_value_y * factor\n",
    "\n",
    "        new_lines.append(\n",
    "            [(i[0][0] + new_dx, i[0][1] - new_dy), (i[1][0] + new_dx, i[1][1] - new_dy)]\n",
    "        )\n",
    "\n",
    "    # find position of intersection of all the lines\n",
    "    new_coords = []\n",
    "    for i in range(len(new_lines)):\n",
    "        new_coords.append((line_intersection(new_lines[i - 1], new_lines[i])))\n",
    "\n",
    "    return new_coords\n",
    "\n",
    "\n",
    "# must be clockwise!\n",
    "coords = [\n",
    "    (0, 0),\n",
    "    (0, 10),\n",
    "    (2, 10),\n",
    "    (3, 8),\n",
    "    (4, 10),\n",
    "    (6, 10),\n",
    "    (6, 0),\n",
    "    (4, 1),\n",
    "    (4, 4),\n",
    "    (2, 4),\n",
    "    (2, 1),\n",
    "]\n",
    "\n",
    "# how much the coordinates are moved as an absolute value\n",
    "SHIFT = 0.5  # [m]\n",
    "\n",
    "up = shrink_polygon(coords, -SHIFT, -SHIFT)\n",
    "lo = shrink_polygon(coords, SHIFT, SHIFT)\n",
    "\n",
    "\"\"\"\n",
    "the last point is out of order for my needs\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def rotate(l, n):\n",
    "    return l[n:] + l[:n]\n",
    "\n",
    "\n",
    "up = rotate(up, 1)\n",
    "lo = rotate(lo, 1)\n",
    "\n",
    "a = np.array(up)\n",
    "b = np.array(coords)\n",
    "c = np.array(lo)\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "plt.plot(a[:, 0], a[:, 1], \"b-\")\n",
    "plt.plot(b[:, 0], b[:, 1], \"r-\")\n",
    "plt.plot(c[:, 0], c[:, 1], \"g-\")\n",
    "plt.axis(\"equal\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "now these edges can be used to compute the path as before! Plus outer and inner track"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.interpolate import interp1d\n",
    "from scipy.signal import savgol_filter\n",
    "\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step=0.1, smooth_factor=7):\n",
    "    \"\"\"\n",
    "    Computes a reference path given a set of waypoints\n",
    "    \"\"\"\n",
    "\n",
    "    final_xp = []\n",
    "    final_yp = []\n",
    "    delta = step  # [m]\n",
    "\n",
    "    for idx in range(len(start_xp) - 1):\n",
    "        section_len = np.sum(\n",
    "            np.sqrt(\n",
    "                np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
    "                + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
    "            )\n",
    "        )\n",
    "\n",
    "        interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
    "\n",
    "        fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
    "        fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
    "\n",
    "        # watch out to duplicate points!\n",
    "        final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
    "        final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
    "\n",
    "    \"\"\"this smoothens up corners\"\"\"\n",
    "    window_size = smooth_factor  # Smoothening filter window\n",
    "    final_xp = savgol_filter(final_xp, window_size, 1)\n",
    "    final_yp = savgol_filter(final_yp, window_size, 1)\n",
    "\n",
    "    dx = np.append(0, np.diff(final_xp))\n",
    "    dy = np.append(0, np.diff(final_yp))\n",
    "    theta = np.arctan2(dy, dx)\n",
    "\n",
    "    return np.vstack((final_xp, final_yp, theta))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0.5, 0, 'x')"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 27484 characters]\n",
      "text/plain": [
       "<Figure size 1080x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "track = compute_path_from_wp(b[:, 0], b[:, 1])\n",
    "track_lower = compute_path_from_wp(c[:, 0], c[:, 1])\n",
    "track_upper = compute_path_from_wp(a[:, 0], a[:, 1])\n",
    "\n",
    "plt.figure(figsize=(15, 10))\n",
    "\n",
    "plt.plot(track[0, :], track[1, :], \"b+\")\n",
    "plt.plot(track_lower[0, :], track_lower[1, :], \"g-\")\n",
    "plt.plot(track_upper[0, :], track_upper[1, :], \"r-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fc509665310>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 26704 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.plot(np.degrees(track[2, :]), \"b-\")\n",
    "plt.plot(np.degrees(track_lower[2, :]), \"g-\")\n",
    "plt.plot(np.degrees(track_upper[2, :]), \"r-\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "probably smarter to to save this in a file and load it in a notebook, so I dont have to carry over the code..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "with open(\"tracks/test.npy\", \"wb\") as f:\n",
    "    np.save(f, track)\n",
    "    np.save(f, track_lower)\n",
    "    np.save(f, track_upper)\n",
    "\n",
    "with open(\"tracks/test.npy\", \"rb\") as f:\n",
    "    a = np.load(f)\n",
    "    b = np.load(f)\n",
    "    c = np.load(f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# matches perfectly!\n",
    "# plt.plot(np.degrees(a[2,:]),\"b-\")\n",
    "# plt.plot(np.degrees(b[2,:]),\"g-\")\n",
    "# plt.plot(np.degrees(c[2,:]),\"r-\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:.conda-jupyter] *",
   "language": "python",
   "name": "conda-env-.conda-jupyter-py"
  },
  "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
