# AUTOGENERATED! DO NOT EDIT! File to edit: ../nbs/00_core.ipynb.

# %% auto 0
__all__ = ['cli', 'v_quat_example', 'd_roll_ex', 'd_pitch_ex', 'd_yaw_ex', 'v_quat_ex', 'f_template_function', 'f_show_doc',
           'f_q2rollPitchYaw', 'f_test_f_q2rollPitchYaw', 'f_rollPitchYaw2q', 'f_test_f_rollPitchYaw2q',
           'f_nav2target_pos', 'f_hyster', 'f_save', 'f_load', 'f_generate_unique_filename', 'f_save_var', 'f_load_var',
           'f_pointCloud', 'f_plot_rotated_axes', 'f_plot_3d_trajectory', 'f_image_grid',
           'f_plot_auv_orientation_subplots', 'f_plot_auv_orientation_trajectory',
           'f_create_auv_orientation_animation_plotly', 'f_add_trajectory_traces', 'f_create_axes',
           'f_add_bounding_box', 'f_add_initial_position_and_axes', 'f_add_animation_frames', 'f_update_layout',
           'f_create_auv_orientation_animation', 'f_plot_polar_sonar_seaking', 'f_plot_polar_sonar_micron',
           'f_plot_horizontal_sonar', 'f_plot_vertical_sonar', 'f_plot_horizontal_sonar_with_angles',
           'f_plot_vertical_sonar_with_angles', 'f_animation_AUV_micron_trajectory', 'f_create_polar_sonar_animation',
           'f_micron_with_trajectory', 'f_seaking_with_trajectory', 'f_remove_macosx_folder', 'f_unzip_file',
           'f_format_time', 'f_add_packages_to_all']

# %% ../nbs/00_core.ipynb 5
# For matrix manipulation and numerical calculations
import numpy as np

# To manage UTC, date, and time operations
# import datetime
from datetime import datetime, timezone

# For basic math operations (e.g., ceil, floor, etc.)
import math

# For creating visualizations (2D, 3D, animations, widgets)
import matplotlib.pyplot as plt
from matplotlib import animation
from matplotlib.animation import FuncAnimation
from matplotlib.widgets import Slider, Button, RadioButtons
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.gridspec as gridspec
from matplotlib.ticker import MaxNLocator
 
# Same with plotly
import plotly.graph_objects as go
import plotly.io as pio
pio.renderers.default = "jupyterlab+notebook+plotly_mimetype"

# For image processing
from PIL import Image

# For interactive widgets in Jupyter notebooks
from ipywidgets import interact, widgets, interactive, IntSlider

# To control file system and path operations
import os

# To execute shell commands from Jupyter notebooks
import subprocess

# For CSV processing and data manipulation
import pandas as pd

# For 3D rotation, transformation, and interpolation
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp

# For general scientific computing, including data interpolation
import scipy

# For 3D data visualization
# import pyvista as pv

# To display HTML content in Jupyter notebooks (e.g., to style output)
from IPython.display import HTML, display, clear_output, Markdown

# For skimage filters and morphology operations
from skimage.morphology import disk
from skimage.filters import rank
from skimage.color import rgb2gray
from skimage import data, filters

# For managing command-line interface commands
import cmd
cli = cmd.Cmd()

# To suppress warnings
import warnings

# For testing
from fastcore.test import *
import pytest

# Type
from typing import Tuple, Optional, List, Dict, Union

# Data management
import pickle

# For time measurements
import time

# For unzipping files
import zipfile
import shutil

# For regular expression (ReGEX)
import re

# %% ../nbs/00_core.ipynb 8
def f_template_function(s_param1: str, d_param2: int, m_param3: np.ndarray) -> List[float]:
    r"""
    Template function demonstrating coding rules, docstring format, and best practices.
    
    ::: {.callout-note}
    This function serves as a template to illustrate our coding standards, documentation 
    requirements, and best practices for writing high-quality, maintainable Python code.
    :::

    **Parameters**  
     \ \  `s_param1`: String parameter description  
     \ \  `d_param2`: Integer parameter description  
     \ \  `m_param3`: numpy.ndarray parameter description  

    **Returns**  
     \ \  List[float]: Description of the returned list of floats  

    **Example**
    
    ```python
    import numpy as np
    
    s_example = "test"
    d_value = 42
    m_array = np.array([1.0, 2.0, 3.0])
    
    result = f_template_function(s_example, d_value, m_array)
    print(result)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Coding Rules and Best Practices**

    1. **Naming Conventions**:
       - Use `s_` prefix for strings
       - Use `d_` prefix for scalars (_e.g._ integers and floats)
       - Use `m_` prefix for matrixes (_e.g._ Numpy arrays)
       - Use `l_` prefix for lists
       - Use `t_` prefix for tuples
       - Use `dic_` prefix for dictionaries
       - Use `df_` prefix for dataframe
       - Use `h_` prefix for handles (e.g., file handles, plot handles)
       - Use `f_` prefix for functions
       - Use `s_` prefix for scripts
       - Use lowercase with underscores for function names

    2. **Type Hinting**:
       - Always include type hints for parameters and return values

    3. **Docstrings**:
       - Use nbdev-compatible docstring format
       - Include a brief description, parameters, returns, and example
       - Add a callout-tip section for detailed explanations or best practices

    4. **Comments**:
       - Add detailed comments for complex operations
       - Explain the 'why' behind the code, not just the 'what'

    5. **Code Structure**:
       - Keep functions focused on a single task
       - Limit function length (aim for under 50 lines)
       - Use meaningful variable names

    6. **Error Handling**:
       - Use try-except blocks for error-prone operations
       - Raise custom exceptions when appropriate

    7. **Performance Considerations**:
       - Use vectorized operations with numpy when possible
       - Avoid unnecessary loops or function calls

    8. **Testing**:
       - Write unit tests for all functions
       - Include edge cases in your tests

    9. **Version Control**:
       - Make frequent, small commits with clear messages
       - Use feature branches for new developments

    10. **Documentation**:
        - Keep docstrings and comments up-to-date
        - Document any assumptions or limitations
    :::

    **References**  
    \ \ 1. [PEP 8 -- Style Guide for Python Code](https://www.python.org/dev/peps/pep-0008/)  
    \ \ 2. [NumPy Documentation](https://numpy.org/doc/stable/)  

    """
    # Detailed comment explaining the purpose of this code block
    try:
        # Code implementation goes here
        l_result = []
        
        # Example of a complex operation that requires explanation
        for d_i in range(d_param2):
            # Detailed comment explaining this step
            d_value = np.mean(m_param3) * d_i
            l_result.append(d_value)
        
        # More implementation...
        
    except ValueError as e:
        # Error handling with custom message
        raise ValueError(f"An error occurred while processing {s_param1}: {str(e)}")
    
    return l_result

# %% ../nbs/00_core.ipynb 11
def f_show_doc(f_function: callable) -> str:
    r"""
    Converts the docstring of a function from Quarto/nbdev style to MyST Markdown,
    removes indentation, replaces the first 'Parameters' with a heading of the
    function name, and displays it directly in a Jupyter Notebook via IPython Markdown.
    
    ::: {.callout-note}
    This function is designed to enhance the readability and presentation of function
    documentation in Jupyter Notebooks, particularly for functions using Quarto/nbdev
    style docstrings.
    :::

    **Parameters**  
     \ \  - `f_function`: The Python function whose docstring we want to convert and display  

    **Returns**  
     \ \  None: This function displays the formatted docstring but doesn't return any value  

    **Example**
    
    ```
    def f_example(s_param: str) -> str:
        '''
        An example function.
        
        **Parameters**  
         \ \  - `s_param`: A string parameter  
        
        **Returns**  
         \ \  str: The input string, reversed  
        '''
        return s_param[::-1]
    
    f_show_doc(f_example)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Function Details**

    1. **Docstring Extraction**:
       - Retrieves the docstring using `f_function.__doc__`
       - Handles cases where the docstring might be None

    2. **Line Processing**:
       - Removes leading/trailing blank lines
       - Strips trailing whitespace from each line
       - Removes Quarto's escaped indentation (\ \ )

    3. **Callout Conversion**:
       - Converts Quarto callouts to MyST admonitions
       - Handles the first "note" callout differently (no dropdown)

    4. **Heading Adjustment**:
       - Replaces the first occurrence of "Parameters" with the function name as a heading

    5. **Formatting**:
       - Removes all leading indentation from each line
       - Adds the function name as an HTML h3 heading
       - Wraps the entire content in a blockquote

    6. **Display**:
       - Uses IPython's Markdown display functionality to render the processed docstring
    :::

    **References**  
    
    \ \ 1. [MyST Markdown](https://myst-parser.readthedocs.io/en/latest/)  
    \ \ 2. [Quarto Documentation](https://quarto.org/)  
    \ \ 3. [IPython Display Module](https://ipython.readthedocs.io/en/stable/api/generated/IPython.display.html)  
    """
    # 1) Extract docstring
    s_doc = f_function.__doc__ or ""
    l_lines = s_doc.splitlines()

    # 2) Remove leading/trailing blank lines
    while l_lines and not l_lines[0].strip():
        l_lines.pop(0)
    while l_lines and not l_lines[-1].strip():
        l_lines.pop()

    # Track whether we're inside a callout, and how many colons (3 or 4) were used
    b_in_callout = False
    n_colons_opened = 0  # Will store either 3 or 4 for the opened callout

    # A backtick character (make sure it's a backtick, not a single quote!)
    backtick = ":"

    l_transformed = []

    for s_line in l_lines:
        # Remove trailing whitespace
        s_line = s_line.rstrip()

        # Remove Quarto's escaped indentation (e.g., "\ \ ")
        s_line = re.sub(r'(\\\s+\\\s+)+', '', s_line)

        s_stripped = s_line.strip()

        # ---- 1) Detect an OPENING callout line: ::: {...} or :::: {...} ----
        # e.g.:  ::: {.callout-tip collapse=true}  or  :::: {.callout-note}
        m_open = re.match(r'^(::+)\s*\{\.callout-(\w+)(.*)\}', s_stripped)
        if m_open:
            s_colons = m_open.group(1)        # ':::' or '::::'
            n_colons = len(s_colons)          # 3 or 4
            s_type   = m_open.group(2)        # e.g. "note", "tip", "warning"
            s_params = m_open.group(3) or ""  # e.g. " collapse=true"

            # Begin MyST admonition with triple or quadruple backticks
            s_backticks = backtick * n_colons  # "```" or "````"

            # If "collapse=true" is present, add the dropdown class
            if "collapse=true" in s_params:
                s_line = f"{s_backticks}{{{s_type}}}\n:class: dropdown"
            else:
                s_line = f"{s_backticks}{{{s_type}}}"

            b_in_callout = True
            n_colons_opened = n_colons

        else:
            # ---- 2) Detect a CLOSING callout line: ::: or :::: (with optional spaces) ----
            # e.g. just ":::", possibly with trailing spaces
            m_close = re.match(r'^(::+)\s*$', s_stripped)
            if m_close and b_in_callout:
                s_colons = m_close.group(1)  # ':::' or '::::'
                n_colons = len(s_colons)     # 3 or 4
                # Only close if the number of colons matches the opened block
                if n_colons == n_colons_opened:
                    # Replace with the same number of backticks
                    s_line = backtick * n_colons  # "```" or "````"
                    b_in_callout = False

        l_transformed.append(s_line)

    # 3) Remove leading indentation from each line
    for i in range(len(l_transformed)):
        l_transformed[i] = l_transformed[i].lstrip()

    # 4) Join them into a single string
    s_intermediate = "\n".join(l_transformed)

    # 5) Insert an extra blank line after '**References**' for correct display
    #    We'll do a small pass line-by-line. If we see a line starting with "**References**"
    #    we add a blank line right after it (unless there's already one).
    l_temp = s_intermediate.split("\n")
    l_result = []
    for i, line in enumerate(l_temp):
        l_result.append(line)
        # If it starts with "**References**", insert a blank line
        if line.strip().startswith("**References**"):
            # Insert a blank line if the next line isn't empty
            if i+1 < len(l_temp) and l_temp[i+1].strip():
                l_result.append("")
    s_intermediate = "\n".join(l_result)

    # 6) Build final displayed markdown
    s_final_markdown = f"<h3>{f_function.__name__}</h3>\n\n{s_intermediate}"

    # 7) Display in Jupyter
    display(Markdown(s_final_markdown))

# %% ../nbs/00_core.ipynb 19
def f_q2rollPitchYaw(v_quaternion: np.ndarray) -> Tuple[float, float, float]:
    r"""
    Convert quaternion to roll, pitch, and yaw (Euler angles) using the 3-2-1 sequence. 
    
    ::: {.callout-note}
    The 3-2-1 or YZX convention is the common one used for Tait-Bryan or nautical angles.
    See [wiki](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
    for more details. 
    :::

    **Parameters**  
     \ \  - `v_quaternion`: Normalized quaternion [qx, qy, qz, qw]

    **Returns**  
    \ \  - Tuple of (roll, pitch, yaw) angles in radians

    **Example**
    
    ```python
    # import numpy
    import numpy as np
    
    # Create quaternion
    v_quat = np.array([0.1, 0.2, 0.3, 0.4])

    # Convert quaternion to Roll, Pitch, and Yaw
    d_roll, d_pitch, d_yaw = f_q2rollPitchYaw(v_quat)

    # Display the results
    print(f"Roll: {d_roll:.4f}, Pitch: {d_pitch:.4f}, Yaw: {d_yaw:.4f}")
    ```
    
    :::: {.callout-tip collapse=true}
    #### **More details on quaternion to Euler Angles (Roll, Pitch, Yaw) conversion used for Tait-Bryan or nautical angles**  

    Given a quaternion $q = (q_x, q_y, q_z, q_w)$, we can convert it to Euler angles (roll $\phi$, pitch $\theta$, yaw $\psi$) using the following method:
    
    **Preliminary Calculations**  
    
    We compute two auxiliary values:
    
    - test = $q_z q_w + q_x q_y$
    - unit = $q_x^2 + q_y^2 + q_z^2 + q_w^2$
    
    These values are used to check for singularities and normalize the calculations.
    
    **Singularity Handling**  
    We check for singularities at the poles:
    
    1. North Pole singularity (if test $> 0.499 \ \cdot$ unit):  
       $\psi = 2 \cdot atan2(q_x, q_w)$  
       θ = π/2  
       φ = 0  
    
    2. South Pole singularity (if test $< -0.499 \ \cdot$ unit):  
       $\psi = -2 \cdot atan2(q_x, q_w)$  
       θ = -π/2  
       φ = 0  
    
    **General Case**  
    
    If no singularity is present, we compute the Euler angles as follows:  
    
    1. Roll (φ, x-axis rotation):  
       φ = atan2(2(q_w q_x + q_y q_z), 1 - 2(q_x^2 + q_y^2))
    
    2. Pitch (θ, y-axis rotation):  
       θ = asin(2(q_y q_w - q_z q_x)/unit)
    
    3. Yaw ($\psi$, z-axis rotation):  
       $\psi = atan2(2(q_w q_z + q_x q_y), 1 - 2(q_y^2 + q_z^2))$
    
    ::: {.callout-note}
    #### Adjustments from the references  
    Note that we applied a correction from the docs here because `math.asin(2*test/unit)` is wrong (in our case)
    :::

    This method ensures proper handling of singularities and provides accurate conversion from quaternions to Euler angles.
    ::::

    **References**  
    \ \ 1. [Quaternion to Euler Conversion](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)  
    \ \ 2. [Singularity Handling](https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/)  

    
    """
    
    # Validate the input quaternion
    if len(v_quaternion) != 4:
        raise ValueError("Input quaternion must be a 4-element array.")

    # Extract quaternion components
    d_qx, d_qy, d_qz, d_qw = v_quaternion

    # Manage singularity and potential rounding error that create a not normalized quaternion
    # More information at https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
    d_test = d_qz * d_qw + d_qx * d_qy
    
    d_sqw = d_qw * d_qw
    d_sqx = d_qx * d_qx
    d_sqy = d_qy * d_qy
    d_sqz = d_qz * d_qz
    d_unit = d_sqx + d_sqy + d_sqz + d_sqw  # Correction factor if not normalized
    
    # Singularity at North Pole
    if d_test > 0.499 * d_unit:
        d_yaw = 2 * math.atan2(d_qx, d_qw)
        d_pitch = np.pi / 2
        d_roll = 0
        return d_roll, d_pitch, d_yaw

    # Singularity at South Pole
    if d_test < -0.499 * d_unit:
        d_yaw = -2 * math.atan2(d_qx, d_qw)
        d_pitch = -np.pi / 2
        d_roll = 0
        return d_roll, d_pitch, d_yaw

    # Roll (x-axis rotation)
    d_roll = math.atan2(2.0 * (d_qw * d_qx + d_qy * d_qz), 1.0 - 2.0 * (d_sqx + d_sqy))
    
    # Pitch (y-axis rotation)
    # CORRECTION from the docs here, math.asin(2*test/unit) is wrong (in our case)
    # pitch = 2*math.atan2(1.0 + 2.0 * (qw * qy - qx * qz),1.0 - 2.0 * (qw * qy - qx * qz)) - np.pi/2
    d_pitch = math.asin(2 * (d_qy * d_qw - d_qz * d_qx) / d_unit)
    
    # Yaw (z-axis rotation)
    d_yaw = math.atan2(2.0 * (d_qw * d_qz + d_qx * d_qy), 1.0 - 2.0 * (d_sqy + d_sqz))
    
    return d_roll, d_pitch, d_yaw

# %% ../nbs/00_core.ipynb 20
def f_test_f_q2rollPitchYaw():
    "Test the quaternion to roll, pitch, yaw conversion function"
    
    # Test case 1: Normal quaternion
    v_quat1 = np.array([0.1, 0.2, 0.3, 0.4])
    d_roll1, d_pitch1, d_yaw1 = f_q2rollPitchYaw(v_quat1)
    test_close(d_roll1, 0.5760, eps=1e-4)
    test_close(d_pitch1, 0.5899, eps=1e-4)
    test_close(d_yaw1, 1.3770, eps=1e-4)

    # Test case 2: Quaternion representing no rotation
    v_quat2 = np.array([0, 0, 0, 1])
    d_roll2, d_pitch2, d_yaw2 = f_q2rollPitchYaw(v_quat2)
    test_close(d_roll2, 0, eps=1e-6)
    test_close(d_pitch2, 0, eps=1e-6)
    test_close(d_yaw2, 0, eps=1e-6)

    # Test case 3: Quaternion near North Pole singularity
    v_quat3 = np.array([0, 1/np.sqrt(2), 0, 1/np.sqrt(2)])
    _, d_pitch3, _ = f_q2rollPitchYaw(v_quat3)
    test_close(d_pitch3, np.pi/2, eps=1e-6)

    # Test case 4: Quaternion near South Pole singularity
    v_quat4 = np.array([0, -1/np.sqrt(2), 0, 1/np.sqrt(2)])
    _, d_pitch4, _ = f_q2rollPitchYaw(v_quat4)
    test_close(d_pitch4, -np.pi/2, eps=1e-6)

    # Test case 5: Invalid input
    test_fail(lambda: f_q2rollPitchYaw(np.array([0.1, 0.2, 0.3])), contains='must be a 4-element array')

# Example in documentation
v_quat_example = np.array([0.1, 0.2, 0.3, 0.4])
d_roll_ex, d_pitch_ex, d_yaw_ex = f_q2rollPitchYaw(v_quat_example)
# print(f"Roll: {d_roll_ex:.4f}, Pitch: {d_pitch_ex:.4f}, Yaw: {d_yaw_ex:.4f}")

