i have the following script. Unfortunately the movement calculated is always 0. most of the delta_times are 1e-6 but maximum value is correct. :import os

import numpy as np
import pandas as pd

from src.raw_dataset_configs import CoordinateDataKeys, RawDataFolder
from src.selector_configs import SelectedData

FPS = 10
TOOL_LENGTH_ESTIMATION = 100

def pre_filter_unrecorded_nir(
    image_list: list[str], raw_coordinates_dataframe: pd.DataFrame, margin: float = 0.5
):
    start_timestamp = int(
        (raw_coordinates_dataframe["timestamp"].min() + margin) * 1000000
    )
    end_timestamp = int(
        (raw_coordinates_dataframe["timestamp"].max() - margin) * 1000000
    )

    return [
        image_name
        for image_name in image_list
        if start_timestamp < int(os.path.splitext(image_name)[0]) < end_timestamp
    ]


def get_interpolated_data(image_name: str, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0):
    timestamp = float(os.path.splitext(image_name)[0]) / 1000000 + offset
    timestamps = raw_coordinates_dataframe["timestamp"]

    # Find the two closest timestamps
    closest_indices = timestamps.sub(timestamp).abs().nsmallest(2).index
    closest_timestamps = timestamps.loc[closest_indices]

    # Interpolate data for these timestamps
    if len(closest_timestamps) == 2:
        lower_idx, upper_idx = closest_indices
        lower_time, upper_time = closest_timestamps.values
        ratio = (timestamp - lower_time) / (upper_time - lower_time)
        # Avoid division by zero if upper_time and lower_time are the same
        if upper_time == lower_time:
            ratio = 0.0
        interpolated_data = raw_coordinates_dataframe.iloc[lower_idx] + (
            raw_coordinates_dataframe.iloc[upper_idx] - raw_coordinates_dataframe.iloc[lower_idx]
        ) * ratio
        # Add delta_time to the interpolated data
        interpolated_data['delta_time'] = upper_time - lower_time  # seconds
    else:
        interpolated_data = raw_coordinates_dataframe.iloc[closest_indices[0]]
        # Set delta_time to a default value or zero
        interpolated_data['delta_time'] = 0.0  # seconds

    return interpolated_data

def match_images_dataframes(image_list, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0) -> pd.DataFrame:
    interpolated_data_list = [
        get_interpolated_data(image_name, raw_coordinates_dataframe, offset)
        for image_name in image_list
    ]

    # Convert the list of Series to a DataFrame
    matched_data = pd.DataFrame(interpolated_data_list)
    # Add the image names
    matched_data = matched_data.assign(image_name=image_list).reset_index(drop=True)

    return matched_data


# def get_closest_timestamp_data(
#     image_name: str, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0
# ) -> int:
#     timestamp = float(os.path.splitext(image_name)[0]) / 1000000 + offset
#     return int(raw_coordinates_dataframe["timestamp"].sub(timestamp).abs().idxmin())
#
#
# def match_images_dataframes(
#     image_list: list[str], raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0
# ) -> pd.DataFrame:
#     row_list = [
#         get_closest_timestamp_data(image_name, raw_coordinates_dataframe, offset)
#         for image_name in image_list
#     ]
#
#     matched_data = raw_coordinates_dataframe.iloc[row_list]
#
#     return matched_data.assign(image_name=image_list).reset_index(drop=True)


def filter_quality_recorded_trackers(
    coordinate_dataframe: pd.DataFrame,
    quality_threshold: float = 0.5,
    verbose: bool = True,
) -> pd.DataFrame:
    coordinate_dataframe = coordinate_dataframe[
        coordinate_dataframe["quality"] > quality_threshold
    ]

    if verbose:
        print(
            f"Filtering trackers quality... Keeping {coordinate_dataframe.shape[0]} frames"
        )

    return coordinate_dataframe


def filter_blurry(
    coordinate_dataframe: pd.DataFrame,
    movement_threshold_mm: float = 7,
    verbose: bool = True,
) -> pd.DataFrame:
    # Copy the dataframe to avoid modifying the original data
    coordinate_dataframe = coordinate_dataframe.copy()

    # Ensure that 'delta_time' is present in the dataframe
    if 'delta_time' not in coordinate_dataframe.columns:
        raise ValueError("delta_time column is missing in coordinate_dataframe")

    # Extract rotation speeds (angular velocities) components in rad/s
    rotation_speeds_columns = CoordinateDataKeys.TOOL_ROTATION_SPEED
    rotation_speeds = coordinate_dataframe[rotation_speeds_columns].to_numpy()

    # Extract translation speeds components
    translation_speeds_columns = CoordinateDataKeys.TOOL_TRANSLATION_SPEED
    translation_speeds = coordinate_dataframe[translation_speeds_columns].to_numpy()

    # Confirm units and convert if necessary
    # Assuming translation speeds are in meters per second (m/s), convert to mm/s
    translation_speeds *= 1000  # Convert from m/s to mm/s

    # Extract delta_time from the dataframe
    delta_times = coordinate_dataframe['delta_time'].to_numpy()  # seconds

    # Handle zero or very small delta_times to avoid division by zero or negligible movements
    min_delta_time = 1e-6  # Minimum allowable delta_time (1 microsecond)
    delta_times = np.where(delta_times < min_delta_time, min_delta_time, delta_times)

    # Use your specific position vector from rotation center to tool tip (in mm)
    r_tip = np.array([70, -1, 250])  # mm

    # Number of frames
    num_frames = rotation_speeds.shape[0]

    # Repeat r_tip for each frame
    r_tips = np.tile(r_tip, (num_frames, 1))  # Shape: (num_frames, 3)

    # Calculate the linear velocity at the tool tip due to rotation for each frame
    v_rotational = np.cross(rotation_speeds, r_tips)  # rad/s x mm -> mm/s

    # Calculate the total velocity at the tool tip
    v_total = translation_speeds + v_rotational  # mm/s

    # Calculate the movement during each time interval
    estimated_movement = np.linalg.norm(v_total, axis=1) * delta_times  # mm

    # Add the estimated movement to the dataframe
    coordinate_dataframe["estimated_movement"] = estimated_movement
    print("Rotation Speeds:")
    print("rotation_speeds shape:", rotation_speeds.shape)
    print(rotation_speeds)
    print("Translation Speeds:")
    print("translation_speeds shape:", translation_speeds.shape)
    print(translation_speeds)
    
    print("delta_times:", delta_times)
    print("Minimum delta_time:", np.min(delta_times))
    print("Maximum delta_time:", np.max(delta_times))

    # Filter out frames where the estimated movement exceeds the threshold
    coordinate_dataframe = coordinate_dataframe[
        coordinate_dataframe["estimated_movement"] < movement_threshold_mm
    ]

    if verbose:
        print(
            f"Filtering blurry images... Keeping {coordinate_dataframe.shape[0]} frames"
        )

    return coordinate_dataframe


