clear
close all
clc

file = './samples/sample13/joint_states_cart_logs/cartesian.csv';

folder_path_trajectroy = './samples/sample13/joint_trajectories_cart';
files_trajectroy = dir(fullfile(folder_path_trajectroy, '*.csv'));

l = 0;
u = 60;
state = readtable(file);
offset = state.timestamp(1);
for i = 1:length(state.timestamp)
        state.timestamp(i)=state.timestamp(i)-offset;
end
val = ["x","y","z","roll", "pitch", "yaw"];
for k = 1:6
    figure(k)
    grid on
    hold on
    plot(state.timestamp(state.timestamp >=l & state.timestamp <=u), state.(val(k))(state.timestamp >=l & state.timestamp <=u), 'Color',[1 0 0],'LineWidth',4, 'DisplayName','Transform');
    title(val(k), 'FontSize',16)
    xlabel('Time [s]', 'FontSize',30)
    ylabel('Pos [m]', 'FontSize',30)
    legend('FontSize',30)
    legend show

    for s = 1:length(files_trajectroy)
        file_name_trajectroy = files_trajectroy(s).name;
        file_path_trajectroy = fullfile(folder_path_trajectroy, file_name_trajectroy);
        % Example: Read the file
        T = readtable(file_path_trajectroy);
        for i=1:length(T.timestamp)
            T.timestamp(i)=T.timestamp(i)-offset;
        end
        
        plot(T.timestamp(T.timestamp >=l & T.timestamp <=u), T.(val(k))(T.timestamp >=l & T.timestamp <=u), 'Color', [0 0 0], 'HandleVisibility','off', 'LineWidth',2);
    
    end
    plot(T.timestamp(T.timestamp >=l & T.timestamp <=u), T.(val(k))(T.timestamp >=l & T.timestamp <=u), 'Color', [0 0 0], 'DisplayName','Trajectories', 'LineWidth',2);

end