main.py

from removenoisebeforeik import *
from runik import *
from getquats import *
from angles import *

import tkinter as tk
from tkinter import filedialog

from scipy.spatial.distance import euclidean
from fastdtw import fastdtw

import shutil
import csv


def plotSyncedAngles(time_mm, time_kinect, shoulder_chest_angles_mm_shifted, shoulder_chest_angles_kinect_shifted,
                     elbow_angles_mm_shifted, elbow_angles_kinect_shifted, movement, viewpoint, participant, trial):

    # Remove '-k' from the movement string if it exists
    clean_movement = movement.replace('-k', '')

    # Mapping of movement codes to full names
    movement_mapping = {
        'elbowflex': 'Elbow Flexion',
        'functchange': 'Collect Change',
        'functcup': 'Drink From Cup',
        'functhair': 'Brush Hair',
        'functper': 'Perineal Care',
        'functspoon': 'Eat With Spoon',
        'sabd': 'Shoulder Abduction',
        'sflex': 'Shoulder Flexion',
        'srot': 'Shoulder Rotation',
        'sup2pro': 'Pronation Supination'
    }

    # Replace clean_movement with the corresponding full name
    clean_movement = movement_mapping.get(clean_movement, clean_movement)

    # Create a figure with three subplots for the shoulder angles
    fig, axs = plt.subplots(3, 1, figsize=(18, 18))  # 3 rows, 1 column for shoulder

    # Subplot 1: Plane of Elevation (Shoulder)
    axs[0].plot(time_mm, shoulder_chest_angles_mm_shifted[:, 0], color='blue', linestyle='-', linewidth=2, label='Plane of Elevation MM')
    axs[0].plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 0], color='green', linestyle='-', linewidth=2, label='Plane of Elevation Kinect')
    axs[0].set_ylabel('Angle (°)')
    axs[0].legend(loc='upper right')
    axs[0].set_title(f'{clean_movement} - Plane of Elevation (Shoulder)')

    # Subplot 2: Angle of Elevation (Shoulder)
    axs[1].plot(time_mm, shoulder_chest_angles_mm_shifted[:, 1], color='blue', linestyle='-', linewidth=2, label='Angle of Elevation MM')
    axs[1].plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 1], color='green', linestyle='-', linewidth=2, label='Angle of Elevation Kinect')
    axs[1].set_ylabel('Angle (°)')
    axs[1].legend(loc='upper right')
    axs[1].set_title(f'{clean_movement} - Angle of Elevation (Shoulder)')

    # Subplot 3: Rotation (Shoulder)
    axs[2].plot(time_mm, shoulder_chest_angles_mm_shifted[:, 2], color='blue', linestyle='-', linewidth=2, label='Rotation MM')
    axs[2].plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 2], color='green', linestyle='-', linewidth=2, label='Rotation Kinect')
    axs[2].set_ylabel('Angle (°)')
    axs[2].set_xlabel('Time (s)')
    axs[2].legend(loc='upper right')
    axs[2].set_title(f'{clean_movement} - Rotation (Shoulder)')

    plt.subplots_adjust(hspace=0.3)  # Adjust space between subplots

    # Construct directory path based on participant, device, and viewpoint
    directory_path = os.path.join('results', trial, participant, viewpoint, 'charts')
    os.makedirs(directory_path, exist_ok=True)

    # Save the shoulder angles plot
    shoulder_filename = f"{clean_movement}_shoulder_angles_subplot.png"
    plt.savefig(os.path.join(directory_path, shoulder_filename), bbox_inches='tight')
    plt.close()  # Close the plot to avoid displaying it

    # Create a new figure with two subplots for the elbow angles
    fig, axs = plt.subplots(2, 1, figsize=(18, 12))  # 2 rows, 1 column for elbow

    # Subplot 1: Flexion (Elbow)
    axs[0].plot(time_mm, elbow_angles_mm_shifted[:, 0], color='blue', linestyle='-', linewidth=2, label='Flexion MM')
    axs[0].plot(time_kinect, elbow_angles_kinect_shifted[:, 0], color='green', linestyle='-', linewidth=2, label='Flexion Kinect')
    axs[0].set_ylabel('Angle (°)')
    axs[0].legend(loc='upper right')
    axs[0].set_title(f'{clean_movement} - Flexion (Elbow)')

    # Subplot 2: Supination/Pronation (Elbow)
    axs[1].plot(time_mm, elbow_angles_mm_shifted[:, 1], color='blue', linestyle='-', linewidth=2, label='Supination/Pronation MM')
    axs[1].plot(time_kinect, elbow_angles_kinect_shifted[:, 1], color='green', linestyle='-', linewidth=2, label='Supination/Pronation Kinect')
    axs[1].set_ylabel('Angle (°)')
    axs[1].set_xlabel('Time (s)')
    axs[1].legend(loc='upper right')
    axs[1].set_title(f'{clean_movement} - Supination/Pronation (Elbow)')

    plt.subplots_adjust(hspace=0.3)  # Adjust space between subplots

    # Save the elbow angles plot
    elbow_filename = f"{clean_movement}_elbow_angles_subplot.png"
    plt.savefig(os.path.join(directory_path, elbow_filename), bbox_inches='tight')
    plt.close()  # Close the plot to avoid displaying it