# %% ../nbs/00_core.ipynb 23
def f_rollPitchYaw2q(d_roll: float, d_pitch: float, d_yaw: float) -> Tuple[float, float, float, float]:
    r"""
    Convert roll, pitch, and yaw angles (in radians) to a quaternion using the 3-2-1 sequence.
    
    ::: {.callout-note}
    The 3-2-1 or YZX convention is the common one used for Tait-Bryan or nautical angles.
    This function performs the inverse operation of `f_q2rollPitchYaw`.
    :::

    **Parameters**  
     \ \  - `d_roll`: The roll angle in radians  
     \ \  - `d_pitch`: The pitch angle in radians  
     \ \  - `d_yaw`: The yaw angle in radians  

    **Returns**  
    \ \  - Tuple representing the quaternion in the form (qx, qy, qz, qw)

    **Example**
    
    ```python
    # Convert Roll, Pitch, and Yaw to quaternion
    d_roll, d_pitch, d_yaw = 0.1, 0.2, 0.3
    v_quat = f_rollPitchYaw2q(d_roll, d_pitch, d_yaw)

    # Display the results
    print(f"Quaternion: ({v_quat:.4f}, {v_quat:.4f}, {v_quat:.4f}, {v_quat:.4f})")
    ```
    
    ::: {.callout-tip collapse=true}
    #### **More details on Euler Angles (Roll, Pitch, Yaw) to quaternion conversion**

    Given Euler angles (roll φ, pitch θ, yaw ψ) in radians, we can convert them to a quaternion $q = (q_x, q_y, q_z, q_w)$ using the following method:

    1. Calculate intermediate values:
       - cy = cos(yaw * 0.5)
       - sy = sin(yaw * 0.5)
       - cp = cos(pitch * 0.5)
       - sp = sin(pitch * 0.5)
       - cr = cos(roll * 0.5)
       - sr = sin(roll * 0.5)

    2. Compute quaternion components:
       - qw = cr * cp * cy + sr * sp * sy
       - qx = sr * cp * cy - cr * sp * sy
       - qy = cr * sp * cy + sr * cp * sy
       - qz = cr * cp * sy - sr * sp * cy

    This method ensures accurate conversion from Euler angles to quaternions, maintaining the 3-2-1 (YZX) rotation sequence.
    :::

    **References**  
    \ \ 1. [Euler Angles to Quaternion Conversion](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion)  
    \ \ 2. [Understanding 3D Rotations](https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm)  

    """
    # Abbreviations for the various angular functions
    d_cr = np.cos(d_roll * 0.5)
    d_sr = np.sin(d_roll * 0.5)
    d_cp = np.cos(d_pitch * 0.5)
    d_sp = np.sin(d_pitch * 0.5)
    d_cy = np.cos(d_yaw * 0.5)
    d_sy = np.sin(d_yaw * 0.5)

    # Quaternion components
    d_qw = d_cr * d_cp * d_cy + d_sr * d_sp * d_sy
    d_qx = d_sr * d_cp * d_cy - d_cr * d_sp * d_sy
    d_qy = d_cr * d_sp * d_cy + d_sr * d_cp * d_sy
    d_qz = d_cr * d_cp * d_sy - d_sr * d_sp * d_cy

    return d_qx, d_qy, d_qz, d_qw

# %% ../nbs/00_core.ipynb 24
def f_test_f_rollPitchYaw2q():
    "Test the roll, pitch, yaw to quaternion conversion function"
    
    # Test case 1: Zero angles
    v_quat1 = f_rollPitchYaw2q(0, 0, 0)
    test_close(v_quat1, (0, 0, 0, 1), eps=1e-6)

    # Test case 2: 90 degree rotations
    v_quat2 = f_rollPitchYaw2q(np.pi/2, 0, 0)
    test_close(v_quat2, (np.sqrt(2)/2, 0, 0, np.sqrt(2)/2), eps=1e-6)

    v_quat3 = f_rollPitchYaw2q(0, np.pi/2, 0)
    test_close(v_quat3, (0, np.sqrt(2)/2, 0, np.sqrt(2)/2), eps=1e-6)

    v_quat4 = f_rollPitchYaw2q(0, 0, np.pi/2)
    test_close(v_quat4, (0, 0, np.sqrt(2)/2, np.sqrt(2)/2), eps=1e-6)

    # Test case 3: Arbitrary angles
    v_quat5 = f_rollPitchYaw2q(0.1, 0.2, 0.3)
    expected_quat5 = (0.0478, 0.0986, 0.1449, 0.9833)
    test_close(v_quat5, expected_quat5, eps=1e-4)

    # Test case 4: Inverse of f_q2rollPitchYaw
    v_quat6 = np.array([0.1, 0.2, 0.3, 0.4])
    d_roll6, d_pitch6, d_yaw6 = f_q2rollPitchYaw(v_quat6)
    v_quat6_reconstructed = f_rollPitchYaw2q(d_roll6, d_pitch6, d_yaw6)
    test_close(v_quat6, v_quat6_reconstructed, eps=1e-6)

# Example in documentation
d_roll_ex, d_pitch_ex, d_yaw_ex = 0.1, 0.2, 0.3
v_quat_ex = f_rollPitchYaw2q(d_roll_ex, d_pitch_ex, d_yaw_ex)
# print(f"Quaternion: ({v_quat_ex[0]:.4f}, {v_quat_ex[1]:.4f}, {v_quat_ex[2]:.4f}, {v_quat_ex[3]:.4f})")

# %% ../nbs/00_core.ipynb 25
def f_nav2target_pos(m_position: np.ndarray, m_YawPitchRoll: np.ndarray, m_estimated_ranges: np.ndarray, 
                     d_angle_rad: float) -> np.ndarray:
    r"""
    Calculate the target's position in global coordinates based on local sensor information.
    
    ::: {.callout-note}
    This function transforms local sensor measurements into global coordinates, accounting 
    for the AUV's position and orientation.
    :::

    **Parameters**  
     \ \  - `m_position`: Array of AUV's position (x, y, z), shape (3,) or (3, 1)  
     \ \  - `m_YawPitchRoll`: Array of yaw, pitch, roll angles in radians, shape (3,)  
     \ \  - `m_estimated_ranges`: Array of estimated ranges to the target, shape (n,) or (n, 1)  
     \ \  - `d_angle_rad`: Angle in radians representing the sensor's beam direction  

    **Returns**  
    \ \  - np.ndarray: Target's position in global coordinates, shape (3, n) or (3, 1)  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    
    # AUV position and orientation
    m_position = np.array([10, 20, -5])  # AUV at (10, 20, -5)
    m_YawPitchRoll = np.array([np.pi/4, np.pi/6, 0])  # 45° yaw, 30° pitch, 0° roll

    # Sensor measurements
    m_estimated_ranges = np.array([50, 60, 70])  # Three range measurements
    d_angle_rad = np.pi/6  # 30° sensor angle

    # Calculate target positions
    m_target_positions = f_nav2target_pos(m_position, m_YawPitchRoll, m_estimated_ranges, d_angle_rad)

    # Visualize
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(*m_position, c='r', s=100, label='AUV')
    ax.scatter(m_target_positions, m_target_positions, m_target_positions, c='b', label='Targets')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.legend()
    plt.title('AUV and Estimated Target Positions')
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Detailed explanation of the coordinate transformation process**

    1. **Input Preparation**:
       - Ensure `m_position` and `m_estimated_ranges` are 2D arrays for consistent processing.

    2. **Local Coordinate Calculation**:
       - Calculate x and y components in the sensor's local coordinate system using polar to Cartesian conversion:
         * x = range * cos(angle)
         * y = range * sin(angle)

    3. **Rotation Matrices**:
       - Create 3D rotation matrices for yaw, pitch, and roll using Euler angles.

    4. **Local to AUV Frame Transformation**:
       - Apply roll, pitch, and yaw rotations sequentially to transform from sensor frame to AUV body frame.

    5. **AUV Frame to Global Frame Transformation**:
       - Add the AUV's global position to the rotated local coordinates.

    This process effectively transforms the sensor's polar measurements (range and angle) 
    into global Cartesian coordinates, accounting for the AUV's position and orientation in 3D space.
    :::

    **References**  
    \ \ 1. [Coordinate Transformations in Robotics](https://cseweb.ucsd.edu/classes/wi18/cse167-a/lec3.pdf)  
    \ \ 2. [SLAM Underwater Vehicle Navigation](https://ieeexplore.ieee.org/document/1638022)  

    """
    
    # Ensure position and ranges are numpy arrays
    m_position = np.array(m_position)
    if m_position.ndim == 1:
        m_position = np.reshape(m_position, (np.shape(m_position)[0], 1))

    if m_estimated_ranges.ndim == 1:
        m_estimated_ranges = np.reshape(m_estimated_ranges, (np.shape(m_estimated_ranges)[0], 1))

    # Calculate the x and y components of the target's position in the local coordinate system
    m_xTargetLocal = m_estimated_ranges * np.cos(d_angle_rad)
    m_yTargetLocal = m_estimated_ranges * np.sin(d_angle_rad)

    # Extract yaw, pitch, and roll angles
    d_yaw, d_pitch, d_roll = m_YawPitchRoll

    # Create rotation matrices for yaw, pitch, and roll
    m_Ryaw = np.array([[np.cos(d_yaw), -np.sin(d_yaw), 0],
                        [np.sin(d_yaw), np.cos(d_yaw), 0],
                        [0, 0, 1]])

    m_Rpitch = np.array([[np.cos(d_pitch), 0, np.sin(d_pitch)],
                          [0, 1, 0],
                          [-np.sin(d_pitch), 0, np.cos(d_pitch)]])

    m_Rroll = np.array([[1, 0, 0],
                         [0, np.cos(d_roll), -np.sin(d_roll)],
                         [0, np.sin(d_roll), np.cos(d_roll)]])

    # Calculate the adjusted position of the target in the AUV's local coordinate system
    m_target_position_local = np.squeeze(np.array([m_xTargetLocal, m_yTargetLocal, np.zeros(np.shape(m_xTargetLocal))]))
    m_target_position_local = np.dot(m_Rroll, m_target_position_local)
    m_target_position_local = np.dot(m_Rpitch, m_target_position_local)
    m_target_position_local = np.dot(m_Ryaw, m_target_position_local)

    if m_target_position_local.ndim == 1:
        m_target_position_local = np.reshape(m_target_position_local, (np.shape(m_target_position_local)[0], 1))

    # Add AUV's position to get the global position of the target, accounting for the AUV's Z coordinate
    m_target_position_global = np.array(m_position) + m_target_position_local

    return m_target_position_global

# %% ../nbs/00_core.ipynb 27
def f_hyster(m_image: np.ndarray, d_low: float, d_high: float, d_overlap: float = 0.5, d_window_size: int = 100) -> np.ndarray:
    r"""
    Apply hysteresis thresholding to an image with window-based processing.
    
    ::: {.callout-note}
    This function performs hysteresis thresholding on an input image using a sliding window approach. 
    It normalizes the image, processes it in overlapping windows, and applies local thresholding.
    :::

    **Parameters**  
     \ \  - `m_image`: Input image matrix  
     \ \  - `d_low`: Low threshold factor  
     \ \  - `d_high`: High threshold factor  
     \ \  - `d_overlap`: Overlap percentage between windows (default: 0.5)  
     \ \  - `d_window_size`: Size of the processing window (default: 100)  

    **Returns**  
    \ \  - np.ndarray: Hysteresis thresholded image matrix  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    from skimage import data
    
    # Load a sample image
    m_image = data.camera()

    # Apply hysteresis thresholding
    m_result = f_hyster(m_image, d_low=0.1, d_high=0.5, d_overlap=0.5, d_window_size=100)

    # Display results
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 6))
    ax1.imshow(m_image, cmap='gray')
    ax1.set_title('Original Image')
    ax2.imshow(m_result, cmap='gray')
    ax2.set_title('Hysteresis Thresholded Image')
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the hysteresis thresholding process**

    1. **Image Normalization**:
       - Normalizes the input image to the range [0, 1].

    2. **Window-based Processing**:
       - Processes the image in overlapping windows.
       - Window size is determined by `d_window_size`.
       - Overlap between windows is controlled by `d_overlap`.

    3. **Local Thresholding**:
       - For each window:
         a. Calculates local high and low thresholds based on the maximum intensity in the window.
         b. Applies hysteresis thresholding using `filters.apply_hysteresis_threshold`.
         c. Retains only the pixels that pass the thresholding.

    4. **Result Compilation**:
       - Combines the processed windows to form the final thresholded image.

    5. **Threshold Factors**:
       - `d_low` and `d_high` determine the lower and upper thresholds as fractions of the local maximum intensity.

    This approach allows for adaptive thresholding that accounts for local intensity variations across the image.
    :::

    **References**  
    \ \ 1. [Hysteresis Thresholding](https://scikit-image.org/docs/dev/auto_examples/filters/plot_hysteresis.html)  
    \ \ 2. [Window-based Image Processing](https://en.wikipedia.org/wiki/Window_function)  

    """
    # Normalize the image to the range [0, 1]
    m_image = (m_image - np.min(m_image)) / (np.max(m_image) - np.min(m_image))
    m_aux_tot = m_image.copy()

    for d_ind in range(0, np.shape(m_image)[1], int(np.round(d_window_size * d_overlap))):
        # Extract the local window from the image
        m_aux_loc = m_image[:, d_ind : d_ind + d_window_size].copy()

        # Calculate high and low thresholds based on local max intensity
        d_high_thresh = (np.max(m_aux_loc) * d_high)  # Remove very low intensity locally compared to global intensity
        d_low_thresh = np.max(m_aux_loc) * d_low

        # Apply hysteresis thresholding
        m_hyst = filters.apply_hysteresis_threshold(m_aux_loc, d_low_thresh, d_high_thresh)

        # Create a zero-filled auxiliary matrix and copy thresholded values
        m_aux = np.zeros(np.shape(m_aux_loc))
        m_aux[m_hyst] = m_aux_loc[m_hyst]

        # Update the total image with the thresholded values
        m_aux_tot[:, d_ind : d_ind + int(np.round(d_window_size * d_overlap))] = m_aux[:, 0 : int(np.round(d_window_size * d_overlap))]

    return m_aux_tot

# %% ../nbs/00_core.ipynb 29
def f_save(s_filename: str, dic_globals: dict, *l_args: str) -> None:
    r"""
    Save specified variables from the global dictionary to a pickle file.
    
    ::: {.callout-note}
    This function selectively saves variables from a provided global dictionary to a file using 
    pickle serialization. The saved variables can be later retrieved using the `f_load` function.
    :::

    **Parameters**  
     \ \  - `s_filename`: String specifying the name of the file to save the variables to  
     \ \  - `dic_globals`: Dictionary representing the global namespace containing all variables  
     \ \  - `*l_args`: Variable number of strings, each representing the name of a variable to be saved  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import numpy as np
    
    # Create sample variables in the global namespace
    v_array = np.array([1, 2, 3, 4, 5])
    dic_sample = {'a': 1, 'b': 2, 'c': 3}
    s_message = "Hello, World!"

    # Get the global dictionary
    dic_globals = globals()

    # Save the variables
    f_save('my_variables.pkl', dic_globals, 'v_array', 'dic_sample', 's_message')

    # These can be loaded later using:
    # f_load('my_variables.pkl', globals())
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the saving process**

    1. **Variable Selection**:
       - Creates an empty dictionary `dic` to store selected variables.
       - Iterates through the variable names provided in `*l_args`.
       - Retrieves each variable from `dic_globals` and stores it in `dic`.

    2. **File Writing**:
       - Opens the specified file (`s_filename`) in binary write mode.
       - Uses `pickle.dump()` to serialize the `dic` dictionary and write it to the file.

    3. **Serialization**:
       - Employs pickle for serialization, allowing saving of complex Python objects.
       - The resulting file is in binary format and not human-readable.

    4. **Scope Consideration**:
       - Only saves variables present in the provided `dic_globals` dictionary.
       - Allows for more flexible use of the function in different contexts.

    5. **File Naming**:
       - The filename should include the desired file extension (e.g., '.pkl' for pickle files).
       - Ensure write permissions in the target directory.
    :::

    **References**  
    \ \ 1. [Python Pickle Module](https://docs.python.org/3/library/pickle.html)  
    \ \ 2. [Python Global and Local Scope](https://docs.python.org/3/tutorial/classes.html#python-scopes-and-namespaces)  

    """
    dic = {}
    for s_var in l_args:
        # Copy over desired values from the globals dictionary
        dic[s_var] = dic_globals[s_var]
    with open(s_filename, 'wb') as f:
        # Put them in the file 
        pickle.dump(dic, f)

# %% ../nbs/00_core.ipynb 30
def f_load(s_filename: str, dic_globals: dict) -> None:
    r"""
    Load variables from a pickle file and update the provided global dictionary.
    
    ::: {.callout-note}
    This function retrieves variables saved by `f_save` from a pickle file and updates the 
    provided global dictionary with these variables. It uses the pickle module for deserialization.
    :::

    **Parameters**  
     \ \  - `s_filename`: String specifying the name of the file to load variables from  
     \ \  - `dic_globals`: Dictionary representing the global namespace to update with loaded variables  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    # Assume we previously saved variables using:
    # f_save('my_variables.pkl', globals(), 'v_array', 'dic_sample', 's_message')

    # To load these variables:
    dic_globals = globals()
    f_load('my_variables.pkl', dic_globals)

    # Now v_array, dic_sample, and s_message are available in the global namespace
    print(v_array)
    print(dic_sample)
    print(s_message)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the loading process**

    1. **File Reading**:
       - Opens the specified file (`s_filename`) in binary read mode.
       - Uses `pickle.load()` to deserialize the content of the file into a dictionary.

    2. **Variable Restoration**:
       - The loaded dictionary contains variable names as keys and their values.
       - Uses the `update()` method of the provided global dictionary (`dic_globals`) 
         to add or update variables with the loaded values.

    3. **Deserialization**:
       - Pickle is used for deserialization, allowing restoration of complex Python objects.

    4. **Scope Consideration**:
       - The function updates the provided dictionary, which is expected to be the global namespace.
       - This approach allows for more flexible use of the function in different contexts.

    5. **Security Note**:
       - Pickle can execute arbitrary code during deserialization.
       - Only load files from trusted sources to avoid potential security risks.
    :::

    **References**  
    \ \ 1. [Python Pickle Module](https://docs.python.org/3/library/pickle.html)  
    \ \ 2. [Python Global and Local Scope](https://docs.python.org/3/tutorial/classes.html#python-scopes-and-namespaces)  

    """
    with open(s_filename, 'rb') as f:
        dic_variables = pickle.load(f)
        # Update the global dictionary with the loaded variables
        dic_globals.update(dic_variables)