# def _synchronise_fiducials_timestamps(
#     selected_data: SelectedData, triangulated_points: pd.DataFrame
# ) -> pd.DataFrame:
#     timestamps = selected_data.coordinate_dataframe["timestamp"].to_numpy()
#
#     return triangulated_points[triangulated_points["time_stamp"].isin(timestamps)]
#

import pandas as pd


def interpolate_data_at_timestamp(timestamp: float, dataframe: pd.DataFrame, fiducial_indexes: list) -> pd.DataFrame:
    interpolated_frames = []

    for fiducial_index in fiducial_indexes:
        # Filter the dataframe for the specific fiducial_index
        df_filtered = dataframe[dataframe['fiducial_index'] == fiducial_index]

        # Find the two closest distinct timestamps for the specific fiducial_index
        closest_indices = df_filtered['time_stamp'].sub(timestamp).abs().nsmallest(2).index
        closest_timestamps = df_filtered.loc[closest_indices, 'time_stamp']

        if len(closest_timestamps) == 2:
            lower_idx, upper_idx = closest_indices
            lower_time, upper_time = closest_timestamps

            # Check if lower_time and upper_time are equal to avoid division by zero
            if lower_time == upper_time:
                interpolated_data = df_filtered.loc[lower_idx]
            else:
                ratio = (timestamp - lower_time) / (upper_time - lower_time)
                interpolated_data = df_filtered.loc[lower_idx] + (
                            df_filtered.loc[upper_idx] - df_filtered.loc[lower_idx]) * ratio
        else:
            interpolated_data = df_filtered.loc[closest_indices[0]]

        # Add the DataFrame to the list
        interpolated_frames.append(interpolated_data.to_frame().T)

    # Combine all interpolated frames
    combined_interpolated_data = pd.concat(interpolated_frames, axis=0)
    return combined_interpolated_data


def _synchronise_fiducials_timestamps(selected_data, triangulated_points: pd.DataFrame) -> pd.DataFrame:
    timestamps = selected_data.coordinate_dataframe["timestamp"].to_numpy()
    fiducial_indexes = set(triangulated_points["fiducial_index"])

    interpolated_data_frames = []

    for timestamp in timestamps:
        interpolated_data = interpolate_data_at_timestamp(timestamp, triangulated_points, fiducial_indexes)
        interpolated_data_frames.append(interpolated_data)

    # Combine all interpolated data frames
    all_interpolated_data = pd.concat(interpolated_data_frames, axis=0).reset_index(drop=True)

    return all_interpolated_data


def _get_fiducial_dataframe(
    synchronised_triangulated_points: pd.DataFrame, fiducial_index: int
) -> pd.DataFrame:
    fiducial_dataframe = synchronised_triangulated_points[
        synchronised_triangulated_points["fiducial_index"] == fiducial_index
    ]

    fiducial_dataframe = fiducial_dataframe[["time_stamp", "x", "y", "z"]]
    fiducial_dataframe = fiducial_dataframe.drop_duplicates(
        subset=["time_stamp"], keep="first"
    )

    return fiducial_dataframe.rename(
        columns={
            "x": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[0],
            "y": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[1],
            "z": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[2],
        }
    )


def add_fiducial_coordinates(
    selected_data: SelectedData, verbose: bool = False
) -> None:
    if verbose:
        print("Joining fiducials...")

    triangulated_points_file = os.path.join(
        selected_data.data_folder, RawDataFolder.TRIANGULATED_POINTS
    )
    triangulated_points = pd.read_csv(triangulated_points_file, delimiter=r"\s+")

    synchronised_triangulated_points = _synchronise_fiducials_timestamps(
        selected_data, triangulated_points
    )



    for fiducial_index in range(4):
        fiducial_dataframe = _get_fiducial_dataframe(
            synchronised_triangulated_points, fiducial_index
        )

        selected_data.coordinate_dataframe = pd.merge(
            selected_data.coordinate_dataframe,
            fiducial_dataframe,
            left_on="timestamp",
            right_on="time_stamp",
            how="inner",
            validate="one_to_one",
        ).drop(columns=["time_stamp"])