# def plotSyncedAngles(time_mm,time_kinect,shoulder_chest_angles_mm_shifted,shoulder_chest_angles_kinect_shifted,elbow_angles_mm_shifted,elbow_angles_kinect_shifted, movement, viewpoint, participant,trial):
#     # Plot the YZY Euler sequence angles for the shoulder relative to the chest
#     # dont plot srot angles and elbow rot,carry due to errors:
#
#     plt.plot(time_mm, shoulder_chest_angles_mm_shifted[:, 0], label='plane of elevation mm')
#     plt.plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 0], label='plane of elevation kinect')
#     plt.xlabel('Frame')
#     plt.ylabel('Angle (degrees)')
#     plt.title('YZY Euler Sequence Angles for shoulder angles (humerus Relative to thorax)')
#     plt.legend()
#
#     second_legend_variable = f"{participant} {viewpoint} {movement} "
#     # Create the first legend
#     first_legend = plt.legend(loc='upper left')
#
#     # Add the first legend manually to the axes
#     plt.gca().add_artist(first_legend)
#
#     # Add centered secondary text below the legend
#     plt.gca().text(0.5, -0.2, second_legend_variable, transform=plt.gca().transAxes, fontsize=12, ha='center')
#
#     # Adjust the subplot parameters to give space for the text
#     plt.subplots_adjust(bottom=0.3)
#
#     # Construct new directory path based on participant, device, and viewpoint
#     directory_path = os.path.join('results', trial, participant, viewpoint, 'charts')
#
#     # Create the directory if it doesn't exist
#     os.makedirs(directory_path, exist_ok=True)
#
#     # Construct new filename
#     new_filename = f"{movement}_shoulder_plane_of_elevation.png"
#
#     # Save plot in the new directory with the new filename
#     plt.savefig(os.path.join(directory_path, new_filename))
#
#     # plt.show()
#     plt.close()
#
#     plt.plot(time_mm, shoulder_chest_angles_mm_shifted[:, 1], label='angle of elevation mm')
#     plt.plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 1], label='angle of elevation kinect')
#     plt.xlabel('Frame')
#     plt.ylabel('Angle (degrees)')
#     plt.title('YZY Euler Sequence Angles for shoulder angles (humerus Relative to thorax)')
#     plt.legend()
#
#     second_legend_variable = f"{participant} {viewpoint} {movement} "
#     # Create the first legend
#     first_legend = plt.legend(loc='upper left')
#
#     # Add the first legend manually to the axes
#     plt.gca().add_artist(first_legend)
#
#     # Add centered secondary text below the legend
#     plt.gca().text(0.5, -0.2, second_legend_variable, transform=plt.gca().transAxes, fontsize=12, ha='center')
#
#     # Adjust the subplot parameters to give space for the text
#     plt.subplots_adjust(bottom=0.3)
#
#     # Construct new filename
#     new_filename = f"{movement}_shoulder_angleofelevation.png"
#
#     # Save plot in the new directory with the new filename
#     plt.savefig(os.path.join(directory_path, new_filename))
#
#     # plt.show()
#     plt.close()
#
#     plt.plot(time_mm, shoulder_chest_angles_mm_shifted[:, 2], label='rotation mm')
#     plt.plot(time_kinect, shoulder_chest_angles_kinect_shifted[:, 2], label='rotation kinect')
#     plt.xlabel('Frame')
#     plt.ylabel('Angle (degrees)')
#     plt.title('YZY Euler Sequence Angles for shoulder angles (humerus Relative to thorax)')
#     plt.legend()
#
#     second_legend_variable = f"{participant} {viewpoint} {movement} "
#     # Create the first legend
#     first_legend = plt.legend(loc='upper left')
#
#     # Add the first legend manually to the axes
#     plt.gca().add_artist(first_legend)
#
#     # Add centered secondary text below the legend
#     plt.gca().text(0.5, -0.2, second_legend_variable, transform=plt.gca().transAxes, fontsize=12, ha='center')
#
#     # Adjust the subplot parameters to give space for the text
#     plt.subplots_adjust(bottom=0.3)
#
#     # Construct new filename
#     new_filename = f"{movement}_shoulder_rotation.png"
#
#     # Save plot in the new directory with the new filename
#     plt.savefig(os.path.join(directory_path, new_filename))
#
#     # plt.show()
#     plt.close()
#
#     # Plot the YZY Euler sequence angles for the shoulder relative to the chest
#     # dont plot srot angles and elbow rot,carry due to errors:
#
#     # if plotangle < 2:
#     plt.plot(time_mm, elbow_angles_mm_shifted[:, 0], label='elbow flexion mm')
#     plt.plot(time_kinect, elbow_angles_kinect_shifted[:, 0], label='elbow flexion kinect')
#     plt.xlabel('Frame')
#     plt.ylabel('Angle (degrees)')
#     plt.title('XYZ Euler Sequence Angles for elbow angles')
#     plt.legend()
#
#     second_legend_variable = f"{participant} {viewpoint} {movement} "
#     # Create the first legend
#     first_legend = plt.legend(loc='upper left')
#
#     # Add the first legend manually to the axes
#     plt.gca().add_artist(first_legend)
#
#     # Add centered secondary text below the legend
#     plt.gca().text(0.5, -0.2, second_legend_variable, transform=plt.gca().transAxes, fontsize=12, ha='center')
#
#     # Adjust the subplot parameters to give space for the text
#     plt.subplots_adjust(bottom=0.3)
#
#     # Construct new filename
#     new_filename = f"{movement}_elbow_flexion.png"
#
#     # Save plot in the new directory with the new filename
#     plt.savefig(os.path.join(directory_path, new_filename))
#     # plt.show()
#     plt.close()
#
#
#     plt.plot(time_mm, elbow_angles_mm_shifted[:, 1], label='supination pronation mm')
#     plt.plot(time_kinect, elbow_angles_kinect_shifted[:, 1], label='supination pronation kinect')
#     plt.xlabel('Frame')
#     plt.ylabel('Angle (degrees)')
#     plt.title('XYZ Euler Sequence Angles for elbow angles')
#     plt.legend()
#
#     second_legend_variable = f"{participant} {viewpoint} {movement} "
#     # Create the first legend
#     first_legend = plt.legend(loc='upper left')
#
#     # Add the first legend manually to the axes
#     plt.gca().add_artist(first_legend)
#
#     # Add centered secondary text below the legend
#     plt.gca().text(0.5, -0.2, second_legend_variable, transform=plt.gca().transAxes, fontsize=12, ha='center')
#
#     # Adjust the subplot parameters to give space for the text
#     plt.subplots_adjust(bottom=0.3)
#     # Construct new filename
#     new_filename = f"{movement}_elbow_sup_pro.png"
#
#     # Save plot in the new directory with the new filename
#     plt.savefig(os.path.join(directory_path, new_filename))
#
#     # plt.show()
#     plt.close()