# %% ../nbs/00_core.ipynb 31
def f_generate_unique_filename(s_directory: str) -> str:
    r"""
    Generate a unique filename that does not already exist in the current directory.
    
    ::: {.callout-note}
    This function checks if a given filename exists in the current directory. If it does, 
    it appends a counter to the base filename until a unique filename is found.
    :::

    **Parameters**  
     \ \  - `s_directory`: String representing the filename (without the path) to use as the base for the new filename  

    **Returns**  
    \ \  - String representing the new unique filename  

    **Example**
    
    ```python
    import os

    # Create a test file
    with open('test_file.txt', 'w') as f:
        f.write('This is a test file')

    # Generate a unique filename
    s_new_filename = f_generate_unique_filename('test_file.txt')
    print(f"Generated unique filename: {s_new_filename}")

    # Try again with the same base filename
    s_another_filename = f_generate_unique_filename('test_file.txt')
    print(f"Generated another unique filename: {s_another_filename}")

    # Clean up
    os.remove('test_file.txt')
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the unique filename generation process**

    1. **Existence Check**:
       - Checks if the given filename already exists in the current directory.

    2. **Filename Parsing**:
       - If the file exists, splits the filename into base name and extension.

    3. **Counter Initialization**:
       - Initializes a counter to 1 for appending to the base filename.

    4. **Unique Name Generation**:
       - Enters a loop to generate new filenames by appending the counter to the base name.
       - Checks if each generated filename exists in the current directory.
       - Increments the counter if the generated filename already exists.

    5. **Return Value**:
       - Returns the original filename if it doesn't exist in the directory.
       - Returns the first generated filename that doesn't exist in the directory.

    This approach ensures that the function always returns a filename that can be safely 
    used without overwriting existing files.
    :::

    **References**  
    \ \ 1. [Python os.path Module](https://docs.python.org/3/library/os.path.html)  
    \ \ 2. [File and Directory Operations in Python](https://docs.python.org/3/library/os.html#files-and-directories)  

    """
    if os.path.exists(s_directory):
        s_base, s_ext = os.path.splitext(s_directory)
        d_counter = 1
    
        while True:
            s_new_filename = f"{s_base}_{d_counter}{s_ext}"
            s_new_full_path = os.path.join('.', s_new_filename)
            if not os.path.exists(s_new_full_path):
                # The new filename doesn't exist, so we can use it
                return s_new_filename
            d_counter += 1
    else: 
        return s_directory

# %% ../nbs/00_core.ipynb 32
def f_save_var(s_directory: str, dic_globals: dict, *l_args: str) -> None:
    r"""
    Save specified variables from the global dictionary to individual pickle files in a unique directory.
    
    ::: {.callout-note}
    This function creates a unique directory and saves each specified variable as a separate pickle file 
    within that directory. It uses the `f_generate_unique_filename` function to ensure a unique directory name.
    :::

    **Parameters**  
     \ \  - `s_directory`: String specifying the base name for the directory to save the files in  
     \ \  - `dic_globals`: Dictionary representing the global namespace containing all variables  
     \ \  - `*l_args`: Variable number of strings, each representing the name of a variable to be saved  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import numpy as np
    import os
    import shutil
    
    # Create sample variables in the global namespace
    v_array = np.array([1, 2, 3, 4, 5])
    dic_sample = {'a': 1, 'b': 2, 'c': 3}
    s_message = "Hello, World!"

    # Get the global dictionary
    dic_globals = globals()

    # Save the variables
    f_save_var('saved_variables', dic_globals, 'v_array', 'dic_sample', 's_message')

    # Check the contents of the created directory
    s_created_dir = [d for d in os.listdir() if d.startswith('saved_variables')]
    print(f"Created directory: {s_created_dir}")
    print("Saved files:")
    for s_file in os.listdir(s_created_dir):
        print(f" - {s_file}")

    # Clean up (remove the created directory)
    shutil.rmtree(s_created_dir)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the variable saving process**

    1. **Directory Creation**:
       - Uses `f_generate_unique_filename` to create a unique directory name.
       - Creates the directory using `os.makedirs` with `exist_ok=True` to avoid errors if the directory already exists.

    2. **Variable Saving**:
       - Iterates through each variable name provided in `*l_args`.
       - For each variable:
         a. Constructs a filename by joining the directory path and the variable name with a '.pkl' extension.
         b. Opens the file in binary write mode.
         c. Uses `pickle.dump()` to serialize the variable and write it to the file.

    3. **File Naming**:
       - Each variable is saved in a separate file named after the variable with a '.pkl' extension.

    4. **Scope Consideration**:
       - Only saves variables present in the provided `dic_globals` dictionary.

    5. **Error Handling**:
       - The function will raise an error if a specified variable is not found in `dic_globals`.

    This approach allows for organized storage of multiple variables, each in its own file within a unique directory.
    :::

    **References**  
    \ \ 1. [Python Pickle Module](https://docs.python.org/3/library/pickle.html)  
    \ \ 2. [Python os Module](https://docs.python.org/3/library/os.html)  
    \ \ 3. [Python File I/O](https://docs.python.org/3/tutorial/inputoutput.html#reading-and-writing-files)  

    """
    # Ensure to have a folder or file that doesn't exist
    s_directory = f_generate_unique_filename(s_directory)

    print(f"Creation of the folder {s_directory}...")
    os.makedirs(s_directory, exist_ok=True)
    
    for s_var in l_args:
        s_filename = os.path.join(s_directory, f"{s_var}.pkl")
        with open(s_filename, 'wb') as f:
            pickle.dump(dic_globals[s_var], f)

# %% ../nbs/00_core.ipynb 33
def f_load_var(s_path: str, d_glob: Dict, l_var: Optional[List[str]] = None, b_verbose=False) -> None:
    r"""
    Load variables from pickle files into the global namespace.
    
    ::: {.callout-note}
    This function loads variables from pickle files located in the specified path.
    If a list of variable names is provided, it loads only those variables.
    If no list is provided, it loads all pickle files in the directory.
    :::

    **Parameters**  
     \ \  - `s_path`: String path to the directory containing pickle files  
     \ \  - `d_glob`: Dictionary representing the global namespace (usually globals())  
     \ \  - `l_var`: Optional list of variable names to load. If None, loads all pickle files.  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import os
    
    # Load specific variables
    l_var = ['m_pos_auv', 'm_ypr']
    glob = globals()
    f_load_var("./data/Pickle_dataset/", glob, l_var)
    del glob

    # Load all pickle files in the directory
    glob = globals()
    f_load_var("./data/Pickle_dataset/", glob)
    del glob
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on variable loading process**

    1. If `l_var` is not provided, the function will load all .pkl files in the directory.
    2. For each variable to be loaded:
       - Constructs the full file path.
       - Checks if the file exists.
       - If it exists, loads the pickle file and assigns it to the global namespace.
       - If it doesn't exist, prints a warning message.
    3. The function modifies the global namespace directly through the `d_glob` parameter.
    :::

    **References**  
    \ \ 1. [Python pickle module](https://docs.python.org/3/library/pickle.html)  
    \ \ 2. [Python globals() function](https://docs.python.org/3/library/functions.html#globals)  
    """
    if l_var is None:
        # If no list is provided, get all .pkl files in the directory
        l_var = [f for f in os.listdir(s_path) if f.endswith('.pkl')]
        print(f"Loading all the .pkl variables from {s_path}...")
        
    for s_var in l_var:
        s_filename = s_var if s_var.endswith('.pkl') else f"{s_var}.pkl"
        s_filepath = os.path.join(s_path, s_filename)
        
        if os.path.exists(s_filepath):
            with open(s_filepath, 'rb') as file:
                d_glob[s_var.replace('.pkl', '')] = pickle.load(file)
            if b_verbose:
                print(f"Loaded: {s_var}")
        else:
            print(f"Warning: {s_filepath} does not exist.")


# %% ../nbs/00_core.ipynb 35
def f_pointCloud(m_pos: np.ndarray, m_ypr: np.ndarray, m_sonar: np.ndarray, v_angle: np.ndarray, 
                 d_max_range: float, d_thresh: float = 0, b_return_intensity: bool = False) -> Union[np.ndarray, Tuple[np.ndarray, List[np.ndarray]]]:
    r"""
    Create a 3D point cloud from sensor data, typically used in underwater sonar applications.
    
    ::: {.callout-note}
    This function processes sonar data to generate a 3D point cloud. It handles sensor position, orientation, 
    and intensity thresholding.
    :::

    **Parameters**  
     \ \  - `m_pos`: Array of sensor positions (x, y, z), shape (n, 3)  
     \ \  - `m_ypr`: Array of sensor orientations (yaw, pitch, roll) in radians, shape (n, 3)  
     \ \  - `m_sonar`: Array of sonar intensity data, shape (m, n) where m is the number of range bins  
     \ \  - `v_angle`: Array of sonar beam angles in radians, shape (n,)  
     \ \  - `d_max_range`: Maximum range of the sonar sensor  
     \ \  - `d_thresh`: Intensity threshold for point inclusion (default: 0)  
     \ \  - `b_return_intensity`: Flag to return intensity values (default: False)  

    **Returns**  
    \ \  - If `b_return_intensity` is False: np.ndarray of shape (k, 3) representing the 3D point cloud  
    \ \  - If `b_return_intensity` is True: Tuple (point_cloud, intensities), where intensities is a list of arrays  

    **Example**
    
    ```python
    import numpy as np
    from scipy.spatial.transform import Rotation as R
    import matplotlib.pyplot as plt
    
    # Generate sample data
    n_samples = 100
    m_pos = np.random.rand(n_samples, 3) * 10
    m_ypr = np.random.rand(n_samples, 3) * np.pi
    m_sonar = np.random.rand(50, n_samples) * 100  # 50 range bins
    v_angle = np.linspace(-np.pi/4, np.pi/4, n_samples)
    d_max_range = 50.0

    # Generate point cloud
    point_cloud = f_pointCloud(m_pos, m_ypr, m_sonar, v_angle, d_max_range, d_thresh=50)

    # Visualize point cloud
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2], c='b', marker='.')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title('3D Point Cloud from Sonar Data')
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Detailed explanation of the point cloud generation process**

    1. **Data Preparation**:
       - Invert the Z-axis of positions to match underwater conventions.
       - Generate a range array based on the maximum range and sonar data shape.
       - Apply a 180° roll rotation to the orientation data for correct alignment.

    2. **Intensity Thresholding**:
       - If `d_thresh` is -1, only the maximum intensity points are kept.
       - Otherwise, points with intensity above `d_thresh` are retained.

    3. **Point Cloud Generation**:
       - For each sonar measurement:
         a. Calculate target positions using the `f_nav2target_pos` function (not shown here).
         b. This function likely converts polar coordinates (range, angle) to Cartesian coordinates.
       - Concatenate all calculated positions into a single point cloud array.

    4. **Intensity Handling**:
       - If `b_return_intensity` is True, the function also returns the intensity values for each point.

    This process effectively transforms raw sonar data into a 3D spatial representation, 
    accounting for sensor movement and orientation changes during data collection.
    :::

    **References**  
    \ \ None
    
    """
    # Put the Z values to negative values
    m_aux = m_pos.copy()
    m_aux[:, 2] = -m_aux[:, 2]

    # Get the range
    v_range = np.linspace(0, d_max_range, np.shape(m_sonar)[0])

    # Create a rotation object and apply a 180° roll rotation
    rot_r = R.from_euler("ZYX", m_ypr + np.array([0, 0, np.pi]))  # Yaw=0, Pitch=0, Roll=180
    m_ypr = rot_r.as_euler("ZYX")  # Final yaw, pitch, and roll

    if d_thresh == -1:
        # Keep only max intensity
        l_range = [v_range[d_elem] for d_elem in np.argmax(m_sonar, axis=0)]
        l_intensity = [d_elem for d_elem in np.max(m_sonar, axis=0)]
    else:
        # Only keep the range where intensity > d_thresh
        m_mask_intensity = (m_sonar > d_thresh) * 1  # Create intensity mask
        l_range = [v_range[v_elems == 1] for v_elems in m_mask_intensity.T]

        # For display if needed
        l_intensity = [v_elem[v_elem > d_thresh] for v_elem in m_sonar.T]

    l_estimated_target_position = []
    for d_ind, v_estimated_ranges in enumerate(l_range):
        l_estimated_target_position.append(f_nav2target_pos(m_aux[d_ind, :], m_ypr[d_ind, :], v_estimated_ranges,
                                                            v_angle[d_ind]).T)

    m_point_cloud = np.concatenate(l_estimated_target_position)

    if b_return_intensity:
        return m_point_cloud, l_intensity

    return m_point_cloud

# %% ../nbs/00_core.ipynb 39
def f_plot_rotated_axes(h_ax: plt.Axes, r_rotation: R, s_name: Optional[str] = None, t_offset: Tuple[float, float, float] = (0, 0, 0), d_scale: float = 1) -> None:
    r"""
    Plot rotated coordinate axes on a 3D plot based on a given rotation.
    
    ::: {.callout-note}
    This function visualizes the orientation of a rotated coordinate system. It helps visualize
    3D rotations and transformations.
    :::

    **Parameters**  
     \ \  - `h_ax`: The Matplotlib 3D axes object where the rotated axes will be plotted  
     \ \  - `r_rotation`: A scipy.spatial.transform.Rotation object representing the rotation  
     \ \  - `s_name`: Name for the axes (optional)  
     \ \  - `t_offset`: A 3-element tuple specifying the offset for the axes (default is (0, 0, 0))  
     \ \  - `d_scale`: Scale factor for the length of the axes (default is 1)  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import matplotlib.pyplot as plt
    from scipy.spatial.transform import Rotation as R
    
    # Create a 3D plot
    fig = plt.figure()
    h_ax = fig.add_subplot(111, projection='3d')

    # Create a rotation (45 degrees around x, 30 around y, and 60 around z)
    r_rotation = R.from_euler('xyz', [45, 30, 60], degrees=True)

    # Plot the rotated axes
    f_plot_rotated_axes(h_ax, r_rotation, s_name='Rotated Axes', d_scale=2)

    # Set plot limits and labels
    h_ax.set_xlim(-3, 3)
    h_ax.set_ylim(-3, 3)
    h_ax.set_zlim(-3, 3)
    h_ax.set_xlabel('X')
    h_ax.set_ylabel('Y')
    h_ax.set_zlabel('Z')

    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **More details on plotting rotated axes**

    The function performs the following steps to plot the rotated axes:

    1. Define colorblind-safe RGB colors for x, y, and z axes.
    2. For each axis (x, y, z):
       a. Create a line representing the axis.
       b. Apply the rotation to the line.
       c. Plot the rotated line.
       d. Add a text label for the axis.
    3. If a name is provided, add a text label at the origin of the rotated axes.

    The rotation is applied using the `apply` method of the `Rotation` object, which
    transforms the coordinates of the axis lines and text labels.

    The `d_scale` parameter allows adjusting the length of the plotted axes, which can be
    useful for visualizing rotations at different scales or in crowded plots.
    :::

    **References**  
    \ \ 1. [Matplotlib 3D plotting](https://matplotlib.org/stable/tutorials/toolkits/mplot3d.html)  
    \ \ 2. [SciPy Spatial Transform Rotation](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)  

    """
    colors = ("#FF6666", "#005533", "#1199EE")  # Colorblind-safe RGB colors for x, y, z axes
    t_offset = np.array(t_offset)

    # Loop over x, y, z axes to plot each axis
    for d_i, (c_color, s_axis_label) in enumerate(zip(colors, ['X', 'Y', 'Z'])):
        # Define a line for the axis (scaled)
        m_line = np.zeros((2, 3))
        m_line[1, d_i] = d_scale

        # Apply the rotation to the line
        m_line_rot = r_rotation.apply(m_line)
        m_line_plot = m_line_rot + t_offset

        # Plot the axis line
        h_ax.plot(m_line_plot[:, 0], m_line_plot[:, 1], m_line_plot[:, 2], color=c_color)

        # Define text position and plot text for the axis
        v_text_loc = m_line[1] * 1.2
        v_text_loc_rot = r_rotation.apply(v_text_loc)
        v_text_plot = v_text_loc_rot + t_offset
        h_ax.text(*v_text_plot, s_axis_label, color=c_color, va="center", ha="center")

    # Add a text label for the axes origin if provided
    if s_name is not None:
        h_ax.text(*t_offset, s_name, color="k", va="center", ha="center",
                  bbox={"fc": "w", "alpha": 0.8, "boxstyle": "circle"})

# %% ../nbs/00_core.ipynb 40
def f_plot_3d_trajectory(ax: plt.Axes, m_trajectory: np.ndarray, s_color: str = 'purple') -> None:
    r"""
    Plot a 3D trajectory on the specified axes, highlighting start and endpoints.

    **Parameters**  
     \ \  - `ax`: Matplotlib axes object for 3D plotting  
     \ \  - `m_trajectory`: 3D trajectory points array of shape (n, 3), where n is the number of points  
     \ \  - `s_color`: Color of the trajectory (default: 'purple')  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    
    # Create a sample 3D trajectory
    t = np.linspace(0, 10, 100)
    x = t * np.cos(t)
    y = t * np.sin(t)
    z = t
    m_trajectory = np.column_stack((x, y, z))

    # Create a 3D plot
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')

    # Plot the trajectory
    f_plot_3d_trajectory(ax, m_trajectory, s_color='blue')

    # Set labels and title
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title('3D Trajectory Visualization')

    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **More details on 3D trajectory plotting**

    The function performs the following steps to visualize the 3D trajectory:

    1. Plot the entire trajectory:
       - Uses `ax.plot()` to draw a continuous line representing the trajectory.
       - The color of the line is specified by the `s_color` parameter.

    2. Mark the start point:
       - Uses `ax.scatter()` to place a green marker at the first point of the trajectory.
       - This helps identify where the trajectory begins.

    3. Mark the end point:
       - Uses `ax.scatter()` to place a red marker at the last point of the trajectory.
       - This clearly shows where the trajectory ends.

    4. Add a legend:
       - Calls `ax.legend()` to add a legend to the plot, explaining the color coding.

    The function assumes that the input `m_trajectory` is a NumPy array where each row represents 
    a point in 3D space (x, y, z coordinates). The function is flexible and can handle trajectories 
    of any length.
    :::

    **References**  
    \ \ 1. [Matplotlib 3D plotting](https://matplotlib.org/stable/tutorials/toolkits/mplot3d.html)  
    \ \ 2. [NumPy array manipulation](https://numpy.org/doc/stable/user/basics.indexing.html)  
    
    """
    
    # Plot the 3D trajectory on the specified axes
    ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], label="Trajectory", color=s_color)

    # Scatter plot the start and end points of the trajectory
    ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green', label='Start')
    ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red', label='End')

    # Add a legend to the plot
    ax.legend()

