{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "from cv2 import aruco\n",
    "from datetime import datetime\n",
    "from loguru import logger\n",
    "from pathlib import Path\n",
    "from typing import cast, Final, TypeAlias\n",
    "from cv2.typing import MatLike\n",
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "NDArray: TypeAlias = np.ndarray"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "INPUT_IMAGE = Path(\"merged_uv_layout.png\")\n",
    "# 7x7\n",
    "DICTIONARY: Final[int] = aruco.DICT_7X7_1000\n",
    "# 400mm\n",
    "MARKER_LENGTH: Final[float] = 0.4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "aruco_dict = aruco.getPredefinedDictionary(DICTIONARY)\n",
    "detector = aruco.ArucoDetector(\n",
    "    dictionary=aruco_dict, detectorParams=aruco.DetectorParameters()\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [],
   "source": [
    "frame = cv2.imread(str(INPUT_IMAGE))\n",
    "grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)\n",
    "# pylint: disable-next=unpacking-non-sequence\n",
    "markers, ids, rejected = detector.detectMarkers(grey)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Note: BGR\n",
    "RED = (0, 0, 255)\n",
    "GREEN = (0, 255, 0)\n",
    "BLUE = (255, 0, 0)\n",
    "YELLOW = (0, 255, 255)\n",
    "GREY = (128, 128, 128)\n",
    "CYAN = (255, 255, 0)\n",
    "MAGENTA = (255, 0, 255)\n",
    "ORANGE = (0, 165, 255)\n",
    "PINK = (147, 20, 255)\n",
    "\n",
    "UI_SCALE = 10\n",
    "UI_SCALE_FONT = 8\n",
    "UI_SCALE_FONT_WEIGHT = 20"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "out = frame.copy()\n",
    "# `markers` is [N, 1, 4, 2]\n",
    "# `ids` is [N, 1]\n",
    "if ids is not None:\n",
    "    markers = np.reshape(markers, (-1, 4, 2))\n",
    "    ids = np.reshape(ids, (-1, 1))\n",
    "    # logger.info(\"markers={}, ids={}\", np.array(markers).shape, np.array(ids).shape)\n",
    "    for m, i in zip(markers, ids):\n",
    "        # logger.info(\"id={}, center={}\", i, center)\n",
    "        center = np.mean(m, axis=0).astype(int) # type: ignore\n",
    "        # BGR\n",
    "        color_map = [RED, GREEN, BLUE, YELLOW]\n",
    "        for color, corners in zip(color_map, m):\n",
    "            corners = corners.astype(int)\n",
    "            out = cv2.circle(out, corners, 5*UI_SCALE, color, -1)\n",
    "        cv2.circle(out, tuple(center), 5*UI_SCALE, CYAN, -1)\n",
    "        cv2.putText(\n",
    "            out,\n",
    "            str(i),\n",
    "            tuple(center),\n",
    "            cv2.FONT_HERSHEY_SIMPLEX,\n",
    "            1*UI_SCALE_FONT,\n",
    "            MAGENTA,\n",
    "            UI_SCALE_FONT_WEIGHT,\n",
    "        )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.imshow(cv2.cvtColor(out, cv2.COLOR_BGR2RGB))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cv2.imwrite(\"merged_uv_layout_with_markers.png\", out)"
   ]
  }
 ],
 "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.12.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}