def collect(trc_file,kinect,start,stop,trial):

    start = start
    stop = stop
    if not kinect:
        print("The filename contains 'mm'.")
        cutfilename = trc_file
    else:
        # prompt user to click start and end of keypoint data to remove noise from kinect signal before processing in osim
        cutfilename,start,stop = removeNoise(trc_file,start,stop,trial)
        print(f"cut file name is {cutfilename}")



    # run IK using opensim API
    if not kinect:
        if runIk_mm(cutfilename):
            print("inverse kinematics complete ")
        else:
            print("return IK failed")
    else:
        if runIk_k(cutfilename):
            print("inverse kinematics complete ")
        else:
            print("return IK failed")

    if not kinect:
        h = "mm"
        if getQuats(h):
            print("orientations of bodies complete")
        else:
            print("return orientations failed")
    else:
        # retrieve the orientations of opensim bodies after ik and return euler joint angles and save to csv
        h = "k"
        if getQuats(h):
            print("orientations of bodies complete")
        else:
            print("return orientations failed")

    # get orientations csv and save and return as angles numpy arrays
    plotfilename, shoulder_chest_angles, elbow_angles = getAngles(cutfilename)
    print(f"angles plot saved to charts/{plotfilename}")

    return shoulder_chest_angles, elbow_angles,start,stop