# %% ../nbs/00_core.ipynb 41
def f_image_grid(l_imgs_path: List[str], d_max_nrow: int = 4, d_max_ncol: int = 5) -> None:
    r"""
    Load and display images in a grid layout from a list of image file paths.
    
    ::: {.callout-note}
    This function creates a visual grid of images, useful for comparing multiple images 
    or creating image catalogs. It automatically resizes images and adjusts the layout 
    for optimal viewing.
    :::

    **Parameters**  
     \ \  - `l_imgs_path`: List of strings, each representing a path to an image file  
     \ \  - `d_max_nrow`: Integer specifying the maximum number of rows in the grid (default: 4)  
     \ \  - `d_max_ncol`: Integer specifying the maximum number of columns in the grid (default: 5)  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import matplotlib.pyplot as plt
    from PIL import Image
    import numpy as np
    import os

    # Create some sample images
    def create_sample_image(s_filename, d_color):
        m_img = np.full((256, 512, 3), d_color, dtype=np.uint8)
        s_img = Image.fromarray(m_img)
        s_img.save(s_filename)
        return s_filename

    l_sample_images = [
        create_sample_image('red.png', (255, 0, 0)),
        create_sample_image('green.png', (0, 255, 0)),
        create_sample_image('blue.png', (0, 0, 255)),
        create_sample_image('yellow.png', (255, 255, 0)),
        create_sample_image('cyan.png', (0, 255, 255)),
        create_sample_image('magenta.png', (255, 0, 255))
    ]

    # Display the images in a grid
    f_image_grid(l_sample_images, d_max_nrow=2, d_max_ncol=3)
    plt.show()

    # Clean up sample images
    for s_file in l_sample_images:
        os.remove(s_file)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the image grid creation process**

    1. **Figure Creation**:
       - Creates a figure with a size of (30, 30/1.618), using the golden ratio for aesthetics.

    2. **Image Processing**:
       - Iterates through each image path in the provided list.
       - Opens each image using PIL (Python Imaging Library).
       - Resizes each image to a fixed size of 512x256 pixels for consistency.

    3. **Grid Layout**:
       - Uses matplotlib's subplot functionality to create a grid layout.
       - Adjusts the layout to minimize space between images (wspace and hspace).

    4. **Image Display**:
       - Creates a new subplot for each image.
       - Turns off the axis for each subplot to focus on the image content.
       - Displays the resized image in its respective subplot.

    5. **Flexibility**:
       - The function adapts to different numbers of images and grid sizes.
       - If there are fewer images than grid cells, empty cells will be left blank.

    This approach provides a flexible and visually appealing way to display multiple images 
    in a grid format, suitable for various image analysis and presentation tasks.
    :::

    **References**  
    \ \ 1. [Matplotlib Subplots](https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.subplots.html)  
    \ \ 2. [PIL (Python Imaging Library)](https://pillow.readthedocs.io/en/stable/)  
    \ \ 3. [Image Resizing Techniques](https://pillow.readthedocs.io/en/stable/reference/Image.html#PIL.Image.Image.resize)  

    """
    # Get the number of images in the list
    d_count = len(l_imgs_path)

    # Create a figure with a specific size
    plt.figure(figsize=(30, 30./1.618))

    # Iterate through the list of image paths
    for d_ix, s_path in enumerate(l_imgs_path):
        # Open the image
        img = Image.open(s_path)

        # Resize the image to a fixed size
        t_resize = (512, 256)
        img = img.resize(t_resize)

        # Adjust the layout of the subplots
        plt.tight_layout()
        plt.subplots_adjust(wspace=0.01, hspace=0.01)

        # Create a new subplot
        ax = plt.subplot(d_max_nrow, d_max_ncol, d_ix + 1)

        # Turn off the axis
        ax.axis('off')

        # Display the image in the subplot
        plt.imshow(img)

# %% ../nbs/00_core.ipynb 43
def f_plot_auv_orientation_subplots(
    m_pos_auv: np.ndarray,
    m_ypr: np.ndarray,
    d_step: int = 200,
    d_scale: float = 1.5,
    d_max_ticks: int = 5
) -> None:
    r"""
    Plot AUV orientation from multiple perspectives in a 2x2 subplot layout.
    
    ::: {.callout-note}
    This function visualizes the AUV's trajectory and orientation from four different 
    perspectives, providing a comprehensive view of the AUV's movement and rotation 
    in 3D space. It can display both 3D and "2D-like" views. For the "2D" views, 
    one axis is hidden to reduce clutter and provide a cleaner perspective.
    :::

    **Parameters**  
     \ \  - `m_pos_auv`: numpy.ndarray containing AUV position data (x, y, z coordinates)  
     \ \  - `m_ypr`: numpy.ndarray containing yaw, pitch, roll data for AUV orientation  
     \ \  - `d_step`: int, step size for sampling points along the trajectory (default=200)  
     \ \  - `d_scale`: float, scale factor for orientation axes (default=1.5)  
     \ \  - `d_max_ticks`: int, maximum number of ticks per axis to prevent clutter (default=5)

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import numpy as np
    from scipy.spatial.transform import Rotation as R
    import matplotlib.pyplot as plt

    # Generate sample data
    d_num_points = 2200
    m_pos_auv = np.column_stack((
        np.linspace(0, 100, d_num_points),  # x
        10 * np.sin(np.linspace(0, 4*np.pi, d_num_points)),  # y
        -5 * np.cos(np.linspace(0, 2*np.pi, d_num_points))   # z
    ))
    m_ypr = np.column_stack((
        np.linspace(0, 2*np.pi, d_num_points),   # yaw
        0.2 * np.sin(np.linspace(0, 4*np.pi, d_num_points)), # pitch
        0.1 * np.cos(np.linspace(0, 6*np.pi, d_num_points))  # roll
    ))

    f_plot_auv_orientation_subplots(m_pos_auv, m_ypr)
    ```

    ::: {.callout-tip collapse=true}
    #### Details on the AUV Orientation Plotting Process

    1. **Data Preparation**:
       - Uses the entire `m_pos_auv` and `m_ypr` arrays.
       - Inverts the z-axis values to represent depth if required.

    2. **Subplot Layout and Views**:
       - Creates a 2x2 subplot layout.
       - Each subplot has a predefined view angle:
         - Perspective: (30°, -45°)
         - Front: (0°, 0°)
         - Side: (0°, 90°)
         - Top: (90°, 90°)

    3. **Orientation Visualization**:
       - At each sampled point (based on `d_step`), plots rotated axes showing the AUV’s orientation.
       - Uses a rotation derived from `m_ypr` for each sampled index.

    4. **2D Views**:
       - For "2D-like" views (Front, Side, Top), hides one axis to reduce clutter.
       - This creates a cleaner 2D projection feel.

    5. **Trajectory Plotting**:
       - Plots the entire 3D trajectory of the AUV.

    6. **Tick Management**:
       - Limits the number of ticks on each axis to `d_max_ticks` to prevent clutter.
       - Uses `MaxNLocator` from Matplotlib to ensure a reasonable number of ticks.

    7. **Generalization**:
       - No hard-coded start/end indices; the entire input arrays are used.
       - Adjustable `d_step`, `d_scale`, and `d_max_ticks` parameters.

    8. **Testing and Validation**:
       - Test with different dataset sizes and steps.
       - Verify correct orientation and axes removal in "2D" views.

    :::

    **References**  
    \ \ 1. [Matplotlib 3D Plotting](https://matplotlib.org/stable/tutorials/toolkits/mplot3d.html)  
    \ \ 2. [SciPy Rotation](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)  
    \ \ 3. [Matplotlib Ticker API](https://matplotlib.org/stable/api/ticker_api.html)

    """
    from matplotlib.ticker import MaxNLocator

    # Copy trajectory and invert Z values if necessary
    m_trajectory = m_pos_auv.copy()
    m_trajectory[:, 2] = -m_trajectory[:, 2]

    # Define the view configurations: (elevation, azimuth), title, axis to hide
    l_views = [
        ((30, -45), "Perspective", None),  # Show all axes
        ((0, 0), "Front", 'x'),            # Hide X-axis for a front (Y-Z) view
        ((0, 90), "Side", 'y'),            # Hide Y-axis for a side (X-Z) view
        ((90, 90), "Top", 'z')             # Hide Z-axis for a top (X-Y) view
    ]

    # Create figure
    h_fig = plt.figure(figsize=(12, 12))

    # Determine the number of points
    d_num_points = m_pos_auv.shape[0]

    for d_i, (t_view, s_title, s_hide_axis) in enumerate(l_views, start=1):
        d_elev, d_azim = t_view
        h_ax = h_fig.add_subplot(2, 2, d_i, projection="3d", proj_type="ortho")

        # Plot rotated axes for each sampled point along the trajectory
        for d_ind in range(0, d_num_points, d_step):
            # Compute the rotation
            r_rotation = R.from_euler("ZYX", m_ypr[d_ind, :] + np.array([0, 0, np.pi]))

            # Plot rotated axes at the current sampled position
            f_plot_rotated_axes(
                h_ax,
                r_rotation,
                t_offset=(m_pos_auv[d_ind, 0], m_pos_auv[d_ind, 1], -m_pos_auv[d_ind, 2]),
                d_scale=d_scale
            )

        # Plot the entire trajectory
        f_plot_3d_trajectory(h_ax, m_trajectory)

        # Set aspect ratio, view angle, and title
        h_ax.set_aspect("auto", adjustable="box")
        h_ax.view_init(d_elev, d_azim)
        h_ax.set_title(s_title, fontsize=14)

        # Add axis labels
        h_ax.set_xlabel("X", labelpad=10)
        h_ax.set_ylabel("Y", labelpad=10)
        h_ax.set_zlabel("Z", labelpad=10)

        # Limit the number of ticks on each axis
        h_ax.xaxis.set_major_locator(MaxNLocator(nbins=d_max_ticks))
        h_ax.yaxis.set_major_locator(MaxNLocator(nbins=d_max_ticks))
        h_ax.zaxis.set_major_locator(MaxNLocator(nbins=d_max_ticks))

        # Hide one axis for 2D-like views, removing ticks and label
        if s_hide_axis == 'x':
            h_ax.xaxis.set_ticks([])
            h_ax.xaxis.label.set_visible(False)
        elif s_hide_axis == 'y':
            h_ax.yaxis.set_ticks([])
            h_ax.yaxis.label.set_visible(False)
        elif s_hide_axis == 'z':
            h_ax.zaxis.set_ticks([])
            h_ax.zaxis.label.set_visible(False)

    # Adjust spacing between subplots
    plt.subplots_adjust(left=0.01, right=0.99, top=0.95, bottom=0.05, wspace=0.05, hspace=0.1)

    # Display the plot
    plt.show()

