{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "cab2d0f1",
   "metadata": {},
   "source": [
    "# **Trajectory** back on original video\n",
    "\n",
    "IDEA:\n",
    "- take the processed positions\n",
    "- apply the inverse of the homography\n",
    "- print on the video frame by frame all points before the current frame"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "4cd7f188",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pathlib import Path\n",
    "import cv2\n",
    "import csv\n",
    "\n",
    "import numpy as np\n",
    "import pandas as pd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "060e2535",
   "metadata": {},
   "outputs": [],
   "source": [
    "VIDEO_NUMBER = \"7\"\n",
    "PROJECT_ROOT = Path().resolve().parent.parent\n",
    "INPUT_VIDEO_PATH = str(\n",
    "    PROJECT_ROOT\n",
    "    / \"data\"\n",
    "    / f\"recording_{VIDEO_NUMBER}\"\n",
    "    / f\"Recording_{VIDEO_NUMBER}.mp4\"\n",
    ")\n",
    "TRASFORMED_CSV_PATH = str(\n",
    "    PROJECT_ROOT\n",
    "    / \"data\"\n",
    "    / \"auxiliary_data\"\n",
    "    / \"reconstructed_positions\"\n",
    "    / f\"Transformed_positions_processed_{VIDEO_NUMBER}.csv\"\n",
    ")\n",
    "LANE_CSV_PATH = str(\n",
    "    PROJECT_ROOT\n",
    "    / \"data\"\n",
    "    / \"auxiliary_data\"\n",
    "    / \"lane_points\"\n",
    "    / f\"Lane_points_{VIDEO_NUMBER}.csv\"\n",
    ")\n",
    "OUTPUT_VIDEO_PATH = str(\n",
    "    PROJECT_ROOT\n",
    "    / \"data\"\n",
    "    / f\"recording_{VIDEO_NUMBER}\"\n",
    "    / f\"Tracked_output_{VIDEO_NUMBER}.mp4\"\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "72b93c45",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load video\n",
    "cap = cv2.VideoCapture(INPUT_VIDEO_PATH)\n",
    "if not cap.isOpened():\n",
    "    print(\"Error: Could not open video.\")\n",
    "    exit\n",
    "\n",
    "# Get video properties\n",
    "frame_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))\n",
    "frame_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))\n",
    "fps = int(cap.get(cv2.CAP_PROP_FPS))\n",
    "\n",
    "# Setup video writer\n",
    "fourcc = cv2.VideoWriter_fourcc(*\"mp4v\")\n",
    "out = cv2.VideoWriter(OUTPUT_VIDEO_PATH, fourcc, fps, (frame_width, frame_height))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "id": "27d15335",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load lane points and compute homographies\n",
    "lane_points = pd.read_csv(LANE_CSV_PATH)\n",
    "homographies = {}\n",
    "\n",
    "width = 106  # Approximate pixels for 1.0668m\n",
    "height = 1829  # Approximate pixels for 18.29m\n",
    "\n",
    "for _, row in lane_points.iterrows():\n",
    "    frame = int(row[\"Frame\"])\n",
    "    pts_src = np.array(\n",
    "        [\n",
    "            [row[\"bottom_left_x\"], row[\"bottom_left_y\"]],\n",
    "            [row[\"bottom_right_x\"], row[\"bottom_right_y\"]],\n",
    "            [row[\"up_right_x\"], row[\"up_right_y\"]],\n",
    "            [row[\"up_left_x\"], row[\"up_left_y\"]],\n",
    "        ],\n",
    "        dtype=np.float32,\n",
    "    )\n",
    "\n",
    "    pts_dst = np.array(\n",
    "        [[0, height], [width, height], [width, 0], [0, 0]], dtype=np.float32\n",
    "    )\n",
    "\n",
    "    H, _ = cv2.findHomography(pts_dst, pts_src)  # Inverse homography\n",
    "    if H is not None:\n",
    "        homographies[frame] = H\n",
    "\n",
    "# Load real-world trajectory positions (top-down space)\n",
    "positions = {}\n",
    "with open(TRASFORMED_CSV_PATH, mode=\"r\") as file:\n",
    "    reader = csv.reader(file)\n",
    "    next(reader)  # Skip header\n",
    "    for coord in reader:\n",
    "        frame_num, x, y = coord\n",
    "        frame_num = int(frame_num)\n",
    "        if x and y:\n",
    "            positions[frame_num] = (float(x), float(y))  # Store in top-down space\n",
    "        else:\n",
    "            positions[frame_num] = None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e63549c3",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "End of video or error reading frame.\n",
      "Tracking video saved to /home/davic/projects/IACV_project/bowling-analysis/data/recording_7/Tracked_output_7.mp4\n"
     ]
    }
   ],
   "source": [
    "# Draw trajectory frame-by-frame\n",
    "frame_count = 0\n",
    "trajectory = []  # Store real-world trajectory (top-down)\n",
    "\n",
    "while True:\n",
    "    ret, frame = cap.read()\n",
    "    if not ret:\n",
    "        print(\"End of video or error reading frame.\")\n",
    "        break\n",
    "\n",
    "    # Collect current position if available\n",
    "    if frame_count in positions and positions[frame_count] is not None:\n",
    "        trajectory.append(positions[frame_count])\n",
    "\n",
    "    # Transform all points in trajectory to current frame using current frame's homography\n",
    "    refreshed_trajectory = []\n",
    "    if frame_count in homographies:\n",
    "        H = homographies[frame_count]\n",
    "        for x, y in trajectory:\n",
    "            transformed = cv2.perspectiveTransform(\n",
    "                np.array([[[x, y]]], dtype=np.float32), H\n",
    "            )[0][0]\n",
    "            refreshed_trajectory.append((int(transformed[0]), int(transformed[1])))\n",
    "\n",
    "    # Draw trajectory lines\n",
    "    for i in range(1, len(refreshed_trajectory)):\n",
    "        cv2.line(\n",
    "            frame, refreshed_trajectory[i - 1], refreshed_trajectory[i], (0, 0, 255), 2\n",
    "        )\n",
    "\n",
    "    # Draw current ball position\n",
    "    if refreshed_trajectory:\n",
    "        x, y = refreshed_trajectory[-1]\n",
    "        cv2.circle(frame, (x, y), 4, (0, 0, 255), -1)  # Red dot\n",
    "\n",
    "    out.write(frame)\n",
    "    frame_count += 1\n",
    "\n",
    "cap.release()\n",
    "out.release()\n",
    "print(f\"Tracking video saved to {OUTPUT_VIDEO_PATH}\")"
   ]
  }
 ],
 "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.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
