{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "c:\\Users\\admin\\Projects\\autopilot\\visualization.py:113: UserWarning: This figure includes Axes that are not compatible with tight_layout, so results might be incorrect.\n",
      "  plt.tight_layout()\n"
     ]
    }
   ],
   "source": [
    "from pathlib import Path\n",
    "from PIL import Image\n",
    "import numpy as np\n",
    "from autopilot import AutoPilot\n",
    "from visualization import VisualizationManager\n",
    "\n",
    "vzm = VisualizationManager()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "points = [[np.float64(0.48700765111972333), np.float64(0.49793590752304234)], [np.float64(0.4983137576734965), np.float64(0.5021469113219258)], [np.float64(0.4966178416904305), np.float64(0.5189909265174597)], [np.float64(0.4813545978428368), np.float64(0.5105689189196927)]]\n",
    "width, height = 1920, 1031\n",
    "points_coords = np.array(list(map(lambda p: [\n",
    "        (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height\n",
    "    ], points)))\n",
    "points_coords *= 2 ** 4\n",
    "\n",
    "vzm.set_target_points(points_coords)\n",
    "autopilot = AutoPilot(points_coords, [], vzm)\n",
    "imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(30)]\n",
    "autopilot.handle(imgs[0])\n",
    "vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-84.81833939223279\n",
      "[[[   0.43529349 -148.93070048]]]\n",
      "164.0668026014628 -13.040291124133018 90.0\n"
     ]
    }
   ],
   "source": [
    "import cv2\n",
    "import math\n",
    "mat = np.array([[ 9.03878625e-02,  9.95998304e-01, -1.36910620e+01],\n",
    " [-9.96729952e-01,  9.12780821e-02,  6.55573605e+00],\n",
    " [ 1.28059260e-05, -4.64443066e-06,  1.00000000e+00]])\n",
    "\n",
    "x = 156.295617\n",
    "y = 0\n",
    "\n",
    "print(np.atan2(mat[1, 0], mat[0, 0]) / math.pi * 180)\n",
    "\n",
    "print(cv2.perspectiveTransform(np.array([x, y]).reshape((1, 1, 2)), mat))\n",
    "\n",
    "nx, ny, na = autopilot.calc_position(np.linalg.inv(mat), x, y, math.pi / 2)\n",
    "\n",
    "print(nx, ny, na / math.pi * 180)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "  [Pilot] translate: -2.2243520368114386e-14 2.083887107525814e-15\n",
      "  [Pilot] Drone Position: (47.97, -11.12)\n",
      "  [Pilot] Angle: -11.3°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 304.9907527982671\n",
      "0.004129499336505887 50.0\n",
      "47.96512465732409 -11.122053958129657\n",
      "\n",
      "  [Pilot] translate: -0.773373251191613 49.85522586610471\n",
      "  [Pilot] Drone Position: (96.75, -21.44)\n",
      "  [Pilot] Angle: -11.1°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 304.9907527982671\n",
      "0.003525142621744165 50.0\n",
      "96.7474713468631 -21.438269533988773\n",
      "\n",
      "  [Pilot] translate: -0.4238729464016157 49.716078726704744\n",
      "  [Pilot] Drone Position: (145.50, -31.21)\n",
      "  [Pilot] Angle: -10.8°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 255.13708614259195\n",
      "0.0019957432639432504 50.0\n",
      "145.49554314093962 -31.210354705238323\n",
      "\n",
      "  [Pilot] translate: -0.5072011337859063 49.476309464169745\n",
      "  [Pilot] Drone Position: (194.03, -40.83)\n",
      "  [Pilot] Angle: -10.6°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 205.42141613269357\n",
      "0.0008527889497846608 50.0\n",
      "194.02953935562192 -40.83395396007064\n",
      "\n",
      "  [Pilot] translate: 0.11201474583194077 50.52107985317721\n",
      "  [Pilot] Drone Position: (243.70, -50.06)\n",
      "  [Pilot] Angle: -10.6°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 155.9448225243591\n",
      "0.0006948810462807098 50.0\n",
      "243.70149729872674 -50.058304936308375\n",
      "\n",
      "  [Pilot] translate: 0.0398933070509686 50.472489376060885\n",
      "  [Pilot] Drone Position: (293.34, -59.18)\n",
      "  [Pilot] Angle: -10.5°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 105.42365806674246\n",
      "-0.005757684837339738 50.0\n",
      "293.3429112368944 -59.17991649962423\n",
      "\n",
      "  [Pilot] translate: -0.5779730654942618 49.87019784134602\n",
      "  [Pilot] Drone Position: (342.21, -69.12)\n",
      "  [Pilot] Angle: -10.8°\n",
      "  [Pilot] Target Index: 1\n",
      "  [Pilot] Target Position: [347.32359333 -69.46471867]\n",
      "  [Pilot] Distance: 54.95171694362866\n",
      "-0.6 50.0\n",
      "342.21488384208834 -69.12481364723727\n",
      "\n",
      "  [Pilot] translate: -0.3671006590822984 49.56271477490151\n",
      "  [Pilot] Drone Position: (376.83, -104.60)\n",
      "  [Pilot] Angle: -45.3°\n",
      "  [Pilot] Target Index: 2\n",
      "  [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
      "  [Pilot] Distance: 282.13933631348874\n",
      "-0.6 50.0\n",
      "  [Pilot] translate: 152.48531037725778 -11.363336940584135\n",
      "376.82813934296445 -104.60043911576437\n",
      "\n",
      "  [Pilot] translate: 0.814116178363097 49.83161557706345\n",
      "  [Pilot] Drone Position: (386.60, -153.47)\n",
      "  [Pilot] Angle: -79.6°\n",
      "  [Pilot] Target Index: 2\n",
      "  [Pilot] Target Position: [ 295.22505433 -347.32359333]\n",
      "  [Pilot] Distance: 256.0734134499727\n",
      "-0.6 50.0\n",
      "  [Pilot] translate: 223.2717832052611 -50.70818702916637\n",
      "386.60059278853873 -153.47120593833623\n"
     ]
    }
   ],
   "source": [
    "for i in range(1, 10):\n",
    "    print()\n",
    "    command = autopilot.handle(imgs[i])\n",
    "    print(command.dangle, command.velocity)\n",
    "    vzm.set_target_index(autopilot.target_idx)\n",
    "    vzm.update_drone_trajectory(autopilot.x, autopilot.y)\n",
    "    pos = autopilot.get_position_by_chunk()\n",
    "    print(autopilot.x, autopilot.y)\n",
    "    vzm.update_global_map(0, 0)\n",
    "    vzm.pause(2.5)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[[  0.18926711 -21.79121458]]]\n",
      "0.40152777578126847\n"
     ]
    }
   ],
   "source": [
    "mat = np.array([[ 9.18211825e-01, -3.88263543e-01,  4.35846954e-01],\n",
    " [ 3.89868385e-01,  9.19587423e-01, -2.49399645e+01],\n",
    " [-3.56258907e-06,  3.01183248e-06,  1.00000000e+00]])\n",
    "\n",
    "pos = np.array([1., 3.]).reshape((1, 1, 2))\n",
    "print(cv2.perspectiveTransform(pos, mat))\n",
    "\n",
    "(mat) @ np.array([1., 3., 1.]).reshape((3))\n",
    "print(np.atan2(mat[1, 0], mat[0, 0]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.70710678  2.12132034]\n",
      " [ 0.70710678  3.53553391]\n",
      " [-2.12132034  3.53553391]\n",
      " [ 0.          0.        ]]\n",
      "[[ 7.07106671e-01 -7.07106696e-01 -9.13243653e-17]\n",
      " [ 7.07106528e-01  7.07106788e-01  3.65297461e-16]\n",
      " [-7.16496367e-08  2.10734232e-09  1.00000000e+00]]\n",
      "0.7853980622449934\n"
     ]
    }
   ],
   "source": [
    "ang = np.radians(45)\n",
    "rot = np.array([\n",
    "    [np.cos(ang), -np.sin(ang)],\n",
    "    [np.sin(ang),  np.cos(ang)]\n",
    "])\n",
    "\n",
    "pts = np.array([\n",
    "    [1., 2.],\n",
    "    [3., 2.],\n",
    "    [1., 4.],\n",
    "    [0., 0.]\n",
    "])\n",
    "\n",
    "dst = pts @ rot.T\n",
    "\n",
    "mat, mask = cv2.findHomography(pts, pts @ rot.T)\n",
    "# print(np.atan2(mat[0, 1], mat[0, 0]))\n",
    "# pts @ rot.T\n",
    "pts @ mat[0:2]\n",
    "# print(cv2.perspectiveTransform(pts[0].reshape((-1, 1, 2)), mat))\n",
    "print(dst)\n",
    "print(mat)\n",
    "\n",
    "\n",
    "mat[0:2, 0:2] @ pts.T\n",
    "\n",
    "print(np.atan2(mat[1, 0], mat[0, 0]))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(1, 2, 3)\n"
     ]
    }
   ],
   "source": [
    "def a(*args):\n",
    "    print(args)\n",
    "\n",
    "a(*(1, 2, 3))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "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.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}