import numpy as np

def normalize_signal(signal):
    """Normalize the signal to zero mean and unit variance."""
    mean = np.mean(signal, axis=0)
    std = np.std(signal, axis=0)
    std[std == 0] = 1  # Avoid division by zero for constant signals
    normalized_signal = (signal - mean) / std
    return normalized_signal, mean, std

def unnormalize_signal(signal, mean, std):
    """Unnormalize the signal to its original scale."""
    return signal * std + mean

def correlate(shoulder_chest_angles_mm, shoulder_chest_angles_kinect, elbow_angles_mm, elbow_angles_kinect, corrAngle):
    # Normalize the signals
    shoulder_chest_angles_mm, mm_mean, mm_std = normalize_signal(shoulder_chest_angles_mm)
    shoulder_chest_angles_kinect, kinect_mean, kinect_std = normalize_signal(shoulder_chest_angles_kinect)
    elbow_angles_mm, elbow_mm_mean, elbow_mm_std = normalize_signal(elbow_angles_mm)
    elbow_angles_kinect, elbow_kinect_mean, elbow_kinect_std = normalize_signal(elbow_angles_kinect)

    # Determine the common length after trimming
    common_len = min(len(shoulder_chest_angles_mm), len(shoulder_chest_angles_kinect),
                     len(elbow_angles_mm), len(elbow_angles_kinect))

    if corrAngle == '1':
        correlationangle = 1
        cross_corr = np.correlate(
            shoulder_chest_angles_mm[:common_len, correlationangle],
            shoulder_chest_angles_kinect[:common_len, correlationangle],
            mode="full"
        )
    else:
        correlationangle = 0
        cross_corr = np.correlate(
            elbow_angles_mm[:common_len, correlationangle],
            elbow_angles_kinect[:common_len, correlationangle],
            mode="full"
        )

    max_corr_idx = np.argmax(cross_corr)
    offset = max_corr_idx - common_len + 1

    if offset < 0:
        shoulder_chest_angles_kinect_shifted = shoulder_chest_angles_kinect[abs(offset):abs(offset) + common_len]
        elbow_angles_kinect_shifted = elbow_angles_kinect[abs(offset):abs(offset) + common_len]
        shoulder_chest_angles_mm_shifted = shoulder_chest_angles_mm[:len(shoulder_chest_angles_kinect_shifted)]
        elbow_angles_mm_shifted = elbow_angles_mm[:len(elbow_angles_kinect_shifted)]
    else:
        shoulder_chest_angles_mm_shifted = shoulder_chest_angles_mm[offset:offset + common_len]
        elbow_angles_mm_shifted = elbow_angles_mm[offset:offset + common_len]
        shoulder_chest_angles_kinect_shifted = shoulder_chest_angles_kinect[:len(shoulder_chest_angles_mm_shifted)]
        elbow_angles_kinect_shifted = elbow_angles_kinect[:len(elbow_angles_mm_shifted)]

    min_len = min(len(shoulder_chest_angles_mm_shifted), len(shoulder_chest_angles_kinect_shifted))
    shoulder_chest_angles_mm_shifted = shoulder_chest_angles_mm_shifted[:min_len]
    shoulder_chest_angles_kinect_shifted = shoulder_chest_angles_kinect_shifted[:min_len]
    elbow_angles_mm_shifted = elbow_angles_mm_shifted[:min_len]
    elbow_angles_kinect_shifted = elbow_angles_kinect_shifted[:min_len]

    # Unnormalize the signals to their original scale
    shoulder_chest_angles_mm_shifted = unnormalize_signal(shoulder_chest_angles_mm_shifted, mm_mean, mm_std)
    shoulder_chest_angles_kinect_shifted = unnormalize_signal(shoulder_chest_angles_kinect_shifted, kinect_mean, kinect_std)
    elbow_angles_mm_shifted = unnormalize_signal(elbow_angles_mm_shifted, elbow_mm_mean, elbow_mm_std)
    elbow_angles_kinect_shifted = unnormalize_signal(elbow_angles_kinect_shifted, elbow_kinect_mean, elbow_kinect_std)

    time_mm = np.arange(len(shoulder_chest_angles_mm_shifted))
    time_kinect = np.arange(len(shoulder_chest_angles_kinect_shifted))

    return time_mm, time_kinect, shoulder_chest_angles_mm_shifted, shoulder_chest_angles_kinect_shifted, elbow_angles_mm_shifted, elbow_angles_kinect_shifted




