clear all
close all

files = dir

csvfiles = dir 
for i = 1:size(csvfiles,1)
    
    % check for start sequence
    temp = strfind(csvfiles(i,1).name,'trajectory_')
    if (1 ~= length(temp) || temp(1) ~= 1)
        continue;
    end
    
    filename = csvfiles(i,1).name;
    
    filename
    
    

    M = csvread(filename);

    sampletime = 0.005
    t_max = size(M,1) * sampletime;

    t = linspace(0,t_max,size(M,1));

    [~,M_d] = gradient( M,0.005);
    [~,M_dd] = gradient( M_d,0.005);

    % create new figure per csv
    f = figure();
    set(f,'name',filename,'numbertitle','off')
    
    % plot the angles
    subplot(3,1,1);
    plot (t,M);
    title('joint movement');
    ylabel('s in [ degree ]');
    xlabel('time in [ seconds ]');
    
    % plot the velocity
    subplot(3,1,2);
    plot (t,M_d);
    title('joint velocity ');
    ylabel('v in [ degree / seconds ]');
    xlabel('time in [ seconds ]');
    
    % plot the acceleration
    subplot(3,1,3);
    plot (t,M_dd);
    title('joint acceleration');
    ylabel('a in [ degree / seconds^2 ]');
    xlabel('time in [ seconds ]');
end