{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Check Canting Notebook\n",
    "\n",
    "This notebook serves two main purposes:\n",
    "\n",
    "1. Rotation Matrix Generation:\n",
    "   - Allows users to input canting values (pitch, yaw, and roll) in degrees.\n",
    "   - Generates a rotation matrix based on these canting values.\n",
    "   - Useful for transforming coordinates or orientations in 3D space.\n",
    "\n",
    "2. Canting Verification:\n",
    "   - Provides a way to verify the correctness of canting calculations.\n",
    "   - Helps users visualize and understand the effects of different canting angles.\n",
    "   - Can be used to cross-check canting values with physical measurements or expected results.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##### <span style=\"color: red;\"> Note: +Pitch is Upwards, +Yaw is Leftward, +Roll is Clockwise, all with respect to user.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Produce Rotation Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "!pip install matplotlib\n",
    "!pip install ipywidgets"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import IPython.display as idisplay\n",
    "import json\n",
    "\n",
    "import ipywidgets\n",
    "# Set the backend to a supported value\n",
    "import matplotlib\n",
    "matplotlib.use('tkagg')  # Using 'tkagg' as it's widely supported\n",
    "\n",
    "# Now import pyplot after setting the backend\n",
    "import matplotlib.pyplot as plt\n",
    "# Enable interactive widgets for VSCode\n",
    "\n",
    "\n",
    "# Create interactive sliders for pitch, yaw, and roll\n",
    "l_pitch_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Left Pitch:')\n",
    "l_yaw_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Left Yaw:')\n",
    "l_roll_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Left Roll:')\n",
    "\n",
    "r_pitch_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Right Pitch:')\n",
    "r_yaw_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Right Yaw:')\n",
    "r_roll_slider = ipywidgets.FloatText(value=0, min=-90, max=90, step=0.01, description='Right Roll:')\n",
    "\n",
    "\n",
    "# Create a container for the sliders\n",
    "sliders = ipywidgets.HBox([ipywidgets.VBox([l_pitch_slider, l_yaw_slider, l_roll_slider]), ipywidgets.VBox([r_pitch_slider, r_yaw_slider, r_roll_slider])])\n",
    "\n",
    "# Create an output widget for the results\n",
    "output = ipywidgets.Output()\n",
    "\n",
    "# Display the sliders and output widget\n",
    "display(sliders)\n",
    "display(output)\n",
    "\n",
    "def create_rotation_matrix(pitch, yaw, roll):\n",
    "    # Convert degrees to radians\n",
    "    pitch_rad = np.radians(pitch)\n",
    "    yaw_rad = np.radians(yaw)\n",
    "    roll_rad = np.radians(roll)\n",
    "    \n",
    "    # Create rotation object\n",
    "    rotation = R.from_euler('xyz', [pitch_rad, yaw_rad, roll_rad])\n",
    "    \n",
    "    # Get rotation matrix\n",
    "    return rotation.as_matrix()\n",
    "\n",
    "def update_matrices(change):\n",
    "    l_pitch = l_pitch_slider.value\n",
    "    l_yaw = l_yaw_slider.value\n",
    "    l_roll = l_roll_slider.value\n",
    "    r_pitch = r_pitch_slider.value\n",
    "    r_yaw = r_yaw_slider.value\n",
    "    r_roll = r_roll_slider.value    \n",
    "    \n",
    "    # Clear only the output widget, not the sliders\n",
    "    output.clear_output(wait=True)\n",
    "    \n",
    "    with output:\n",
    "        left_eye_matrix = create_rotation_matrix(l_pitch, l_yaw, l_roll)\n",
    "        right_eye_matrix = create_rotation_matrix(r_pitch, r_yaw, r_roll)\n",
    "        \n",
    "        np.set_printoptions(precision=6, floatmode='fixed', suppress=True)\n",
    "        print(\"Left Eye Rotation Matrix:\")\n",
    "        print(json.dumps(left_eye_matrix.tolist()))\n",
    "        print(\"\\nRight Eye Rotation Matrix:\")\n",
    "        print(json.dumps(right_eye_matrix.tolist()))\n",
    "\n",
    "# Connect the update function to all sliders\n",
    "l_pitch_slider.observe(update_matrices, names='value')\n",
    "l_yaw_slider.observe(update_matrices, names='value')\n",
    "l_roll_slider.observe(update_matrices, names='value')\n",
    "r_pitch_slider.observe(update_matrices, names='value')\n",
    "r_yaw_slider.observe(update_matrices, names='value')\n",
    "r_roll_slider.observe(update_matrices, names='value')\n",
    "\n",
    "# Initial update\n",
    "update_matrices(None)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Check Angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "import json\n",
    "# Load the JSON file\n",
    "from glob import glob\n",
    "\n",
    "# Find the first JSON file in the directory\n",
    "json_file = glob('./*.json')[0]\n",
    "\n",
    "if json_file:\n",
    "    with open(json_file, 'r') as file:\n",
    "        data = json.load(file)\n",
    "\n",
    "assert data is not None, \"Failed to load JSON data\"\n",
    "assert isinstance(data, dict), \"Loaded data is not a dictionary\"\n",
    "assert 'tracking_to_eye_transform' in data, \"Missing 'tracking_to_eye_transform' key in JSON data\"\n",
    "assert len(data['tracking_to_eye_transform']) == 2, \"Expected data for two eyes in 'tracking_to_eye_transform'\"\n",
    "assert all('eye_to_head' in eye_data for eye_data in data['tracking_to_eye_transform']), \"Missing 'eye_to_head' data for one or both eyes\"\n",
    "\n",
    "#pull out the eye_to_head matrix\n",
    "left_eye = 0\n",
    "right_eye = 1\n",
    "eye_names = ['Left', 'Right']\n",
    "\n",
    "eye_to_head = {}\n",
    "print(\"Matrices found in file:\")\n",
    "for eye in [left_eye, right_eye]:\n",
    "    eye_to_head[eye] = data['tracking_to_eye_transform'][eye]['eye_to_head']\n",
    "    eye_to_head[eye] = np.array(eye_to_head[eye])\n",
    "    print(eye_to_head[eye])\n",
    "    print()\n",
    "\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import inspect \n",
    "for eye in [left_eye, right_eye]:\n",
    "    rotation = R.from_matrix(eye_to_head[eye])\n",
    "    rotation_euler = rotation.as_euler('xyz', degrees=True)\n",
    "    print(\"Eye: \", eye_names[eye])\n",
    "    print(\"Upward pitch: \", f\"{rotation_euler[0]:.6f}\")\n",
    "    print(\"Leftward yaw: \", f\"{rotation_euler[1]:.6f}\")\n",
    "    print(\"Clockwise roll: \", f\"{rotation_euler[2]:.6f}\")\n",
    "    print()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv (3.10.11)",
   "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.10.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}