def quickRMSE(shoulder_chest_angles_mm_shifted,shoulder_chest_angles_kinect_shifted,elbow_angles_mm_shifted,elbow_angles_kinect_shifted):
    # calculate quick rmses

    mm_data = shoulder_chest_angles_mm_shifted[:, 0]
    kinect_data = shoulder_chest_angles_kinect_shifted[:, 0]

    # Calculating the squared differences
    squared_diff = (mm_data - kinect_data) ** 2

    # Calculating the mean of squared differences
    mean_squared_diff = np.mean(squared_diff)

    # Calculating the square root of mean squared differences
    rmse = np.sqrt(mean_squared_diff)

    print("plane of elevation RMSE:", rmse)

    mm_data = shoulder_chest_angles_mm_shifted[:, 1]
    kinect_data = shoulder_chest_angles_kinect_shifted[:, 1]

    # Calculating the squared differences
    squared_diff = (mm_data - kinect_data) ** 2

    # Calculating the mean of squared differences
    mean_squared_diff = np.mean(squared_diff)

    # Calculating the square root of mean squared differences
    rmse = np.sqrt(mean_squared_diff)

    print("angle of elevation RMSE:", rmse)

    mm_data = elbow_angles_mm_shifted[:, 0]
    kinect_data = elbow_angles_kinect_shifted[:, 0]

    # Calculating the squared differences
    squared_diff = (mm_data - kinect_data) ** 2

    # Calculating the mean of squared differences
    mean_squared_diff = np.mean(squared_diff)

    # Calculating the square root of mean squared differences
    rmse = np.sqrt(mean_squared_diff)

    print("elbow flexion RMSE:", rmse)

def savetocsv(shoulder_chest_angles_kinect_shifted, elbow_angles_kinect_shifted,shoulder_chest_angles_mm_shifted,elbow_angles_mm_shifted, movement, viewpoint, participant,trial):
    # CAN SAVE XML FILE FROM SCALING
    # USED AVERAGE FOR UNKNOWN KINECT SCALING PARTS
    # DO NOT KNOW THE ONES FOR MM FULLY SO NEED TO UNDERSTAND THOSE

    # Prepare kinect data for saving
    shoulder_data = shoulder_chest_angles_kinect_shifted[:, [0, 1, 2]]
    elbow_data = elbow_angles_kinect_shifted[:, [0, 1, 2]]

    data = np.hstack((shoulder_data, elbow_data))
    # Define the column names
    column_names = ['Plane of Elevation', 'Angle of Elevation', 'Rotation', 'Flexion Extension', 'Pronation Supination',
                    'Carrying Angle']

    # Construct directory path based on participant, device, and viewpoint
    directory_path = os.path.join('results', trial, participant, viewpoint)

    # Create the directory if it doesn't exist
    os.makedirs(directory_path, exist_ok=True)

    # Construct the CSV file path with the name of the movement variable
    csv_file = os.path.join(directory_path, f'{participant}_{viewpoint}_{movement}_angles_kinect_processed.csv')

    # Write the data to the CSV file with column names
    with open(csv_file, 'w', newline='') as file:
        writer = csv.writer(file)

        # Write the column names as the first row
        writer.writerow(column_names)

        # Write the data rows
        writer.writerows(data)


    # Prepare kinect data for saving
    shoulder_data = shoulder_chest_angles_mm_shifted[:, [0, 1, 2]]
    elbow_data = elbow_angles_mm_shifted[:, [0, 1, 2]]

    data = np.hstack((shoulder_data, elbow_data))
    # Define the column names
    column_names = ['Plane of Elevation', 'Angle of Elevation', 'Rotation', 'Flexion Extension', 'Pronation Supination',
                    'Carrying Angle']

    # Construct the CSV file path with the name of the movement variable
    csv_file = os.path.join(directory_path, f'{participant}_{viewpoint}_{movement}_angles_mm_processed.csv')

    # Write the data to the CSV file with column names
    with open(csv_file, 'w', newline='') as file:
        writer = csv.writer(file)

        # Write the column names as the first row
        writer.writerow(column_names)

        # Write the data rows
        writer.writerows(data)