# %% ../nbs/00_core.ipynb 46
def f_plot_auv_orientation_trajectory(m_pos_auv: np.ndarray, m_ypr: np.ndarray) -> None:
    r"""
    Plot AUV orientation over trajectory using interactive widgets.
    
    ::: {.callout-note}
    This function creates an interactive 3D plot showing the AUV's trajectory and its orientation 
    at different points along the path. It uses ipywidgets to allow dynamic exploration of the AUV's 
    orientation throughout its journey.
    :::

    **Parameters**  
     \ \  - `m_pos_auv`: numpy.ndarray containing AUV position data (x, y, z coordinates)  
     \ \  - `m_ypr`: numpy.ndarray containing yaw, pitch, roll data for AUV orientation  

    **Returns**  
    \ \  None

    **Example**
    
    ```python
    import numpy as np
    from scipy.spatial.transform import Rotation as R
    import matplotlib.pyplot as plt
    from ipywidgets import interact, IntSlider

    # Generate sample data
    d_num_points = 1000
    m_pos_auv = np.column_stack((
        np.linspace(0, 100, d_num_points),  # x
        10 * np.sin(np.linspace(0, 4*np.pi, d_num_points)),  # y
        -5 * np.cos(np.linspace(0, 2*np.pi, d_num_points))   # z
    ))
    m_ypr = np.column_stack((
        np.linspace(0, 2*np.pi, d_num_points),  # yaw
        0.2 * np.sin(np.linspace(0, 4*np.pi, d_num_points)),  # pitch
        0.1 * np.cos(np.linspace(0, 6*np.pi, d_num_points))   # roll
    ))

    # Plot interactive AUV orientation
    f_plot_auv_orientation_trajectory(m_pos_auv, m_ypr)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the interactive AUV orientation plotting process**

    1. **Data Preparation**:
       - Uses the entire input data range for visualization.
       - Inverts the z-axis values to represent depth.

    2. **Figure Setup**:
       - Creates a figure with a size of 12x9 inches and 100 DPI.
       - Sets up a 3D subplot with orthographic projection and custom aspect ratio.

    3. **Trajectory Plotting**:
       - Plots the entire AUV trajectory.
       - Marks the start (green) and end (red) points of the trajectory.

    4. **Orientation Data Preparation**:
       - Pre-computes rotation objects for each data point to improve performance.

    5. **Interactive Visualization**:
       - Uses `ipywidgets.interact` to create an interactive slider.
       - The slider controls which point along the trajectory is currently visualized.

    6. **Update Function**:
       - `update_plot` function is called whenever the slider value changes.
       - It updates the AUV orientation axes at the selected point on the trajectory.
       - Removes previous orientation axes and redraws them at the new position.

    7. **Plot Customization**:
       - Sets axis labels, title, and legend.
       - Adjusts plot limits to ensure the entire trajectory is visible.

    This approach provides an interactive way to explore the AUV's orientation changes 
    along its trajectory, allowing for detailed analysis of its movement and rotation.
    :::

    **References**  
    \ \ 1. [Matplotlib 3D Plotting](https://matplotlib.org/stable/tutorials/toolkits/mplot3d.html)  
    \ \ 2. [ipywidgets Documentation](https://ipywidgets.readthedocs.io/en/latest/)  

    """
    # Data setup
    d_start = 0
    d_end = np.shape(m_pos_auv)[0]
    d_step = 1

    # Get a trajectory
    m_trajectory = m_pos_auv[d_start:d_end+100,:].copy()

    # Put the depth values to negative
    m_trajectory[:,2] = -m_trajectory[:,2]

    h_fig_auv_orientation = plt.figure(figsize=(12,9), dpi=100, facecolor='w', edgecolor='k')
    h_ax = h_fig_auv_orientation.add_subplot(projection="3d", proj_type="ortho", box_aspect=(5,4,2))
    h_ax.set_title("AUV orientation over trajectory", {'fontsize': 30})

    h_ax.view_init(30,45)
    h_ax.dist = 0
    h_ax.invert_yaxis()

    l_r = [] 
    for d_ind in range(d_start, d_end, d_step):
        l_r.append(R.from_euler("ZYX", m_ypr[d_ind,:] + np.array([0, 0, np.pi])))

    m_data = m_pos_auv
    h_line, = h_ax.plot([], [], [])

    def update_plot(d_num):
        # Remove the previous AUV axis
        for h_a in h_ax.lines[-4:]:
            h_a.remove()
        
        for h_b in h_ax.texts[-4:]:
            h_b.remove()
            
        f_plot_rotated_axes(h_ax, l_r[d_num], t_offset=(m_trajectory[d_num*d_step,0], m_trajectory[d_num*d_step,1], m_trajectory[d_num*d_step,2]), d_scale=5)
        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], label="Trajectory", color='purple')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red', label='End')
        h_ax.set_title("AUV orientation over trajectory", fontsize=30)
        h_ax.set_xlabel('X')
        h_ax.set_ylabel('Y')
        h_ax.set_zlabel('Z')
        h_ax.set_xlim3d([np.min(m_trajectory[:,0])-3, np.max(m_trajectory[:,0])+3])
        h_ax.set_ylim3d([np.min(m_trajectory[:,1])-3, np.max(m_trajectory[:,1])+3])
        h_ax.set_zlim3d([np.min(m_trajectory[:,2])-3, np.max(m_trajectory[:,2])+3])
    
    interact(update_plot, d_num=IntSlider(min=0, max=(d_end-d_start)//d_step-1, step=1, value=0))
    h_ax.legend()
    plt.tight_layout()
    plt.show()

# %% ../nbs/00_core.ipynb 47
def f_create_auv_orientation_animation_plotly(
    m_pos_auv: np.ndarray,
    m_ypr: np.ndarray,
    v_timestamp_nav: np.ndarray,
    d_step: int = 100
) -> go.Figure:
    r"""
    Create an animated Plotly figure showing AUV orientation and trajectory.
    
    ::: {.callout-note}
    This function generates a 3D animated plot of an Autonomous Underwater Vehicle (AUV) 
    trajectory and orientation using Plotly. It visualizes the AUV's path and its 
    orientation changes over time.
    :::

    **Parameters**  
     \ \  - `m_pos_auv`: numpy.ndarray containing AUV positions (N x 3)  
     \ \  - `m_ypr`: numpy.ndarray containing yaw, pitch, roll data (N x 3)  
     \ \  - `v_timestamp_nav`: numpy.ndarray of timestamps for navigation data  
     \ \  - `d_step`: Step size for frame generation (default: 100)  

    **Returns**  
     \ \  go.Figure: Plotly Figure object with the animated AUV trajectory and orientation  

    **Example**
    
    ```
    m_pos_sub = m_pos_auv[::10].copy()  # Downsample by factor of 10
    m_ypr_sub = m_ypr[::10].copy()
    v_timestamp_sub = v_timestamp_nav[::10].copy()
    d_step = 20  # Show every 20th point of the downsampled data

    fig = f_create_auv_orientation_animation_plotly(m_pos_sub, m_ypr_sub, v_timestamp_sub, d_step)
    fig.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Function Details**

    1. **Data Preparation**:
       - Inverts the Z-axis of the trajectory for proper 3D visualization.

    2. **Figure Initialization**:
       - Creates a new Plotly figure object.

    3. **Plot Boundaries**:
       - Calculates and sets the plot boundaries with margins for better visualization.

    4. **Layout Configuration**:
       - Sets up the 3D scene with custom aspect ratio and orthographic projection.

    5. **Trajectory and Initial Position**:
       - Adds the AUV trajectory and initial position to the plot.

    6. **Animation Frames**:
       - Generates animation frames for the AUV movement.

    7. **Final Layout Update**:
       - Configures animation controls and time slider.

    Note: This function relies on helper functions (`f_add_trajectory_traces`, 
    `f_add_initial_position_and_axes`, `f_add_animation_frames`, `f_update_layout`) 
    which should be implemented separately.
    :::

    **References**  
    \ \ 1. [Plotly Python Documentation](https://plotly.com/python/)  
    \ \ 2. [NumPy Documentation](https://numpy.org/doc/stable/)  
    """
    try:
        # Validate input data
        if not all(isinstance(arr, np.ndarray) for arr in [m_pos_auv, m_ypr, v_timestamp_nav]):
            raise ValueError("Input arrays must be numpy.ndarray objects.")
        if not all(arr.ndim == 2 for arr in [m_pos_auv, m_ypr]) or v_timestamp_nav.ndim != 1:
            raise ValueError("Invalid array dimensions.")
        if not (m_pos_auv.shape[0] == m_ypr.shape[0] == v_timestamp_nav.shape[0]):
            raise ValueError("Input arrays must have the same length.")

        # Invert Z-axis for plotting
        m_trajectory = m_pos_auv.copy()
        m_trajectory[:, 2] = -m_trajectory[:, 2]

        # Initialize Plotly figure
        h_fig = go.Figure()

        # Calculate plot boundaries with margin
        d_margin = 5  # Margin size for plot boundaries
        l_min = np.min(m_trajectory, axis=0) - d_margin
        l_max = np.max(m_trajectory, axis=0) + d_margin
        dic_axis_ranges = {
            axis: dict(range=[l_min[i], l_max[i]], autorange=False)
            for i, axis in enumerate(['xaxis', 'yaxis', 'zaxis'])
        }

        # Update layout with calculated boundaries and orthographic projection
        h_fig.update_layout(
            scene=dict(
                **dic_axis_ranges,
                aspectmode='manual',
                aspectratio=dict(x=1.8, y=1.2, z=0.8),
                camera=dict(projection=dict(type='orthographic')),
            )
        )
        
        # Add trajectory traces to the figure
        f_add_trajectory_traces(h_fig, m_trajectory)

        # Add initial position and orientation axes
        f_add_initial_position_and_axes(h_fig, m_trajectory, m_ypr)

        # Add animation frames
        f_add_animation_frames(h_fig, m_trajectory, m_ypr, d_step)

        # Update layout with animation controls and timestamps
        f_update_layout(h_fig, m_trajectory, v_timestamp_nav, d_step)
        
        return h_fig

    except Exception as e:
        raise RuntimeError(f"Error in creating AUV orientation animation: {str(e)}") from e


# %% ../nbs/00_core.ipynb 50
def f_add_trajectory_traces(h_fig: go.Figure, m_trajectory: np.ndarray) -> None:
    r"""
    Add trajectory, start, and end point traces to the figure.
    
    ::: {.callout-note}
    This function adds three traces to the given figure: the full trajectory,
    the start point, and the end point.
    :::

    **Parameters**  
     \ \  - `h_fig`: Plotly figure object to which traces will be added  
     \ \  - `m_trajectory`: numpy.ndarray containing trajectory points (N x 3)  

    **Returns**  
     \ \  None  

    **Example**
    
    ```python
    import plotly.graph_objects as go
    import numpy as np
    
    h_fig = go.Figure()
    m_trajectory = np.random.rand(100, 3)
    f_add_trajectory_traces(h_fig, m_trajectory)
    h_fig.show()
    ```
    """
    h_fig.add_trace(go.Scatter3d(
        x=m_trajectory[:, 0], y=m_trajectory[:, 1], z=m_trajectory[:, 2],
        mode='lines', name='Trajectory', line=dict(color='purple', width=2)
    ))
    h_fig.add_trace(go.Scatter3d(
        x=[m_trajectory[0, 0]], y=[m_trajectory[0, 1]], z=[m_trajectory[0, 2]],
        mode='markers', name='Start', marker=dict(color='green', size=2)
    ))
    h_fig.add_trace(go.Scatter3d(
        x=[m_trajectory[-1, 0]], y=[m_trajectory[-1, 1]], z=[m_trajectory[-1, 2]],
        mode='markers', name='End', marker=dict(color='red', size=2)
    ))

# %% ../nbs/00_core.ipynb 51
def f_create_axes(t_position: Tuple[float, float, float], r_rotation: R, d_scale: float = 4) -> List[go.Scatter3d]:
    r"""
    Create 3D axes at a given position and orientation.
    
    ::: {.callout-note}
    This function generates three 3D scatter traces representing the X, Y, and Z axes
    at a specified position and orientation.
    :::

    **Parameters**  
     \ \  - `t_position`: Tuple of (x, y, z) coordinates for the axes origin  
     \ \  - `r_rotation`: Rotation object specifying the orientation of the axes  
     \ \  - `d_scale`: Scale factor for the axes length (default: 4)  

    **Returns**  
     \ \  List[go.Scatter3d]: List of three Scatter3d objects representing the axes  

    **Example**
    
    ```python
    import plotly.graph_objects as go
    from scipy.spatial.transform import Rotation as R
    
    t_position = (0, 0, 0)
    r_rotation = R.from_euler('xyz', [0, 0, 45], degrees=True)
    l_axes = f_create_axes(t_position, r_rotation)
    
    h_fig = go.Figure(data=l_axes)
    h_fig.show()
    ```
    """
    m_axes = np.array([[d_scale, 0, 0], [0, d_scale, 0], [0, 0, d_scale]])
    m_rotated_axes = r_rotation.apply(m_axes)
    l_colors = ['red', 'green', 'blue']
    l_labels = ['X', 'Y', 'Z']
    
    return [go.Scatter3d(
        x=[t_position[0], t_position[0] + m_rotated_axes[d_i, 0]],
        y=[t_position[1], t_position[1] + m_rotated_axes[d_i, 1]],
        z=[t_position[2], t_position[2] + m_rotated_axes[d_i, 2]],
        mode='lines+text',
        line=dict(color=l_colors[d_i], width=5),
        text=['', l_labels[d_i]],
        textposition='top center',
        showlegend=False
    ) for d_i in range(3)]

# %% ../nbs/00_core.ipynb 52
def f_add_bounding_box(h_fig: go.Figure, m_trajectory: np.ndarray) -> None:
    r"""
    Calculate and add bounding box to the figure.
    
    ::: {.callout-note}
    This function calculates the bounding box for the given trajectory and
    updates the figure's layout to ensure all points are visible.
    :::

    **Parameters**  
     \ \  - `h_fig`: Plotly figure object to be updated  
     \ \  - `m_trajectory`: numpy.ndarray containing trajectory points (N x 3)  

    **Returns**  
     \ \  None  

    **Example**
    
    ```python
    import plotly.graph_objects as go
    import numpy as np
    
    h_fig = go.Figure()
    m_trajectory = np.random.rand(100, 3) * 100
    f_add_bounding_box(h_fig, m_trajectory)
    h_fig.show()
    ```
    """
    d_x_min, d_x_max = np.min(m_trajectory[:, 0]), np.max(m_trajectory[:, 0])
    d_y_min, d_y_max = np.min(m_trajectory[:, 1]), np.max(m_trajectory[:, 1])
    d_z_min, d_z_max = np.min(m_trajectory[:, 2]), np.max(m_trajectory[:, 2])

    d_padding = 0.1 * max(d_x_max - d_x_min, d_y_max - d_y_min, d_z_max - d_z_min)
    h_fig.update_layout(scene=dict(
        xaxis=dict(range=[d_x_min - d_padding, d_x_max + d_padding], title='X'),
        yaxis=dict(range=[d_y_min - d_padding, d_y_max + d_padding], title='Y'),
        zaxis=dict(range=[d_z_min + d_padding, d_z_max - d_padding], title='Z'),
        aspectmode='manual',
        aspectratio=dict(x=1, y=1, z=0.5),
        camera=dict(eye=dict(x=1.15, y=1.15, z=1), up=dict(x=0, y=0, z=1))
    ))

# %% ../nbs/00_core.ipynb 53
def f_add_initial_position_and_axes(h_fig: go.Figure, m_trajectory: np.ndarray, m_ypr: np.ndarray) -> None:
    r"""
    Add initial position and axes to the figure.
    
    ::: {.callout-note}
    This function adds a marker for the initial position and 3D axes representing
    the initial orientation to the figure.
    :::

    **Parameters**  
     \ \  - `h_fig`: Plotly figure object to be updated  
     \ \  - `m_trajectory`: numpy.ndarray containing trajectory points (N x 3)  
     \ \  - `m_ypr`: numpy.ndarray containing yaw, pitch, roll values (N x 3)  

    **Returns**  
     \ \  None  

    **Example**
    
    ```python
    import plotly.graph_objects as go
    import numpy as np
    
    h_fig = go.Figure()
    m_trajectory = np.random.rand(100, 3) * 100
    m_ypr = np.random.rand(100, 3) * np.pi
    f_add_initial_position_and_axes(h_fig, m_trajectory, m_ypr)
    h_fig.show()
    ```
    """
    r_initial_rotation = R.from_euler('ZYX', m_ypr[0] + np.array([0, 0, np.pi]))
    h_fig.add_trace(go.Scatter3d(
        x=[m_trajectory[0, 0]], y=[m_trajectory[0, 1]], z=[m_trajectory[0, 2]],
        mode='markers', marker=dict(color='blue', size=4), name='Current Position'
    ))
    h_fig.add_traces(f_create_axes(tuple(m_trajectory[0]), r_initial_rotation))

# %% ../nbs/00_core.ipynb 54
def f_add_animation_frames(h_fig: go.Figure, m_trajectory: np.ndarray, m_ypr: np.ndarray, d_step: int = 100) -> None:
    r"""
    Create and add animation frames to the Plotly figure for AUV trajectory visualization.
    
    ::: {.callout-note}
    This function generates animation frames for the AUV trajectory, updating the
    position and orientation of the AUV at each step. It creates a series of frames
    that can be used to animate the AUV's movement along its path.
    :::

    **Parameters**  
     \ \  - `h_fig`: Plotly figure object to be updated  
     \ \  - `m_trajectory`: numpy.ndarray containing trajectory points (N x 3)  
     \ \  - `m_ypr`: numpy.ndarray containing yaw, pitch, roll values (N x 3)  
     \ \  - `d_step`: Step size for frame generation  

    **Returns**  
     \ \  None  

    **Example**
    
    ```
    import plotly.graph_objects as go
    import numpy as np
    from scipy.spatial.transform import Rotation as R

    h_fig = go.Figure()
    m_trajectory = np.random.rand(100, 3)
    m_ypr = np.random.rand(100, 3) * np.pi
    f_add_animation_frames(h_fig, m_trajectory, m_ypr, d_step=10)
    h_fig.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Function Details**

    1. **Input Validation**:
       - Checks for consistent array lengths and valid input types.

    2. **Frame Index Generation**:
       - Creates a list of indices for frame generation, including the last point.

    3. **Frame Creation**:
       - For each index, creates a frame with the AUV position and orientation.
       - Uses `f_create_axes` to generate orientation axes for each frame.

    4. **Frame Addition**:
       - Adds all created frames to the Plotly figure object.

    Note: This function assumes the existence of a helper function `f_create_axes` 
    for generating orientation axes. Ensure this function is implemented separately.
    :::

    **References**  
    \ \ 1. [Plotly Animations](https://plotly.com/python/animations/)  
    \ \ 2. [SciPy Rotation](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)  
    """
    try:
        if len(m_trajectory) != len(m_ypr):
            raise ValueError("Trajectory and YPR arrays must have the same length.")

        # Generate indices for frames, ensuring the last index is included
        l_indices = list(range(0, len(m_trajectory) - 1, d_step)) + [len(m_trajectory) - 1]
        l_indices = sorted(set(l_indices))  # Remove duplicates and sort

        l_frames = []
        for d_i in l_indices:
            l_frame_data = [go.Scatter3d(
                x=[m_trajectory[d_i, 0]], y=[m_trajectory[d_i, 1]], z=[m_trajectory[d_i, 2]],
                mode='markers', marker=dict(color='blue', size=4), showlegend=False
            )]
            r_rotation = R.from_euler('ZYX', m_ypr[d_i] + np.array([0, 0, np.pi]))
            l_frame_data.extend(f_create_axes(tuple(m_trajectory[d_i]), r_rotation))
            l_frames.append(go.Frame(data=l_frame_data, traces=[3, 4, 5, 6], name=f'frame{d_i}'))

        h_fig.frames = l_frames

    except Exception as e:
        raise RuntimeError(f"Error in creating animation frames: {str(e)}")

# %% ../nbs/00_core.ipynb 56
def f_update_layout(h_fig: go.Figure, m_trajectory: np.ndarray, v_timestamp_nav: np.ndarray, d_step: int = 100) -> None:
    r"""
    Update the layout of the figure with animation controls and time slider.
    
    ::: {.callout-note}
    This function enhances the Plotly figure by adding animation controls and a time slider,
    allowing for interactive visualization of the AUV trajectory over time.
    :::

    **Parameters**  
     \ \  - `h_fig`: Plotly figure object to be updated  
     \ \  - `m_trajectory`: numpy.ndarray containing trajectory points (N x 3)  
     \ \  - `v_timestamp_nav`: numpy.ndarray containing timestamps for each point  
     \ \  - `d_step`: Step size for slider steps  

    **Returns**  
     \ \  None  

    **Example**
    
    ```
    import plotly.graph_objects as go
    import numpy as np
    
    h_fig = go.Figure()
    m_trajectory = np.random.rand(100, 3)
    v_timestamp_nav = np.linspace(1609459200, 1609545600, 100)  # 24 hours of timestamps
    f_update_layout(h_fig, m_trajectory, v_timestamp_nav, d_step=10)
    h_fig.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Function Details**

    1. **Slider Steps Generation**:
       - Creates evenly spaced steps for the time slider, including the last point.

    2. **Layout Configuration**:
       - Sets the figure title, dimensions, and overall appearance.

    3. **Animation Controls**:
       - Adds play and pause buttons for controlling the animation.

    4. **Time Slider**:
       - Implements a slider that shows timestamps and allows manual navigation.
       - Uses custom time formatting for readable labels.

    Note: This function assumes the existence of a helper function `f_format_time` 
    for formatting timestamps. Ensure this function is implemented separately.
    :::

    **References**  
    \ \ 1. [Plotly Layout in Python](https://plotly.com/python/reference/layout/)  
    \ \ 2. [Plotly Animations](https://plotly.com/python/animations/)  
    """
    try:
        # Input validation
        if not isinstance(h_fig, go.Figure):
            raise TypeError("h_fig must be a plotly.graph_objects.Figure object")
        if not isinstance(m_trajectory, np.ndarray) or m_trajectory.ndim != 2 or m_trajectory.shape[1] != 3:
            raise ValueError("m_trajectory must be a 2D numpy array with shape (N, 3)")
        if not isinstance(v_timestamp_nav, np.ndarray) or v_timestamp_nav.ndim != 1:
            raise ValueError("v_timestamp_nav must be a 1D numpy array")
        if len(m_trajectory) != len(v_timestamp_nav):
            raise ValueError("m_trajectory and v_timestamp_nav must have the same length")
        if not isinstance(d_step, int) or d_step <= 0:
            raise ValueError("d_step must be a positive integer")

        # Generate indices for slider steps, ensuring the last index is included
        l_indices = list(range(0, len(m_trajectory) - 1, d_step)) + [len(m_trajectory) - 1]
        l_indices = sorted(set(l_indices))  # Remove duplicates and sort

        h_fig.update_layout(
            title=dict(
                text='3D AUV Trajectory with Animated Reference Axes',
                font=dict(size=24)
            ),
            width=1000,
            height=800,
            updatemenus=[dict(
                type='buttons',
                showactive=False,
                x=0.1,
                y=0,
                xanchor='right',
                yanchor='top',
                pad=dict(t=0, r=10),
                buttons=[
                    dict(label='Play', 
                         method='animate', 
                         args=[None, dict(frame=dict(duration=50, redraw=True), 
                                          fromcurrent=True, 
                                          mode='immediate')]),
                    dict(label='Pause', 
                         method='animate', 
                         args=[[None], dict(frame=dict(duration=0, redraw=False), 
                                            mode='immediate')])
                ]
            )],
            sliders=[dict(
                active=0,
                yanchor='top',
                xanchor='left',
                currentvalue=dict(
                    font=dict(size=16),
                    prefix='UTC Time: ',
                    visible=True,
                    xanchor='right'
                ),
                transition=dict(duration=300, easing='cubic-in-out'),
                pad=dict(b=10, t=50),
                len=0.9,
                x=0.1,
                y=0,
                steps=[dict(
                    method='animate',
                    label=f_format_time(v_timestamp_nav[d_i]),
                    args=[[f'frame{d_i}'], dict(frame=dict(duration=50, redraw=True), 
                                                mode='immediate', 
                                                transition=dict(duration=0))]
                ) for d_i in l_indices]
            )]
        )

    except Exception as e:
        raise RuntimeError(f"An error occurred while updating the layout: {str(e)}") from e


# %% ../nbs/00_core.ipynb 59
def f_create_auv_orientation_animation(
    m_pos_auv: np.ndarray,
    m_ypr: np.ndarray,
    d_step: int = 200,
    d_scale: float = 1.5,
    d_max_ticks: int = 5,
    v_figsize=(12, 5)
) -> FuncAnimation:
    r"""
    Create an animation of an AUV orientation over a given trajectory and orientations.
    
    ::: {.callout-note}
    This function generates a 3D animation showing the orientation of an Autonomous Underwater Vehicle (AUV) 
    along its trajectory. It's useful for visualizing the AUV's movement and orientation changes over time.
    :::

    **Parameters**  
     \ \  - `m_pos_auv`: Downsampled AUV positions (N x 3 numpy array)  
     \ \  - `m_ypr`: Downsampled yaw-pitch-roll orientations (N x 3 numpy array) corresponding to m_pos_auv  
     \ \  - `d_step`: Step for frame increments (default: 10)  
     \ \  - `d_scale`: Scale factor for the displayed AUV axes (default: 1.5)  
     \ \  - `d_max_ticks`: Max number of ticks per axis (default: 5)  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  FuncAnimation: The matplotlib animation object  

    **Example**
    
    ```python
    import numpy as np
    from matplotlib.animation import FuncAnimation
    
    # Example data
    m_pos_auv = np.random.rand(100, 3) * 10
    m_ypr = np.random.rand(100, 3) * np.pi
    
    animation = f_create_auv_orientation_animation3(m_pos_auv, m_ypr)
    # To save the animation:
    # animation.save('auv_animation.gif', writer='pillow')
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Implementation Details**

    1. **Data Preparation**:
       - The function inverts the Z-axis of the trajectory for plotting purposes.
       - It calculates the number of frames based on the input step size.

    2. **Figure Setup**:
       - Creates a 3D figure with specific dimensions and projection type.
       - Sets initial view angles and axis properties.

    3. **Animation Function**:
       - The `animate` function is called for each frame of the animation.
       - It clears the previous frame, resets the view, and plots the new AUV position and orientation.
       - The full trajectory is plotted with start and end points highlighted.

    4. **Optimization**:
       - Uses vectorized operations for efficiency.
       - Pre-computes rotations to avoid redundant calculations in each frame.

    5. **Customization**:
       - Allows customization of scale, step size, and number of axis ticks.

    6. **Error Handling**:
       - Input validation should be added to ensure correct array shapes and types.

    Note: This function assumes the existence of a helper function `f_plot_rotated_axes` 
    which is not provided in this template. Ensure this function is implemented separately.
    :::

    **References**  
    \ \ 1. [Matplotlib Animation Tutorial](https://matplotlib.org/stable/api/animation_api.html)  
    \ \ 2. [NumPy Documentation](https://numpy.org/doc/stable/)  
    \ \ 3. [Scipy Rotation](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html)  

    """
    try:
        # Invert Z-axis for plotting
        m_trajectory = m_pos_auv.copy()
        m_trajectory[:, 2] = -m_trajectory[:, 2]

        # Setup figure
        h_fig_auv_orientation_animation = plt.figure(figsize=v_figsize, dpi=100)
        h_ax = h_fig_auv_orientation_animation.add_subplot(
            projection="3d", proj_type="ortho", box_aspect=(5, 4, 2)
        )
        h_ax.set_title("AUV orientation over trajectory", {'fontsize': 18})

        # Set initial view properties
        h_ax.view_init(30, 45)
        h_ax.dist = 0
        h_ax.invert_xaxis()

        # Calculate frame indices
        n_points = len(m_trajectory)
        frames = (n_points + d_step - 1) // d_step
        indices = [(i * d_step if i * d_step < n_points else n_points - 1) for i in range(frames)]
        indices[-1] = n_points - 1  # Ensure last frame is last point

        # Pre-compute rotations
        l_r = [R.from_euler("ZYX", m_ypr[idx, :] + np.array([0, 0, np.pi])) for idx in indices]

        def animate(d_num):
            h_ax.clear()
            h_ax.set_title("AUV orientation over trajectory", {'fontsize': 18})
            h_ax.view_init(30, 45)
            h_ax.dist = 0
            h_ax.invert_xaxis()

            idx = indices[d_num]

            # Plot rotated AUV axes
            f_plot_rotated_axes(
                h_ax, l_r[d_num], 
                t_offset=(m_trajectory[idx, 0], m_trajectory[idx, 1], m_trajectory[idx, 2]),
                d_scale=d_scale
            )

            # Plot trajectory and points
            h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], color='purple', label='Trajectory')
            h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green', label='Start')
            h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red', label='End')

            # Set axis properties
            d_margin = 3
            for i, (min_val, max_val) in enumerate(zip(np.min(m_trajectory, axis=0), np.max(m_trajectory, axis=0))):
                getattr(h_ax, f'set_{["x", "y", "z"][i]}lim3d')([min_val - d_margin, max_val + d_margin])
                getattr(h_ax, f'set_{["x", "y", "z"][i]}label')(["X", "Y", "Z"][i])
                getattr(h_ax, f'{["x", "y", "z"][i]}axis').set_major_locator(MaxNLocator(d_max_ticks))

        # Reduce whitespace
        h_fig_auv_orientation_animation.tight_layout()
        
        # Create and return animation
        anim = FuncAnimation(h_fig_auv_orientation_animation, animate, interval=200, frames=frames, repeat=False)
        
        plt.close(h_fig_auv_orientation_animation)
        return anim

    except Exception as e:
        raise ValueError(f"An error occurred while creating the AUV orientation animation: {str(e)}")

