
%% Read data for cesar

% clear all;close all;clc
% 
% csv_file = '/home/shared/essua/teste_tarde09/teste_tarde09_without_pressure_sensors.csv';
% 
% vicon = readViconCSV(csv_file);
% 
% save 'vicon_9.mat' vicon;

%% Read data file and write to matlab format
% clear all;close all;clc
% 
% csv_file = '/home/shared/essua/teste_tarde05/teste_tarde05.csv';
% 
% vicon = readViconCSV(csv_file);
% 
% save 'vicon_5.mat' vicon;

%% Read the matlab format data file

cd /home/shared/essua/matlab
clear all;close all;clc

load 'vicon_5.mat';

%% Plot the data points

close all;clc

%get the labels
x_index = find(~cellfun(@isempty,vicon.tags));
y_index = x_index + 1;
z_index = x_index + 2;

tags = char(vicon.tags);

cell_tags=[];
for i=1:size(tags,1)
    first_char = findstr(tags(i,:),':');
    if ~isempty(first_char)
        cell_tags = [cell_tags cellstr(lower(tags(i,first_char+1:end)))];
    end
end
tags = cell_tags;

% tags(:,1:length('Jose Rosado:')) = [];
% tags = lower(tags);
% tags = tags(x_index);
trial_length = length(vicon.data);

for i=1:length(x_index)
    eval(sprintf('%s=vicon.data(:,%d:%d)'';',char(tags{i}),x_index(i),z_index(i)))
end

%Figure related controls
% figure('color',[1 1 1],'position',[80 20 1280 720])
figure('color',[1 1 1],'position',[80 20 800 800])
hold on

cx = 0;
cy = 1000;
cz = 1000;

cube_width = 6500;

axis([cx-cube_width/2 cx+cube_width/2 cy-cube_width/2 cy+cube_width/2 cz-cube_width/2 cz+cube_width/2])
axis square
grid on
view(3)
% zoom(2)
% set(gca,'projection','perspective')
% set(gca,'projection','orthographic')
% view(-60,15)
% zoom(3)

% set(gca,'CameraTarget',[0 500 1000])
% set(gca,'CameraPosition',[-1500 -1500 1000])
% set(gca,'CameraViewAngle',30)

% ground_plane(1,:) = [cx-cube_width/2 cx+cube_width/2 cx+cube_width/2 cx-cube_width/2];
% ground_plane(2,:) = [cy-cube_width/2 cy-cube_width/2 cy+cube_width/2 cy+cube_width/2];

ground_plane(1,:) = [cx-cube_width cx+cube_width/2 cx+cube_width/2 cx-cube_width];
ground_plane(2,:) = [cy-cube_width cy-cube_width cy+cube_width/2 cy+cube_width/2];
ground_plane(3,:) = [0 0 0 0];

hp = fill3(ground_plane(1,:),ground_plane(2,:),ground_plane(3,:),'b');
set(hp,'facecolor',[0.8 0.8 0.8])

set(gcf,'renderer','opengl')

xlabel('x (mm)')
ylabel('y (mm)')
zlabel('z (mm)')

connections = {'rsho','lsho'};
connections = [connections; {'rsho','relb'}];
connections = [connections; {'relb','rwra'}];
connections = [connections; {'rwra','rfin'}];

connections = [connections; {'lsho','lelb'}];
connections = [connections; {'lelb','lwra'}];
connections = [connections; {'lwra','lfin'}];

connections = [connections; {'rsho','rasi'}];
connections = [connections; {'lsho','lasi'}];
connections = [connections; {'rsho','rpsi'}];
connections = [connections; {'lsho','lpsi'}];

connections = [connections; {'rasi','lasi'}];
connections = [connections; {'rpsi','lpsi'}];
connections = [connections; {'rasi','rpsi'}];
connections = [connections; {'lasi','lpsi'}];

connections = [connections; {'rasi','rkne'}];
connections = [connections; {'rpsi','rkne'}];

connections = [connections; {'lasi','lkne'}];
connections = [connections; {'lpsi','lkne'}];

connections = [connections; {'rkne','rhee'}];
connections = [connections; {'rtoe','rhee'}];

connections = [connections; {'lkne','lhee'}];
connections = [connections; {'lkne','lank'}];
connections = [connections; {'ltoe','lhee'}];
connections = [connections; {'lank','ltoe'}];
connections = [connections; {'lank','lhee'}];

connections = [connections; {'c1','c2'}];
connections = [connections; {'c2','c4'}];
connections = [connections; {'c4','c3'}];
connections = [connections; {'c3','c1'}];

h=[];
hl=[];
start = tic;

person=zeros(3,length(tags));

% writerObj = VideoWriter('RENAME_ME.avi');
% set(writerObj,'FrameRate',50);
% set(writerObj,'Quality',90);
% open(writerObj);

% jump = 2;

k=1;
while k<trial_length
    delete(hl)
    
    for i=1:length(tags)
        eval(sprintf('person(1,%d) = %s(1,%d);',i,char(tags{i}),k));
        eval(sprintf('person(2,%d) = %s(2,%d);',i,char(tags{i}),k));
        eval(sprintf('person(3,%d) = %s(3,%d);',i,char(tags{i}),k));
    end
    
    h = plotPerson(person,connections,tags,h);
    
    span = toc(start);
    start = tic;
    freq = 1/span;
    
    jump = round(100/freq);
    k=k+jump;
    
    hl(1) = text(cx,cy,3200,sprintf('Vicon freq: %.2f',100));
    hl(2) = text(cx,cy,3000,sprintf('Plot freq: %.2f jumping: %d',1/span,jump));
    hl(3) = text(cx,cy,3400,sprintf('Frame: %d',k));
    
    pause(0.001)
%     drawnow
%     frame = getframe();
%     writeVideo(writerObj,frame);
%     fprintf('Added: %d of %d (%.1f %%)\n',k,trial_length,(k/trial_length)*100)
    
end

% close(writerObj);
fprintf('Closed video file\n');
