clear
close all
clc

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

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

l = 0;
u = 60;

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
    plot(state.timestamp(state.timestamp >=l & state.timestamp <=u), state.position(state.timestamp >=l & state.timestamp <=u), 'Color',[1 0 0], 'DisplayName', 'Position','LineWidth',4);
    %plot(state.timestamp(state.timestamp >=l & state.timestamp <=u), state.velocity(state.timestamp >=l & state.timestamp <=u), 'Color',[0 1 0], 'DisplayName','Velocity')
    %plot(state.timestamp(state.timestamp >=l & state.timestamp <=u), state.effort(state.timestamp >=l & state.timestamp <=u), 'Color',[0 0 1], 'DisplayName','Effort')
    title(replace(file_name(1:end-4),'_',' '), 'FontSize',16)
    xlabel('Time [s]', 'FontSize',30)
    ylabel('Joint Angle [rad]', '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
        if s == 1
            plot(T.timestamp(T.timestamp >=l & T.timestamp <=u), T.(file_name(1:end-4))(T.timestamp >=l & T.timestamp <=u), 'Color', [0 0 0], 'DisplayName','Trajectories', 'LineWidth',2);
        else
            plot(T.timestamp(T.timestamp >=l & T.timestamp <=u), T.(file_name(1:end-4))(T.timestamp >=l & T.timestamp <=u), 'Color', [0 0 0], 'HandleVisibility','off', 'LineWidth',2);
    
        end
    end
    plot(T.timestamp(T.timestamp >=l & T.timestamp <=u), T.(file_name(1:end-4))(T.timestamp >=l & T.timestamp <=u), 'Color', [0 0 0], 'DisplayName','Trajectories', 'LineWidth',2);
end