# %% ../nbs/00_core.ipynb 61
def f_plot_polar_sonar_seaking(m_sonar: np.ndarray, v_angle: np.ndarray, v_figsize=(8, 8)) -> None:
    r"""
    Plot the polar view of sonar data from a SeaKing sonar or similar device.
    
    ::: {.callout-note}
    This function creates a polar plot of sonar data, typically used for visualizing 
    underwater acoustic measurements. It helps analyze the 
    spatial distribution of sonar intensities around the sensor.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_angles)  
     \ \  - `v_angle`: numpy.ndarray of angle data in radians, shape (n_angles,)  
     \ \  - `v_figsize`: tuple of figure width and height size  
     
    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample sonar data
    d_n_ranges = 100
    d_n_angles = 360
    m_sonar = np.random.rand(d_n_ranges, d_n_angles)
    v_angle = np.linspace(0, 2*np.pi, d_n_angles)

    # Create some artificial features in the sonar data
    m_sonar[30:40, 180:200] = 0.8  # Simulated object
    m_sonar[60:70, 90:110] = 0.9   # Another simulated object

    # Plot the sonar data
    f_plot_polar_sonar_seaking(m_sonar, v_angle)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the polar sonar plot creation**

    1. **Figure Setup**:
       - Creates a figure with a size of 10x10 inches.
       - Uses a polar projection for the subplot.

    2. **Data Plotting**:
       - Uses `pcolormesh` to create a color-mapped representation of the sonar data.
       - The radial dimension represents range, and the angular dimension represents angle.

    3. **Polar Plot Customization**:
       - Sets the angle direction to clockwise.
       - Offsets the angle by π/2 to place 0° at the top.

    4. **Tick Label Adjustment**:
       - Customizes the angular tick labels to show degrees from 0° to 330°.

    5. **Axis Labeling**:
       - Adds labels for angle and range axes.
       - Adjusts label padding for better visibility.

    6. **Colorbar and Title**:
       - Adds a colorbar to represent sonar intensity.
       - Sets a title for the plot and labels for the axes.

    This visualization approach allows for intuitive interpretation of sonar data, 
    making it easier to identify objects or features in the scanned area.
    :::

    **References**  
    \ \ 1. [Matplotlib Polar Plots](https://matplotlib.org/stable/gallery/pie_and_polar_charts/polar_demo.html)  

    """
    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize, subplot_kw={'projection': 'polar'})

    # Plot the sonar data in polar view
    h_im = h_ax.pcolormesh(v_angle, range(m_sonar.shape[0]), m_sonar, cmap='viridis')

    # Set the angle direction to be clockwise with 0 degrees at the top
    h_ax.set_theta_direction(-1)
    h_ax.set_theta_offset(np.pi / 2)

    # Adjust the tick labels
    h_ax.set_xticks([0, np.pi/6, np.pi/3, np.pi/2, 2*np.pi/3, 5*np.pi/6, np.pi, 7*np.pi/6, 4*np.pi/3, 3*np.pi/2, 5*np.pi/3, 11*np.pi/6, 2*np.pi])
    h_ax.set_xticklabels(['0°', '30°', '60°', '90°', '120°', '150°', '180°', '210°', '240°', '270°', '300°', '330°','0°'])

    # Adjust the positions of x and y labels
    h_ax.set_xlabel('Angle', labelpad=20)
    h_ax.set_ylabel('Range', labelpad=20)

    # Add a colorbar, title, and other labels
    h_cbar = h_fig.colorbar(h_im, ax=h_ax)
    h_cbar.set_label('Sonar Intensity')
    h_ax.set_title('Vertical Sonar in Polar View')
    h_ax.set_xlabel('Range from 0 to 10 meters')
    h_ax.set_ylabel('Angle', labelpad=30)

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 62
def f_plot_polar_sonar_micron(m_sonar: np.ndarray, v_angle: np.ndarray, v_figsize=(8, 8)) -> None:
    r"""
    Plot the polar view of sonar data from a Micron sonar or similar horizontal scanning device.
    
    ::: {.callout-note}
    This function creates a polar plot of horizontal sonar data, typically used for visualizing 
    underwater acoustic measurements in a planar view. It's particularly useful for analyzing 
    the spatial distribution of sonar intensities in a horizontal plane around the sensor.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_angles)  
     \ \  - `v_angle`: numpy.ndarray of angle data in radians, shape (n_angles,)  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample horizontal sonar data
    d_n_ranges = 200  # Increased range for horizontal sonar
    d_n_angles = 360
    m_sonar = np.random.rand(d_n_ranges, d_n_angles)
    v_angle = np.linspace(0, 2*np.pi, d_n_angles)

    # Create some artificial features in the sonar data
    m_sonar[50:70, 180:200] = 0.8  # Simulated distant object
    m_sonar[100:120, 90:110] = 0.9   # Another simulated object
    m_sonar[30:40, 270:290] = 0.7  # Closer object

    # Plot the sonar data
    f_plot_polar_sonar_micron(m_sonar, v_angle)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the horizontal polar sonar plot creation**

    1. **Figure Setup**:
       - Creates a figure with a size of 8x8 inches.
       - Uses a polar projection for the subplot, suitable for horizontal scanning.

    2. **Data Plotting**:
       - Uses `pcolormesh` to create a color-mapped representation of the sonar data.
       - The radial dimension represents range (up to 20 meters), and the angular dimension represents angle.

    3. **Polar Plot Customization**:
       - Sets the angle direction to clockwise, matching typical horizontal sonar conventions.
       - Offsets the angle by π/2 to place 0° at the top, aligning with standard navigation orientations.

    4. **Tick Label Adjustment**:
       - Customizes the angular tick labels to show degrees from 0° to 330°, covering a full 360° scan.

    5. **Axis Labeling**:
       - Adds labels for angle and range axes.
       - Adjusts label padding for better visibility in the circular plot.

    6. **Colorbar and Title**:
       - Adds a colorbar to represent sonar intensity.
       - Sets a title specifying "Horizontal Sonar" and labels the range axis up to 20 meters.

    This visualization approach is tailored for horizontal scanning sonars, providing an 
    intuitive top-down view of the underwater environment, which is crucial for navigation, 
    obstacle detection, and mapping in autonomous underwater vehicles (AUVs) or remotely 
    operated vehicles (ROVs).
    :::

    **References**  
    \ \ 1. [Micron Sonar Documentation](https://www.tritech.co.uk/product/micron-sonar)  

    """
    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize, subplot_kw={'projection': 'polar'})

    # Plot the sonar data in polar view
    h_im = h_ax.pcolormesh(v_angle, range(m_sonar.shape[0]), m_sonar, cmap='viridis')

    # Set the angle direction to be clockwise with 0 degrees at the top
    h_ax.set_theta_direction(-1)
    h_ax.set_theta_offset(np.pi / 2)

    # Adjust the tick labels
    h_ax.set_xticks([0, np.pi/6, np.pi/3, np.pi/2, 2*np.pi/3, 5*np.pi/6, np.pi, 7*np.pi/6, 4*np.pi/3, 3*np.pi/2, 5*np.pi/3, 11*np.pi/6, 2*np.pi])
    h_ax.set_xticklabels(['0°', '30°', '60°', '90°', '120°', '150°', '180°', '210°', '240°', '270°', '300°', '330°','0°'])

    # Adjust the positions of x and y labels
    h_ax.set_xlabel('Angle', labelpad=20)
    h_ax.set_ylabel('Range', labelpad=20)

    # Add a colorbar, title, and other labels
    h_cbar = h_fig.colorbar(h_im, ax=h_ax)
    h_cbar.set_label('Sonar Intensity')
    h_ax.set_title('Horizontal Sonar in Polar View')
    h_ax.set_xlabel('Range from 0 to 20 meters')
    h_ax.set_ylabel('Angle', labelpad=30)

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 63
def f_plot_horizontal_sonar(m_sonar: np.ndarray, d_start_micron: int, d_image_width: int, v_range_micron: np.ndarray,v_figsize=(16, 6)) -> None:
    r"""
    Plot the horizontal sonar data in a 2D image format.
    
    ::: {.callout-note}
    This function creates a 2D visualization of horizontal sonar data, typically used for 
    displaying continuous sonar scans over time. It's particularly useful for analyzing 
    temporal changes in the underwater environment and detecting objects or features 
    across multiple sonar revolutions.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_time_steps)  
     \ \  - `d_start_micron`: Integer representing the starting index of the sonar data  
     \ \  - `d_image_width`: Integer specifying the width of the sonar image in time steps  
     \ \  - `v_range_micron`: numpy.ndarray representing the range values of the sonar data  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample horizontal sonar data
    d_n_ranges = 397  # 397 bins over 19.95 meters
    d_n_time_steps = 2000  # Multiple revolutions
    m_sonar = np.random.rand(d_n_ranges, d_n_time_steps)
    v_range_micron = np.linspace(0, 19.95, d_n_ranges)  # 20 meters range

    # Create some artificial features in the sonar data
    m_sonar[50:70, 400:600] = 0.8  # Simulated object across multiple scans
    m_sonar[100:120, 800:1000] = 0.9  # Another simulated object
    m_sonar[30:40, 1200:1400] = 0.7  # Closer object

    # Plot the sonar data
    f_plot_horizontal_sonar(m_sonar, d_start_micron=194, d_image_width=1400, v_range_micron=v_range_micron)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the horizontal sonar plot creation**

    1. **Figure Setup**:
       - Creates a figure with a size of 16x9 inches, suitable for widescreen displays.

    2. **Data Plotting**:
       - Uses `imshow` to create a 2D color-mapped representation of the sonar data.
       - The y-axis represents range (up to 20 meters), and the x-axis represents time or scan number.

    3. **Plot Customization**:
       - Sets a title "Horizontal Sonar" with large font size.
       - Labels the x-axis to indicate the time scale (1 revolution every 200 points).
       - Labels the y-axis to show the range scale (397 bins over 19.95 meters).
       - Adjusts tick label sizes for better readability.

    4. **Image Extent**:
       - Sets the extent of the image to match the start index and image width.
       - Inverts the y-axis so that closer ranges appear at the top of the plot.

    5. **Colorbar Addition**:
       - Adds a colorbar to the right of the main plot to represent sonar intensity.
       - Adjusts the colorbar position and size to fit well with the main plot.

    This visualization approach is particularly useful for analyzing continuous sonar data, 
    allowing for the detection of persistent features, moving objects, or temporal changes 
    in the underwater environment. It provides a comprehensive view of the sonar data across 
    multiple scans or revolutions.
    :::

    **References**  
    \ \ None

    """
    # Calculate the end index
    d_end_micron = d_start_micron + d_image_width

    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize)

    # Plot the sonar data
    h_ax.set_title("Horizontal Sonar", {'fontsize': int(24 * v_figsize[0]/16)})
    h_ax.set_xlabel('1 revolution every 200 points', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.set_ylabel('397 bins over 19.95 meters', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.tick_params(axis='both', labelsize=int(18 * v_figsize[0]/16))
    h_im = h_ax.imshow(m_sonar, aspect='auto', interpolation='none', extent=[d_start_micron-194, d_end_micron-194, v_range_micron[-1], 0])
    
    # Add colorbar
    h_cax = h_fig.add_axes([h_ax.get_position().x1+0.01, h_ax.get_position().y0, 0.02, h_ax.get_position().height])
    h_colorbar = plt.colorbar(h_im, cax=h_cax)
    h_colorbar.ax.tick_params(labelsize=int(18 * v_figsize[0]/16))

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 64
def f_plot_vertical_sonar(m_sonar: np.ndarray, d_start_seaking: int, d_image_width: int, v_range_seaking: np.ndarray,v_figsize=(16, 6)) -> None:
    r"""
    Plot the vertical sonar data in a 2D image format.
    
    ::: {.callout-note}
    This function creates a 2D visualization of vertical sonar data, typically used for 
    displaying continuous sonar scans over time. It's particularly useful for analyzing 
    temporal changes in the underwater environment and detecting objects or features 
    across multiple sonar revolutions.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_time_steps)  
     \ \  - `d_start_seaking`: Integer representing the starting index of the sonar data  
     \ \  - `d_image_width`: Integer specifying the width of the sonar image in time steps  
     \ \  - `v_range_seaking`: numpy.ndarray representing the range values of the sonar data  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample vertical sonar data
    d_n_ranges = 50  # 50 bins over 9.80 meters
    d_n_time_steps = 2000  # Multiple revolutions
    m_sonar = np.random.rand(d_n_ranges, d_n_time_steps)
    v_range_seaking = np.linspace(0, 10, d_n_ranges)  # 10 meters range

    # Create some artificial features in the sonar data
    m_sonar[50:70, 400:600] = 0.8  # Simulated object across multiple scans
    m_sonar[100:120, 800:1000] = 0.9  # Another simulated object
    m_sonar[30:40, 1200:1400] = 0.7  # Closer object

    # Plot the sonar data
    f_plot_vertical_sonar(m_sonar, d_start_seaking=194, d_image_width=1400, v_range_seaking=v_range_seaking)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the vertical sonar plot creation**

    1. **Figure Setup**:
       - Creates a figure with a size of 16x9 inches, suitable for widescreen displays.

    2. **Data Plotting**:
       - Uses `imshow` to create a 2D color-mapped representation of the sonar data.
       - The y-axis represents range (up to 20 meters), and the x-axis represents time or scan number.

    3. **Plot Customization**:
       - Sets a title "Vertical Sonar" with large font size.
       - Labels the x-axis to indicate the time scale (1 revolution every 200 points).
       - Labels the y-axis to show the range scale (397 bins over 19.95 meters).
       - Adjusts tick label sizes for better readability.

    4. **Image Extent**:
       - Sets the extent of the image to match the start index and image width.
       - Inverts the y-axis so that closer ranges appear at the top of the plot.

    5. **Colorbar Addition**:
       - Adds a colorbar to the right of the main plot to represent sonar intensity.
       - Adjusts the colorbar position and size to fit well with the main plot.

    This visualization approach is particularly useful for analyzing continuous sonar data, 
    allowing for the detection of persistent features, moving objects, or temporal changes 
    in the underwater environment. It provides a comprehensive view of the sonar data across 
    multiple scans or revolutions.
    :::

    **References**  
    \ \ None

    """
    # Calculate the end index
    d_end_seaking = d_start_seaking + d_image_width

    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize)
    
    # Plot the sonar data
    h_ax.set_title("Vertical Sonar", {'fontsize': int(24 * v_figsize[0]/16)})
    h_ax.set_xlabel('1 revolution every 201 points', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.set_ylabel('50 bins over 9.80 meters', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.tick_params(axis='both', labelsize=int(18 * v_figsize[0]/16))
    h_im = h_ax.imshow(m_sonar, aspect='auto', interpolation='none', extent=[d_start_seaking-95, d_end_seaking-95, v_range_seaking[-1], 0])
    
    # Add colorbar
    h_cax = h_fig.add_axes([h_ax.get_position().x1+0.01, h_ax.get_position().y0, 0.02, h_ax.get_position().height])
    h_colorbar = plt.colorbar(h_im, cax=h_cax)
    h_colorbar.ax.tick_params(labelsize=int(18 * v_figsize[0]/16))

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 65
def f_plot_horizontal_sonar_with_angles(m_sonar: np.ndarray, v_angles_rad: np.ndarray, d_start_micron: int, d_image_width: int, v_range_micron: np.ndarray,v_figsize=(12, 6)) -> None:
    r"""
    Plot the horizontal sonar data in a 2D image format with angle information.
    
    ::: {.callout-note}
    This function creates a 2D visualization of horizontal sonar data, incorporating angle 
    information on the x-axis. It's particularly useful for analyzing the angular distribution 
    of sonar intensities across a full 360-degree scan, allowing for precise localization of 
    underwater features or objects.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_angles)  
     \ \  - `v_angles_rad`: numpy.ndarray of angle data in radians  
     \ \  - `d_start_micron`: Integer representing the starting index of the sonar data  
     \ \  - `d_image_width`: Integer specifying the width of the sonar image in angle steps  
     \ \  - `v_range_micron`: numpy.ndarray representing the range values of the sonar data  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample horizontal sonar data
    d_n_ranges = 397  # 397 bins over 19.95 meters
    d_n_angles = 200  # One full revolution
    m_sonar = np.random.rand(d_n_ranges, d_n_angles)
    v_range_micron = np.linspace(0, 20, d_n_ranges)  # 20 meters range
    v_angles_rad = np.linspace(0, 2*np.pi, d_n_angles)

    # Create some artificial features in the sonar data
    m_sonar[50:70, 30:50] = 0.8  # Simulated object at about 45 degrees
    m_sonar[100:120, 90:110] = 0.9  # Another object at about 180 degrees
    m_sonar[30:40, 150:170] = 0.7  # Closer object at about 270 degrees

    # Plot the sonar data
    f_plot_horizontal_sonar_with_angles(m_sonar, v_angles_rad, d_start_micron=0, d_image_width=200, v_range_micron=v_range_micron)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the horizontal sonar plot creation with angles**

    1. **Figure Setup**:
       - Creates a figure with a size of 16x9 inches, suitable for widescreen displays.

    2. **Data Plotting**:
       - Uses `imshow` to create a 2D color-mapped representation of the sonar data.
       - The y-axis represents range (up to 20 meters), and the x-axis represents angles (0 to 360 degrees).

    3. **Angle Conversion**:
       - Converts the input angles from radians to degrees for more intuitive visualization.

    4. **Plot Customization**:
       - Sets a title "Horizontal Sonar" with large font size.
       - Labels the x-axis to indicate the angular scale (360 degrees per revolution).
       - Labels the y-axis to show the range scale (397 bins over 19.95 meters).
       - Adjusts tick label sizes for better readability.

    5. **Angular Tick Adjustment**:
       - Sets custom tick labels for angles, showing degrees from 0° to 360° at 30° intervals.

    6. **Grid Addition**:
       - Adds a grid to the plot for easier reference of angles and ranges.

    7. **Colorbar Addition**:
       - Adds a colorbar to the right of the main plot to represent sonar intensity.
       - Adjusts the colorbar position and size to fit well with the main plot.

    This visualization approach provides a comprehensive view of the sonar data across a full 
    360-degree scan, allowing for precise angular localization of underwater features or objects. 
    It's particularly useful for navigation, obstacle detection, and environmental mapping in 
    autonomous underwater vehicles (AUVs) or remotely operated vehicles (ROVs).
    :::

    **References**  
    \ \ None 

    """
    # Calculate the end index
    d_end_micron = d_start_micron + d_image_width

    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize)

    # Plot the sonar data
    h_ax.set_title("Horizontal Sonar", {'fontsize': int(24 * v_figsize[0]/16)})
    h_ax.set_xlabel('1 revolution (360 degrees) every 200 points', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.set_ylabel('397 bins over 19.95 meters', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.tick_params(axis='both', labelsize=int(18 * v_figsize[0]/16))

    # Convert angles from radians to degrees
    v_aux_angles = np.degrees(v_angles_rad[d_start_micron:d_end_micron])

    # Plot sonar with angles
    h_im = h_ax.imshow(m_sonar, aspect='auto', interpolation='none', extent=[v_aux_angles[0], 360, v_range_micron[-1], 0])

    # Adjust the tick labels
    h_ax.set_xticks(np.linspace(0, 360, 13))
    h_ax.set_xticklabels(['0°', '30°', '60°', '90°', '120°', '150°', '180°', '210°', '240°', '270°', '300°', '330°', '360°'])

    # Add grid
    h_ax.grid()

    # Add colorbar
    h_cax = h_fig.add_axes([h_ax.get_position().x1 + 0.01, h_ax.get_position().y0, 0.02, h_ax.get_position().height])
    h_colorbar = plt.colorbar(h_im, cax=h_cax)
    h_colorbar.ax.tick_params(labelsize=int(18 * v_figsize[0]/16))

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 66
def f_plot_vertical_sonar_with_angles(m_sonar: np.ndarray, v_angles_rad: np.ndarray, d_start: int, d_image_width: int, v_range: np.ndarray, v_figsize=(12,6)) -> None:
    r"""
    Plot the vertical sonar data in a 2D image format with angle information.
    
    ::: {.callout-note}
    This function creates a 2D visualization of vertical sonar data, incorporating angle 
    information on the x-axis. It's particularly useful for analyzing the vertical profile 
    of underwater environments, allowing for the detection of features such as seafloor 
    topography, water column structures, or suspended objects across a full 360-degree scan.
    :::

    **Parameters**  
     \ \  - `m_sonar`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_angles)  
     \ \  - `v_angles_rad`: numpy.ndarray of angle data in radians  
     \ \  - `d_start`: Integer representing the starting index of the sonar data  
     \ \  - `d_image_width`: Integer specifying the width of the sonar image in angle steps  
     \ \  - `v_range`: numpy.ndarray representing the range values of the sonar data  
     \ \  - `v_figsize`: tuple of figure width and height size  

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt

    # Generate sample vertical sonar data
    d_n_ranges = 50  # 50 bins over 9.80 meters
    d_n_angles = 201  # One full revolution
    m_sonar = np.random.rand(d_n_ranges, d_n_angles)
    v_range = np.linspace(0, 10, d_n_ranges)  # 10 meters range
    v_angles_rad = np.linspace(0, 2*np.pi, d_n_angles)

    # Create some artificial features in the sonar data
    m_sonar[20:30, 30:50] = 0.8  # Simulated seafloor feature at about 45 degrees
    m_sonar[10:15, 90:110] = 0.9  # Water column structure at about 180 degrees
    m_sonar[5:10, 150:170] = 0.7  # Near-surface object at about 270 degrees

    # Plot the sonar data
    f_plot_vertical_sonar_with_angles(m_sonar, v_angles_rad, d_start=0, d_image_width=201, v_range=v_range)
    plt.show()
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the vertical sonar plot creation with angles**

    1. **Figure Setup**:
       - Creates a figure with a size of 16x9 inches, suitable for widescreen displays.

    2. **Data Plotting**:
       - Uses `imshow` to create a 2D color-mapped representation of the sonar data.
       - The y-axis represents range (up to 10 meters), and the x-axis represents angles (0 to 360 degrees).

    3. **Angle Conversion**:
       - Converts the input angles from radians to degrees for more intuitive visualization.

    4. **Plot Customization**:
       - Sets a title "Vertical Sonar" with large font size.
       - Labels the x-axis to indicate the angular scale (360 degrees per revolution, 201 points).
       - Labels the y-axis to show the range scale (50 bins over 9.80 meters).
       - Adjusts tick label sizes for better readability.

    5. **Angular Tick Adjustment**:
       - Sets custom tick labels for angles, showing degrees from 0° to 360° at 30° intervals.

    6. **Grid Addition**:
       - Adds a grid to the plot for easier reference of angles and ranges.

    7. **Colorbar Addition**:
       - Adds a colorbar to the right of the main plot to represent sonar intensity.
       - Adjusts the colorbar position and size to fit well with the main plot.

    This visualization approach provides a comprehensive view of the vertical sonar data across 
    a full 360-degree scan, allowing for analysis of water column structures, seafloor profiling, 
    and detection of suspended objects. It's particularly useful for environmental monitoring, 
    bathymetry studies, and vertical obstacle detection in underwater robotics applications.
    :::

    **References**  
    \ \ None

    """
    # Calculate the end index
    d_end = d_start + d_image_width

    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=v_figsize)

    # Plot the sonar data
    h_ax.set_title("Vertical Sonar", {'fontsize': int(24 * v_figsize[0]/16)})
    h_ax.set_xlabel('1 revolution (360 degrees) every 201 points', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.set_ylabel('50 bins over 9.80 meters', {'fontsize': int(18 * v_figsize[0]/16)})
    h_ax.tick_params(axis='both', labelsize=int(18 * v_figsize[0]/16))

    # Convert angles from radians to degrees
    v_aux_angles = np.degrees(v_angles_rad[d_start:d_end])

    # Plot sonar with angles
    h_im = h_ax.imshow(m_sonar, aspect='auto', interpolation='none', extent=[v_aux_angles[0], 360, v_range[-1], 0])

    # Adjust the tick labels
    h_ax.set_xticks(np.linspace(0, 360, 13))
    h_ax.set_xticklabels(['0°', '30°', '60°', '90°', '120°', '150°', '180°', '210°', '240°', '270°', '300°', '330°', '360°'])

    # Add grid
    h_ax.grid()

    # Add colorbar
    h_cax = h_fig.add_axes([h_ax.get_position().x1 + 0.01, h_ax.get_position().y0, 0.02, h_ax.get_position().height])
    h_colorbar = plt.colorbar(h_im, cax=h_cax)
    h_colorbar.ax.tick_params(labelsize=int(18 * v_figsize[0]/16))

    # Note: plt.show() is commented out to allow for further customization if needed

# %% ../nbs/00_core.ipynb 68
def f_animation_AUV_micron_trajectory(m_interp_pos_micron_final: np.ndarray, 
                                      m_sonarReceivedIntensity_micron_final: np.ndarray, 
                                      v_range_micron: np.ndarray) -> animation.FuncAnimation:
    r"""
    Create an animation of AUV trajectory along with horizontal sonar data visualization.
    
    ::: {.callout-note}
    This function generates a dynamic visualization combining the AUV's trajectory in 3D space 
    with its horizontal sonar data. It provides a comprehensive view of the AUV's movement and 
    the underwater environment it's scanning, useful for mission analysis and data interpretation.
    :::

    **Parameters**  
     \ \  - `m_interp_pos_micron_final`: numpy.ndarray of interpolated AUV position data, shape (n_points, 3)  
     \ \  - `m_sonarReceivedIntensity_micron_final`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_points)  
     \ \  - `v_range_micron`: numpy.ndarray representing the range values of the sonar data  

    **Returns**  
     \ \  matplotlib.animation.FuncAnimation: Animation object of the AUV trajectory and sonar data  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    from matplotlib import animation
    from IPython.display import HTML

    # Generate sample data
    d_n_points = 50000
    d_n_ranges = 397

    # AUV trajectory data
    m_interp_pos_micron_final = np.column_stack((
        np.linspace(0, 100, d_n_points),  # x
        10 * np.sin(np.linspace(0, 8*np.pi, d_n_points)),  # y
        -5 * np.cos(np.linspace(0, 4*np.pi, d_n_points))   # z
    ))

    # Sonar intensity data
    m_sonarReceivedIntensity_micron_final = np.random.rand(d_n_ranges, d_n_points)
    v_range_micron = np.linspace(0, 20, d_n_ranges)

    # Create animation
    anim = f_animation_AUV_micron_trajectory(m_interp_pos_micron_final, 
                                             m_sonarReceivedIntensity_micron_final, 
                                             v_range_micron)

    # Display animation in notebook
    HTML(anim.to_jshtml())
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the AUV trajectory and sonar animation process**

    1. **Animation Setup**:
       - Creates a figure with a 16:9 aspect ratio, suitable for widescreen displays.
       - Loads and resizes a background image for context (e.g., satellite view of the mission area).

    2. **Update Function**:
       - Defines `update_plot` function that is called for each frame of the animation.
       - Clears the previous frame and sets up a new grid of subplots for each update.

    3. **Trajectory Visualization**:
       - Plots the full AUV trajectory and the current segment in two 3D subplots.
       - One subplot shows a top-down view, the other a perspective view.
       - Uses color-coding for start and end points, and different colors for full and current trajectories.

    4. **Sonar Data Visualization**:
       - Displays the horizontal sonar data as a 2D color plot below the trajectory plots.
       - Shows a moving window of sonar data corresponding to the current position in the trajectory.

    5. **Layout and Styling**:
       - Uses a grid layout to organize the background image, 3D trajectory plots, and sonar data plot.
       - Applies consistent styling, including font sizes and color schemes.

    6. **Animation Creation**:
       - Uses `matplotlib.animation.FuncAnimation` to create the animation.
       - Sets the frame range, interval, and links it to the update function.

    This approach provides a dynamic and comprehensive visualization of both the AUV's movement 
    through 3D space and the sonar data it collects along its trajectory, enabling intuitive 
    interpretation of the mission data and environmental features.
    :::

    **References**  
    \ \ 1. [Matplotlib Animation](https://matplotlib.org/stable/api/animation_api.html)  

    """
    
    def update_plot(d_index):
        d_image_width = 1400
        d_start_micron = d_index
        d_end_micron = d_start_micron + d_image_width

        # Create a 3D trajectory
        m_trajectory = m_interp_pos_micron_final[d_start_micron:d_end_micron, :].copy()
        m_trajectory[:, 2] = -m_trajectory[:, 2]

        m_trajectory_tot = m_interp_pos_micron_final.copy()
        m_trajectory_tot[:, 2] = -m_trajectory_tot[:, 2]

        # Create the subplots
        h_fig.clear()
        h_gs = gridspec.GridSpec(6, 6, wspace=0.1, hspace=0.1)

        h_ax1 = h_fig.add_subplot(h_gs[0:3, 0:2])
        h_ax1.axis('off')
        h_ax1.imshow(m_im)

        h_ax = h_fig.add_subplot(h_gs[0:3, 2:4], projection="3d", proj_type="ortho")
        h_ax.cla()
        h_ax.view_init(elev=-90, azim=90, roll=-90)

        t_colors = ("#FF6666", "#005533", "#1199EE")  # Colorblind-safe RGB

        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory",
                  color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green',
                     label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red',
                     label='End')
        h_ax.legend()

        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], label="Trajectory", color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red', label='End')

        # Second view in 3D
        h_ax = h_fig.add_subplot(h_gs[0:3, 4:6], projection="3d", proj_type="ortho")
        h_ax.cla()
        h_ax.view_init(elev=30, azim=45, roll=0)

        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory",
                  color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green',
                     label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red',
                     label='End')
        h_ax.legend()

        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], label="Trajectory", color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red', label='End')

        m_sonar_micron = m_sonarReceivedIntensity_micron_final[:, d_start_micron:d_end_micron].copy()

        h_ax2 = h_fig.add_subplot(h_gs[3:6, 0:6])
        h_ax2.set_title(" "*50 + "Horizontal Sonar", {'fontsize': 24})
        h_ax2.set_xlabel('1 revolution every 200 points', {'fontsize': 18})
        h_ax2.set_ylabel('397 bins over 19.95 meters', {'fontsize': 18})
        h_im = h_ax2.imshow(m_sonar_micron, aspect='auto', interpolation='none',
                            extent=[d_start_micron, d_end_micron, v_range_micron[-1], 0])

        h_cax = h_fig.add_axes([h_ax2.get_position().x1 + 0.01, h_ax2.get_position().y0, 0.02, h_ax2.get_position().height])
        h_colorbar = plt.colorbar(h_im, cax=h_cax)
        h_colorbar.ax.tick_params(labelsize=18)

        return h_fig

    # Create the initial figure
    h_fig = plt.figure(figsize=(16, 9), facecolor='white')

    # Load the image
    m_im = Image.open(os.path.join('.','img','CavesGoogleEarth2.jpg'))
    t_resize = (800, 600)
    m_im = m_im.resize(t_resize)

    # Create the animation
    h_ani = animation.FuncAnimation(h_fig, update_plot, frames=range(194, 45587 - 1400 - 1, 200), interval=100)

    # Close figure to prevent extra osd
    plt.close()
    return h_ani