def main():
    csv_file = 'similar_files_all_with_exclusions_num8.csv'
    trial = '8'

    # Define the file path
    trial_path = 'trial_number.txt'


    # Step 1: Create the file and write the trial number
    with open(trial_path, 'w') as file:
        file.write(str(trial))

    sys.exit()
    # Read rows from the original CSV file
    with open(csv_file, mode='r+', newline='') as file:
        reader = csv.DictReader(file)
        rows = list(reader)

        # Process rows and update the 'StartK' and 'StopK' values
        for row in rows:
            kinect_file = row['Kinect File']
            mm_file = row['MM File']
            start_k = row['StartK']
            stop_k = row['StopK']
            corrAngle = row['CorrAngle']
            process = row['Process']

            # Extract filename without extension
            filename_without_extension = os.path.splitext(os.path.basename(kinect_file))[0]

            # Extract movement, viewpoint, device, and participant
            movement = filename_without_extension
            viewpoint = kinect_file.split(os.path.sep)[-2]  # Get the second-to-last part of the path
            # device = kinect_file.split(os.path.sep)[-3]  # Get the third-to-last part of the path
            participant = kinect_file.split(os.path.sep)[-4]  # Get the fourth-to-last part of the path


            print("filename:", kinect_file)
            print("Movement:", movement)
            print("Viewpoint:", viewpoint)
            # print("Device:", device)
            print("Participant:", participant)



            if process == '1':

                print(f"Processing pair: Kinect file: {kinect_file}, MM file: {mm_file}")
                kinect = True
                # Process Kinect file
                shoulder_chest_angles_kinect, elbow_angles_kinect, start, stop = collect(kinect_file, kinect, start_k,
                                                                                         stop_k,trial)

                # Update start and stop values in the row
                row['StartK'] = start
                row['StopK'] = stop
                row['Process'] = 0

                # Move file pointer to the beginning
                file.seek(0)

                # Write the updated rows back to the CSV file
                writer = csv.DictWriter(file, fieldnames=reader.fieldnames)
                writer.writeheader()
                writer.writerows(rows)

                # Truncate the file if the new content is shorter
                file.truncate()

                # Process MM file
                kinect = False
                shoulder_chest_angles_mm, elbow_angles_mm, start,stop = collect(mm_file,kinect,start_k,stop_k,trial)



                # Cross correlate to sync motion monitor and kinect angle data
                time_mm, time_kinect, shoulder_chest_angles_mm_shifted, shoulder_chest_angles_kinect_shifted, elbow_angles_mm_shifted, elbow_angles_kinect_shifted = correlate(shoulder_chest_angles_mm, shoulder_chest_angles_kinect, elbow_angles_mm, elbow_angles_kinect, corrAngle)
                print("Cross correlation done")

                # Plot synced angle plots for each euler and save plot png to charts
                plotSyncedAngles(time_mm, time_kinect, shoulder_chest_angles_mm_shifted, shoulder_chest_angles_kinect_shifted, elbow_angles_mm_shifted, elbow_angles_kinect_shifted, movement, viewpoint, participant,trial)

                # Calculate RMSE between kinect and motion monitor angles that will be output to console
                quickRMSE(shoulder_chest_angles_mm_shifted, shoulder_chest_angles_kinect_shifted, elbow_angles_mm_shifted, elbow_angles_kinect_shifted)

                # Save angles to csv files located in current dir, use this to compare with quaternion angle data
                savetocsv(shoulder_chest_angles_kinect_shifted, elbow_angles_kinect_shifted, shoulder_chest_angles_mm_shifted, elbow_angles_mm_shifted, movement, viewpoint, participant,trial)

                # sys.exit()
            else:
                print(f'skip processing {kinect_file} and {mm_file}')


    print("Done processing")




print("done processing")

if __name__ == "__main__":
    main()