clear
close all
clc

folder_path = './samples/sample4/joint_states_logs';
files = dir(fullfile(folder_path, '*.csv'));

folder_path_trajectroy = './samples/sample4/joint_trajectories';
files_trajectroy = dir(fullfile(folder_path_trajectroy, '*.csv'));

l = 2.5;
u = 3.5;

for k = 1:length(files)
    figure(k)
    grid on
    file_name = files(k).name;
    file_path = fullfile(folder_path, file_name);
    % Example: Read the file
    state = readtable(file_path);
    if k == 1
        offset = state.timestamp(1);
    end
    for i = 1:length(state.timestamp)
        state.timestamp(i)=state.timestamp(i)-offset;
    end
    hold on
    
    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.(file_name(1:end-4)), 'Color', [0 0 0])
        dy = diff(T.(file_name(1:end-4))) ./ diff(T.timestamp);
        x_mid = T.timestamp(1:end-1) + diff(T.timestamp)/2;  % midpoints between x-values
        plot(x_mid,dy,'Color',[1 0 0])
    end

end