# %% ../nbs/00_core.ipynb 69
def f_create_polar_sonar_animation(m_sonarReceivedIntensity_micron_final: np.ndarray, 
                                   v_angles_rad_micron_final: np.ndarray) -> animation.FuncAnimation:
    r"""
    Create an animation of sonar data in polar view, visualizing the scanning process over time.
    
    ::: {.callout-note}
    This function generates a dynamic polar plot of sonar data, simulating the rotating scan 
    of a sonar sensor. It's particularly useful for visualizing how the sonar beam sweeps 
    across the environment, providing an intuitive representation of the scanning process 
    and detected features.
    :::

    **Parameters**  
     \ \  - `m_sonarReceivedIntensity_micron_final`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_angles)  
     \ \  - `v_angles_rad_micron_final`: numpy.ndarray of angle data in radians, shape (n_angles,)  

    **Returns**  
     \ \  matplotlib.animation.FuncAnimation: Animation object of the polar sonar scan  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    from matplotlib import animation
    from IPython.display import HTML

    # Generate sample sonar data
    d_n_ranges = 397  # 397 bins over 19.95 meters
    d_n_angles = 2000  # Multiple revolutions
    m_sonarReceivedIntensity_micron_final = np.random.rand(d_n_ranges, d_n_angles)
    v_angles_rad_micron_final = np.linspace(0, 10*2*np.pi, d_n_angles)  # 10 full rotations

    # Create some artificial features in the sonar data
    for i in range(10):  # Create features in each rotation
        angle_start = i * 200
        m_sonarReceivedIntensity_micron_final[50:70, angle_start+30:angle_start+50] = 0.8  # Object at about 45 degrees
        m_sonarReceivedIntensity_micron_final[100:120, angle_start+90:angle_start+110] = 0.9  # Object at about 180 degrees
        m_sonarReceivedIntensity_micron_final[30:40, angle_start+150:angle_start+170] = 0.7  # Object at about 270 degrees

    # Create animation
    anim = f_create_polar_sonar_animation(m_sonarReceivedIntensity_micron_final, v_angles_rad_micron_final)

    # Display animation in notebook
    HTML(anim.to_jshtml())
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the polar sonar animation process**

    1. **Animation Setup**:
       - Creates a figure with a polar projection, suitable for circular sonar scans.
       - Initializes the plot with the first few data points.

    2. **Polar Plot Customization**:
       - Sets the angle direction to clockwise with 0 degrees at the top, matching typical sonar conventions.
       - Customizes angular tick labels to show degrees from 0° to 330°.

    3. **Color Mapping**:
       - Uses a colorbar to represent sonar intensity, allowing for easy interpretation of signal strength.

    4. **Animation Function**:
       - Defines `f_animate` function that updates the plot for each frame.
       - Adds new data to the plot in small increments, simulating a rotating sonar beam.
       - Handles wraparound when the scan completes a full revolution.

    5. **Animation Creation**:
       - Uses `matplotlib.animation.FuncAnimation` to create the animation.
       - Sets the frame range, interval, and links it to the update function.

    This approach provides a dynamic visualization of the sonar scanning process, allowing 
    viewers to observe how the sonar data is built up over time and how features in the 
    environment appear and evolve during the scan.
    :::

    **References**  
    \ \ 1. [Matplotlib Animation in Polar Coordinates](https://matplotlib.org/stable/gallery/animation/animate_polar.html)  

    """
    d_start = 194
    d_end = d_start + 200 * 5  # m_sonarReceivedIntensity_micron_final.shape[1]
    d_step = 5

    # Create the figure and axes
    h_fig, h_ax = plt.subplots(figsize=(10, 10), subplot_kw={'projection': 'polar'})

    # Initialize the plot
    h_im = h_ax.pcolormesh(v_angles_rad_micron_final[d_start:d_start+5], np.arange(m_sonarReceivedIntensity_micron_final.shape[0]),
                           m_sonarReceivedIntensity_micron_final[:, d_start:d_start+5], cmap='viridis')
    h_cbar = h_fig.colorbar(h_im, ax=h_ax)
    h_cbar.set_label('Sonar Intensity')

    # Set the angle direction to be clockwise with 0 degrees at the top
    h_ax.set_theta_direction(-1)
    h_ax.set_theta_offset(np.pi / 2)

    # Adjust the tick labels
    h_ax.set_xticks(np.linspace(0, 2 * np.pi, 13))
    h_ax.set_xticklabels(['0°', '30°', '60°', '90°', '120°', '150°', '180°', '210°', '240°', '270°', '300°', '330°', '0°'])

    h_ax.set_title('Sonar Data in Polar View')
    h_ax.set_xlabel('Range from 0 to 20 meters')
    h_ax.set_ylabel('Angle', labelpad=30)

    # Animation function
    def f_animate(d_i):
        d_aux_start = 194
        if (d_aux_start + (d_i + 1) * d_step) > d_aux_start + 200:
            d_aux_start = d_aux_start + 200
        else:
            d_aux_start = 194
        # Plot the new data on top of the previous data
        h_im = h_ax.pcolormesh(v_angles_rad_micron_final[d_aux_start + d_i * d_step:d_aux_start + (d_i + 1) * d_step],
                               np.arange(m_sonarReceivedIntensity_micron_final.shape[0]),
                               m_sonarReceivedIntensity_micron_final[:, d_aux_start + d_i * d_step:d_aux_start + (d_i + 1) * d_step],
                               cmap='viridis', rasterized=True)
        return [h_im]

    # Create the animation
    h_ani = FuncAnimation(h_fig, f_animate, interval=100, frames=(d_end - d_start) // d_step, repeat=False)
    plt.close(h_fig)
    
    return h_ani

# %% ../nbs/00_core.ipynb 71
def f_micron_with_trajectory(m_interp_pos_micron_final: np.ndarray, 
                             m_sonarReceivedIntensity_micron_final: np.ndarray, 
                             v_range_micron: np.ndarray) -> widgets.interactive:
    r"""
    Create an interactive visualization of AUV trajectory and sonar data.
    
    ::: {.callout-note}
    This function generates an interactive plot combining the AUV's 3D trajectory with its 
    horizontal sonar data. It allows users to explore different segments of the mission 
    using a slider, providing a comprehensive view of the AUV's movement and the 
    underwater environment it's scanning.
    :::

    **Parameters**  
     \ \  - `m_interp_pos_micron_final`: numpy.ndarray of interpolated AUV position data, shape (n_points, 3)  
     \ \  - `m_sonarReceivedIntensity_micron_final`: numpy.ndarray of sonar intensity data, shape (n_ranges, n_points)  
     \ \  - `v_range_micron`: numpy.ndarray representing the range values of the sonar data  

    **Returns**  
     \ \  ipywidgets.interactive: Interactive widget for exploring the AUV trajectory and sonar data  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    from ipywidgets import interactive
    from IPython.display import display

    # Generate sample data
    d_n_points = 50000
    d_n_ranges = 397

    # AUV trajectory data
    m_interp_pos_micron_final = np.column_stack((
        np.linspace(0, 100, d_n_points),  # x
        10 * np.sin(np.linspace(0, 8*np.pi, d_n_points)),  # y
        -5 * np.cos(np.linspace(0, 4*np.pi, d_n_points))   # z
    ))

    # Sonar intensity data
    m_sonarReceivedIntensity_micron_final = np.random.rand(d_n_ranges, d_n_points)
    v_range_micron = np.linspace(0, 20, d_n_ranges)

    # Create interactive widget
    widget = f_micron_with_trajectory(m_interp_pos_micron_final, 
                                      m_sonarReceivedIntensity_micron_final, 
                                      v_range_micron)

    # Display the widget
    display(widget)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the interactive visualization process**

    1. **Image Loading**:
       - Loads and resizes a background image (e.g., satellite view of the mission area) for context.

    2. **Update Function**:
       - Defines `update` function that is called whenever the slider value changes.
       - Creates a new figure for each update with a complex grid layout.

    3. **Trajectory Visualization**:
       - Plots the full AUV trajectory and the current segment in two 3D subplots.
       - One subplot shows a top-down view, the other a perspective view.
       - Uses color-coding for start and end points, and different colors for full and current trajectories.

    4. **Sonar Data Visualization**:
       - Displays the horizontal sonar data as a 2D color plot below the trajectory plots.
       - Shows a segment of sonar data corresponding to the current position in the trajectory.

    5. **Layout and Styling**:
       - Uses a grid layout to organize the background image, 3D trajectory plots, and sonar data plot.
       - Applies consistent styling, including font sizes and color schemes.

    6. **Interactive Widget Creation**:
       - Creates an interactive slider widget for selecting different segments of the mission data.
       - Links the slider to the update function for dynamic visualization.

    This approach provides an intuitive and interactive way to explore the relationship between 
    the AUV's movement through 3D space and the sonar data it collects, enabling detailed 
    analysis of the mission data and environmental features.
    :::

    **References**  
    \ \ 1. [ipywidgets Documentation](https://ipywidgets.readthedocs.io/en/latest/)  

    """
    # Load and resize the image
    m_im = Image.open(os.path.join('.','img','CavesGoogleEarth2.jpg'))
    t_resize = (800, 600)
    m_im = m_im.resize(t_resize)

    def update(d_index):
        # Clear previous output
        clear_output(wait=True)

        d_image_width = 1400
        d_start_micron = d_index
        d_end_micron = d_start_micron + d_image_width

        # Create a 3D trajectory
        m_trajectory = m_interp_pos_micron_final[d_start_micron:d_end_micron,:].copy()
        m_trajectory[:,2] = -m_trajectory[:,2]

        m_trajectory_tot = m_interp_pos_micron_final.copy()
        m_trajectory_tot[:,2] = -m_trajectory_tot[:,2]

        # Create the figure and subplots
        h_fig = plt.figure(figsize=(16, 9), facecolor='white')
        h_gs = gridspec.GridSpec(6, 6, wspace=0.1, hspace=0.1)

        h_ax1 = plt.subplot(h_gs[0:3, 0:2])
        h_ax1.axis('off')
        plt.imshow(m_im)

        # First 3D plot
        h_ax = h_fig.add_subplot(h_gs[0:3, 2:4], projection="3d", proj_type="ortho")

        t_colors = ("#FF6666", "#005533", "#1199EE")  # Colorblind-safe RGB
        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.view_init(elev=-90, azim=90, roll=-90)

        # Total 3D trajectory
        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory", color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red', label='End')
        h_ax.legend()

        # Current segment in orange
        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red')

        # Second 3D plot
        h_ax = h_fig.add_subplot(h_gs[0:3, 4:6], projection="3d", proj_type="ortho")

        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.view_init(elev=30, azim=45, roll=0)

        # Total trajectory again
        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory", color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red', label='End')
        h_ax.legend()

        # Current segment in orange
        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red')

        # Sonar data
        m_sonar_micron = m_sonarReceivedIntensity_micron_final[:, d_start_micron:d_end_micron].copy()

        h_ax2 = plt.subplot(h_gs[3:6, 0:6])
        h_ax2.set_title(" "*50 + "Horizontal Sonar", {'fontsize': 24})
        h_ax2.set_xlabel('1 revolution every 200 points', {'fontsize': 18})
        h_ax2.set_ylabel('397 bins over 19.95 meters', {'fontsize': 18})
        h_im = h_ax2.imshow(m_sonar_micron, aspect='auto', interpolation='none', extent=[d_start_micron, d_end_micron, v_range_micron[-1], 0])

        # Colorbar
        h_cax = h_fig.add_axes([h_ax2.get_position().x1+0.01, h_ax2.get_position().y0, 0.02, h_ax2.get_position().height])
        h_colorbar = plt.colorbar(h_im, cax=h_cax)
        h_colorbar.ax.tick_params(labelsize=18)

        # Show the figure
        plt.show()

        return h_fig

    h_slider = widgets.IntSlider(min=194, max=45587-1400-1, step=200, value=194)

    h_widget_micron_trajectory = interactive(
        update,
        d_index=h_slider
    )
    
    return h_widget_micron_trajectory

# %% ../nbs/00_core.ipynb 72
def f_seaking_with_trajectory(m_interp_pos_micron_final: np.ndarray, 
                              m_sonarReceivedIntensity_micron_final: np.ndarray, 
                              v_range_micron: np.ndarray) -> widgets.interactive:
    r"""
    Create an interactive visualization of AUV trajectory and vertical sonar data.
    
    ::: {.callout-note}
    This function generates an interactive plot combining the AUV's 3D trajectory with its 
    vertical sonar data. It allows users to explore different segments of the mission 
    using a slider, providing a comprehensive view of the AUV's movement and the 
    vertical profile of the underwater environment it's scanning.
    :::

    **Parameters**  
     \ \  - `m_interp_pos_micron_final`: numpy.ndarray of interpolated AUV position data, shape (n_points, 3)  
     \ \  - `m_sonarReceivedIntensity_micron_final`: numpy.ndarray of vertical sonar intensity data, shape (n_ranges, n_points)  
     \ \  - `v_range_micron`: numpy.ndarray representing the range values of the sonar data  

    **Returns**  
     \ \  ipywidgets.interactive: Interactive widget for exploring the AUV trajectory and vertical sonar data  

    **Example**
    
    ```python
    import numpy as np
    import matplotlib.pyplot as plt
    from ipywidgets import interactive
    from IPython.display import display

    # Generate sample data
    d_n_points = 100000
    d_n_ranges = 50

    # AUV trajectory data
    m_interp_pos_micron_final = np.column_stack((
        np.linspace(0, 100, d_n_points),  # x
        10 * np.sin(np.linspace(0, 8*np.pi, d_n_points)),  # y
        -5 * np.cos(np.linspace(0, 4*np.pi, d_n_points))   # z
    ))

    # Vertical sonar intensity data
    m_sonarReceivedIntensity_micron_final = np.random.rand(d_n_ranges, d_n_points)
    v_range_micron = np.linspace(0, 10, d_n_ranges)

    # Create interactive widget
    widget = f_seaking_with_trajectory(m_interp_pos_micron_final, 
                                       m_sonarReceivedIntensity_micron_final, 
                                       v_range_micron)

    # Display the widget
    display(widget)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on the interactive visualization process**

    1. **Image Loading**:
       - Loads and resizes a background image (e.g., satellite view of the mission area) for context.

    2. **Update Function**:
       - Defines `update` function that is called whenever the slider value changes.
       - Creates a new figure for each update with a complex grid layout.

    3. **Trajectory Visualization**:
       - Plots the full AUV trajectory and the current segment in two 3D subplots.
       - One subplot shows a top-down view, the other a perspective view.
       - Uses color-coding for start and end points, and different colors for full and current trajectories.

    4. **Vertical Sonar Data Visualization**:
       - Displays the vertical sonar data as a 2D color plot below the trajectory plots.
       - Shows a segment of sonar data corresponding to the current position in the trajectory.
       - The y-axis represents depth (50 bins over 9.80 meters), providing a vertical profile view.

    5. **Layout and Styling**:
       - Uses a grid layout to organize the background image, 3D trajectory plots, and sonar data plot.
       - Applies consistent styling, including font sizes and color schemes.

    6. **Interactive Widget Creation**:
       - Creates an interactive slider widget for selecting different segments of the mission data.
       - Links the slider to the update function for dynamic visualization.

    This approach provides an intuitive and interactive way to explore the relationship between 
    the AUV's movement through 3D space and the vertical sonar data it collects, enabling detailed 
    analysis of the mission data, bathymetry, and vertical environmental features.
    :::

    **References**  
    \ \ 1. [ipywidgets Documentation](https://ipywidgets.readthedocs.io/en/latest/)  

    """
    # Load and resize the image
    m_im = Image.open(os.path.join('.','img','CavesGoogleEarth2.jpg'))
    t_resize = (800, 600)
    m_im = m_im.resize(t_resize)

    def update(d_index):
        # Clear previous output
        clear_output(wait=True)

        d_image_width = 1400
        d_start_micron = d_index
        d_end_micron = d_start_micron + d_image_width
        
        # Create a 3D trajectory
        m_trajectory = m_interp_pos_micron_final[d_start_micron:d_end_micron,:].copy()
        m_trajectory[:,2] = -m_trajectory[:,2]

        m_trajectory_tot = m_interp_pos_micron_final.copy()
        m_trajectory_tot[:,2] = -m_trajectory_tot[:,2]

        # Create the subplots
        h_fig = plt.figure(figsize=(16, 9), facecolor='white')
        h_gs = gridspec.GridSpec(6, 6, wspace=0.1, hspace=0.1)
        
        h_ax1 = plt.subplot(h_gs[0:3, 0:2])
        h_ax1.axis('off')
        plt.imshow(m_im)
        
        # Create the axes for the first 3D plot
        h_ax = h_fig.add_subplot(h_gs[0:3, 2:4], projection="3d", proj_type="ortho")

        t_colors = ("#FF6666", "#005533", "#1199EE")  # Colorblind-safe RGB

        # Loop over x, y, z axes for the first 3D plot
        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.view_init(elev=-90, azim=90, roll=-90)

        # Add the total 3D trajectory
        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory", color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red', label='End')
        h_ax.legend()

        # Add the considered part in orange over the trajectory
        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red')

        # Create the axes for the second 3D plot
        h_ax = h_fig.add_subplot(h_gs[0:3, 4:6], projection="3d", proj_type="ortho")

        for d_i, (h_axis, s_c) in enumerate(zip((h_ax.xaxis, h_ax.yaxis, h_ax.zaxis), t_colors)):
            s_axlabel = h_axis.axis_name
            h_axis.set_label_text(s_axlabel)
            h_axis.label.set_color(s_c)
            h_axis.line.set_color(s_c)
            h_axis.set_tick_params(colors=s_c)

        h_ax.view_init(elev=30, azim=45, roll=0)

        # Add the total 3D trajectory
        h_ax.plot(m_trajectory_tot[:, 0], m_trajectory_tot[:, 1], m_trajectory_tot[:, 2], label="Trajectory", color='purple')
        h_ax.scatter(m_trajectory_tot[0, 0], m_trajectory_tot[0, 1], m_trajectory_tot[0, 2], color='green', label='Start')
        h_ax.scatter(m_trajectory_tot[-1, 0], m_trajectory_tot[-1, 1], m_trajectory_tot[-1, 2], color='red', label='End')
        h_ax.legend()

        # Add the considered part in orange over the trajectory
        h_ax.plot(m_trajectory[:, 0], m_trajectory[:, 1], m_trajectory[:, 2], color='orange')
        h_ax.scatter(m_trajectory[0, 0], m_trajectory[0, 1], m_trajectory[0, 2], color='green')
        h_ax.scatter(m_trajectory[-1, 0], m_trajectory[-1, 1], m_trajectory[-1, 2], color='red')

        # Plot the sonar data
        m_sonar_micron = m_sonarReceivedIntensity_micron_final[:, d_start_micron:d_end_micron].copy()

        h_ax2 = plt.subplot(h_gs[3:6, 0:6])
        h_ax2.set_title(" "*50 + "Vertical Sonar", {'fontsize': 24})
        h_ax2.set_xlabel('1 revolution every 201 points', {'fontsize': 18})
        h_ax2.set_ylabel('50 bins over 9.80 meters', {'fontsize': 18})
        h_im = h_ax2.imshow(m_sonar_micron, aspect='auto', interpolation='none', 
                            extent=[d_start_micron, d_end_micron, v_range_micron[-1], 0])

        # Create a colorbar for the sonar data
        h_cax = h_fig.add_axes([h_ax2.get_position().x1+0.01, h_ax2.get_position().y0, 0.02, h_ax2.get_position().height])
        h_colorbar = plt.colorbar(h_im, cax=h_cax)
        h_colorbar.ax.tick_params(labelsize=18)

        # Show the figure
        plt.show()

        return h_fig

    h_slider = widgets.IntSlider(min=95, max=97477-1400-1, step=200, value=95)

    h_widget_seaking_trajectory = interactive(
        update,
        d_index=h_slider
    )
    
    return h_widget_seaking_trajectory

# %% ../nbs/00_core.ipynb 78
def f_remove_macosx_folder(s_path: str) -> None:
    r"""
    Remove the __MACOSX folder and its contents if it exists.
    
    ::: {.callout-note}
    This function is designed to clean up the __MACOSX folder that is often created
    when unzipping files originating from macOS systems.
    :::

    **Parameters**  
     \ \  - `s_path`: String representing the path where to look for the __MACOSX folder  

    **Returns**  
     \ \  None  

    **Example**
    
    ```python
    s_extract_path = "/path/to/extracted/folder"
    f_remove_macosx_folder(s_extract_path)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Implementation Details**

    1. The function uses `os.path.join()` to create a platform-independent path to the __MACOSX folder.
    2. It checks for the existence of the folder using `os.path.exists()`.
    3. If the folder exists, it uses `shutil.rmtree()` to recursively remove the folder and its contents.
    4. Error handling is implemented to catch and report any issues during the removal process.
    :::

    **References**  
    \ \ 1. [Python os.path module](https://docs.python.org/3/library/os.path.html)  
    \ \ 2. [Python shutil module](https://docs.python.org/3/library/shutil.html)  

    """
    s_macosx_path = os.path.join(s_path, '__MACOSX')
    
    # Check if the __MACOSX folder exists
    if os.path.exists(s_macosx_path):
        try:
            # Attempt to remove the __MACOSX folder and its contents
            shutil.rmtree(s_macosx_path)
            print(f"Successfully removed {s_macosx_path}")
        except Exception as e:
            # Handle any errors that occur during the removal process
            print(f"Error removing {s_macosx_path}: {str(e)}")
    else:
        # Inform if the __MACOSX folder doesn't exist
        print(f"No __MACOSX folder found in {s_path}")


# %% ../nbs/00_core.ipynb 79
def f_unzip_file(s_zip_path: str, s_extract_path: str) -> None:
    r"""
    Unzip a file from the given zip_path to the extract_path.
    
    ::: {.callout-note}
    This function extracts the contents of a zip file to a specified directory
    and removes the __MACOSX folder if present.
    :::

    **Parameters**  
     \ \  - `s_zip_path`: String representing the path to the zip file  
     \ \  - `s_extract_path`: String representing the path where the contents will be extracted  

    **Returns**  
     \ \  None  

    **Example**
    
    ```python
    s_zip_file = "/path/to/archive.zip"
    s_extract_dir = "/path/to/extract/folder"
    f_unzip_file(s_zip_file, s_extract_dir)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Implementation Details**

    1. The function first checks if the zip file exists.
    2. It creates the extraction directory if it doesn't exist.
    3. The zip file is then extracted using the `zipfile` module.
    4. After extraction, it calls `f_remove_macosx_folder` to clean up any __MACOSX folder.
    5. Error handling is implemented for various scenarios, including invalid zip files.
    :::

    **References**  
    \ \ 1. [Python zipfile module](https://docs.python.org/3/library/zipfile.html)  
    \ \ 2. [Python os module](https://docs.python.org/3/library/os.html)  

    """
    # Check if the zip file exists
    if not os.path.exists(s_zip_path):
        print(f"Error: The zip file {s_zip_path} does not exist.")
        return

    # Create the extract directory if it doesn't exist
    if not os.path.exists(s_extract_path):
        os.makedirs(s_extract_path)

    # Unzip the file
    try:
        with zipfile.ZipFile(s_zip_path, 'r') as h_zip_ref:
            h_zip_ref.extractall(s_extract_path)
        print(f"Successfully extracted {s_zip_path} to {s_extract_path}")
        
        # Remove __MACOSX folder
        f_remove_macosx_folder(s_extract_path)
    except zipfile.BadZipFile:
        print(f"Error: {s_zip_path} is not a valid zip file.")
    except Exception as e:
        print(f"An error occurred while extracting the zip file: {str(e)}")


# %% ../nbs/00_core.ipynb 82
def f_format_time(d_timestamp: float) -> str:
    r"""
    Convert timestamp to HH:MM:SS format.
    
    ::: {.callout-note}
    This function converts a Unix timestamp to a formatted time string using UTC.
    :::

    **Parameters**  
     \ \  - `d_timestamp`: Unix timestamp (seconds since epoch)  

    **Returns**  
     \ \  str: Formatted time string in HH:MM:SS format  

    **Example**
    
    ```python
    d_timestamp = 1609459200  # 2021-01-01 00:00:00 UTC
    s_formatted_time = f_format_time(d_timestamp)
    print(s_formatted_time)  # Output: "00:00:00"
    ```
    """
    return datetime.fromtimestamp(d_timestamp, tz=timezone.utc).strftime('%H:%M:%S')

# %% ../nbs/00_core.ipynb 83
def f_add_packages_to_all() -> None:
    r"""
    Add commonly used package names to the global __all__ list.
    
    ::: {.callout-note}
    This function populates or extends the global __all__ list with names of commonly uszed 
    packages and modules. This is useful for controlling what is exported when using 
    'from module import *' in other parts of the project.
    :::

    **Parameters**  
     \ \  None

    **Returns**  
     \ \  None

    **Example**
    
    ```python
    # Before calling the function
    print(globals().get('__all__', 'Not defined'))

    # Call the function
    f_add_packages_to_all()

    # After calling the function
    print(__all__)
    ```
    
    ::: {.callout-tip collapse=true}
    #### **Details on package addition process**

    1. **Global __all__ Creation**:
       - Checks if __all__ exists in the global namespace.
       - If not, it creates an empty __all__ list.

    2. **Package List**:
       - Defines a comprehensive list of commonly used package and module names.
       - Includes packages for numerical computing, data manipulation, visualization, 
         scientific computing, and more.

    3. **List Extension**:
       - Extends the __all__ list with the defined package names.

    4. **Usage in Modules**:
       - This function is typically called at the end of a module file.
       - It ensures that all necessary package names are included in __all__.

    5. **Import Control**:
       - Helps in controlling what is imported when using 'from module import *'.
       - Useful for creating a clean namespace when importing the module.

    6. **Maintenance**:
       - The list of packages can be easily updated or extended as project requirements change.

    Note: While this approach can be convenient, it's generally recommended to use 
    explicit imports in production code for better readability and maintainability.
    :::

    **References**  
    \ \ 1. [Python __all__ special variable](https://docs.python.org/3/tutorial/modules.html#importing-from-a-package)  
    \ \ 2. [PEP 8 -- Style Guide for Python Code](https://www.python.org/dev/peps/pep-0008/#imports)  
    \ \ 3. [Python Modules and Packages](https://docs.python.org/3/tutorial/modules.html)  

    """
    global __all__
    if '__all__' not in globals():
        __all__ = []
    from datetime import datetime, timezone
    l_packages = [
        'np', 'datetime', 'timezone', 'math', 'plt', 'animation', 'FuncAnimation', 
        'Slider', 'Button', 'RadioButtons', 'Axes3D', 'gridspec', 'go', 'Image', 
        'interact', 'widgets', 'interactive', 'IntSlider', 'os', 'subprocess', 
        'pd', 'R', 'Slerp', 'scipy', 'HTML', 'display', 'clear_output', 'Markdown', 'disk', 'rank', 'rgb2gray', 'data', 
        'filters', 'cmd', 'cli', 'warnings', 'pytest', 'Tuple', 'Optional', 
        'List', 'Dict', 'Union', 'pickle', 'time', 'pio', 'zipfile', 'shutil', 'MaxNLocator', 're'
    ]
    __all__.extend(l_packages)

# Call the function to add packages to __all__
f_add_packages_to_all()
