%% Read file using function
clear all;close all;clc

warning('on','all');

file_path = '/home/shared/essua/2014_06_17_uncompressed/jorge almeida/jorge almeida/jorge almeida/jorge almeida02.c3d';

[header, groups, data] = c3dread(file_path);

for g=1:length(groups)    
    for p=1:length(groups{g}.parameters)
        eval(sprintf('%s.%s = groups{%d}.parameters{%d}.data;',groups{g}.name,groups{g}.parameters{p}.name,g,p))
    end
end

clear g p;

% Copy data to usefull variables

n_labels = POINT.USED;
labels = POINT.LABELS;
n_analog_channels = ANALOG.USED;
n_analog_samples_per_frame = header.number_analog_samples_per_frame;
n_frames = size(data,1);

data_backup = data;
time = data(:,1);
data(:,1) = [];
points = data(:,1:n_labels*4);
data(:,1:n_labels*4) = [];

%Convert to meters
if strcmp(POINT.UNITS,'mm')
    x = points(:,1:4:end)/1000;
    y = points(:,2:4:end)/1000;
    z = points(:,3:4:end)/1000;
    fl = points(:,4:4:end);
    
    %-1 marks an invalid measurement, the rest of the values correspond to
    %measuremnt residual and camera coverage, check the C3D doc for more
    %information
    
    x(fl==-1)=NaN;
    y(fl==-1)=NaN;
    z(fl==-1)=NaN;
else
    warning(['Unexpected measurement units: ' POINT.UNITS])
end

if n_analog_channels

    analog = data;
    analog_channels = zeros(n_analog_channels,n_frames,n_analog_samples_per_frame);
    analog_time = linspace(time(1),time(end),n_frames*n_analog_samples_per_frame);

    for c=1:n_analog_channels
        readings = analog(:,c:n_analog_channels:end);
        analog_channels(c,:,:) = (readings - ANALOG.OFFSET(c))*ANALOG.GAIN(c)*ANALOG.GEN_SCALE;
    end
end

trial_length = size(x,1);

data = data_backup;
clear data_backup

% Create the basic labels

global tags t 

tags = {'t.C7'};
tags{end+1} = 't.L1';
tags{end+1} = 't.L2';
tags{end+1} = 't.L3';
tags{end+1} = 't.L4';
tags{end+1} = 't.L5';
tags{end+1} = 't.L6';
tags{end+1} = 't.L7';
tags{end+1} = 't.L8';
tags{end+1} = 't.L9';
tags{end+1} = 't.L10';
tags{end+1} = 't.L11';
tags{end+1} = 't.L12';
tags{end+1} = 't.L13';
tags{end+1} = 't.L14';
tags{end+1} = 't.L15';
tags{end+1} = 't.L16';
tags{end+1} = 't.L17';
tags{end+1} = 't.L18';
tags{end+1} = 't.L19';
tags{end+1} = 't.L20';
tags{end+1} = 't.L21';
tags{end+1} = 't.L22';
tags{end+1} = 't.L23';
tags{end+1} = 't.L24';
tags{end+1} = 't.L25';
tags{end+1} = 't.L26';
tags{end+1} = 't.L27';
tags{end+1} = 't.L26';
tags{end+1} = 't.L29';
tags{end+1} = 't.L30';
tags{end+1} = 't.L31';
tags{end+1} = 't.L32';
tags{end+1} = 't.L33';
tags{end+1} = 't.L34';
tags{end+1} = 't.L35';

%Create variables with the tags names
    
for i=1:length(tags)
    eval(sprintf('%s=[x(:,%d) y(:,%d) z(:,%d)];',tags{i},i,i,i))
    eval(sprintf('%s=%s'';',tags{i},tags{i}))
end

global connections

connections = {'t.rfhd','t.lfhd'};
connections = [connections; {'t.rsho','t.relb'}];
connections = [connections; {'t.relb','t.rwra'}];
connections = [connections; {'t.rwra','t.rfin'}];
connections = [connections; {'t.rasi','t.lasi'}];
connections = [connections; {'t.lsho','t.lelb'}];
connections = [connections; {'t.lelb','t.lwra'}];
connections = [connections; {'t.lwra','t.lfin'}];
connections = [connections; {'t.rasi','t.rkne'}];
connections = [connections; {'t.rasi','t.rknei'}];
connections = [connections; {'t.rkne','t.rknei'}];
connections = [connections; {'t.lasi','t.lknei'}];
connections = [connections; {'t.lasi','t.lkne'}];
connections = [connections; {'t.lkne','t.lknei'}];
connections = [connections; {'t.lwrb','t.lfin'}];
connections = [connections; {'t.lwra','t.lwrb'}];
connections = [connections; {'t.lelb','t.lwrb'}];
connections = [connections; {'t.rfhd','t.rbhd'}];
connections = [connections; {'t.lfhd','t.lbhd'}];
connections = [connections; {'t.rbhd','t.lbhd'}];
connections = [connections; {'t.ltoe','t.lanki'}];
connections = [connections; {'t.ltoe','t.lank'}];
connections = [connections; {'t.lanki','t.lank'}];
connections = [connections; {'t.rtoe','t.rank'}];
connections = [connections; {'t.rtoe','t.ranki'}];
connections = [connections; {'t.rank','t.ranki'}];
connections = [connections; {'t.lpsi','t.lasi'}];
connections = [connections; {'t.rsho','t.C7'}];
connections = [connections; {'t.C7','t.lsho'}];
connections = [connections; {'t.rpsi','t.lpsi'}];
connections = [connections; {'t.rpsi','t.rasi'}];
connections = [connections; {'t.rsho','t.rasi'}];
connections = [connections; {'t.rsho','t.rpsi'}];
connections = [connections; {'t.lsho','t.lpsi'}];
connections = [connections; {'t.lsho','t.lasi'}];
connections = [connections; {'t.rwrb','t.rwra'}];
connections = [connections; {'t.rwrb','t.rfin'}];
connections = [connections; {'t.relb','t.rwrb'}];
connections = [connections; {'t.rank','t.rhee'}];
connections = [connections; {'t.rhee','t.ranki'}];
connections = [connections; {'t.lanki','t.lhee'}];
connections = [connections; {'t.lhee','t.lank'}];
connections = [connections; {'t.rkne','t.rank'}];
connections = [connections; {'t.rknei','t.ranki'}];
connections = [connections; {'t.lkne','t.lank'}];
connections = [connections; {'t.lknei','t.lanki'}];
connections = [connections; {'t.lpsi','t.lkne'}];
connections = [connections; {'t.lpsi','t.lknei'}];
connections = [connections; {'t.rpsi','t.rkne'}];
connections = [connections; {'t.rpsi','t.rknei'}];

%% Create an auto handle function to detect selections on the 3d points
% clear all;close all;clc

global data_file
data_file = 'processed_labels';

% load(data_file)

close all

figure('color',[0.3 0.3 0.3],'position',[80 20 900 900])
set(gca,'Color',[0.3 0.3 0.3]);
hold on

cx = 0;
cy = 2;
cz = 1;

cube_width = 6.5;

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 equal
axis vis3d
grid on
view(3)

cube_width = 6.0;

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(3,:) = [0 0 0 0];

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

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

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


global k h colors

h = [];

colors = [21, 72, 144
          21, 72, 144
          255, 0, 0
          255, 0, 0
          255, 102, 0
          255, 255, 255];
      
colors = colors/255;
k=1;

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

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,colors);
    
set(gcf,'WindowButtonDownFcn',{@callbackPointSelection});
set(gcf,'WindowKeyPressFcn',{@callbackKeyPressed});

% invalid = isnan(person);
% invalid = invalid(1,:);

% p = person(:,~invalid);
% mean_pos = mean(p,2);

% camtarget(mean_pos)
    


%% Plot the data to check if it is ok
close all

%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 = 2;
cz = 1;

cube_width = 6.5;

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 equal
axis vis3d
grid on
view(3)

cube_width = 6.0;

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)')


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

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

colors = [21, 72, 144
          21, 72, 144
          255, 102, 0]/255;
      
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,colors);
    
    invalid = isnan(person);
    invalid = invalid(1,:);
    
    p = person(:,~invalid);
    mean_pos = mean(p,2);
    
    camtarget(mean_pos)
    
    span = toc(start);
    start = tic;
    freq = 1/span;
    
    jump = round(100/freq);
    k=k+jump;
    
    hl(1) = text(cx,cy,3.2,sprintf('Vicon freq: %.2f',100));
    hl(2) = text(cx,cy,3,sprintf('Plot freq: %.2f jumping: %d',1/span,jump));
    hl(3) = text(cx,cy,3.400,sprintf('Frame: %d',k));
    
%     pause(0.001)
    pause
%     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');

%% Follow a label to mark it

close all

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

cx = 0;
cy = 2;
cz = 1;

cube_width = 6.5;

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])

cube_width = 6.0;

axis vis3d
grid on
view(3)

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)')

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

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

trial_length = size(x,1);

k=1;

is_corrected = false;
tag_id = 2;

% labeling = {};

% for i=1:trial_length
%     for l=1:size(labeling,2)
%         
%         
%         
%         
%         
%     end
% end

correct_tag = '';

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);
    
    eval(sprintf('ref_point = %s(:,%d);',char(tags{tag_id}),k))

    if ~isnan(ref_point(1)) && is_corrected == false
        %Point is detected but not tagged
        fprintf('Iteration: %d\n',k);
        correct_tag = input('What is correct label? ','s');
        labeling{k,tag_id} = correct_tag;
        is_corrected = true;
    elseif ~isnan(ref_point(1)) && is_corrected == true
        %Point is tagged and detected
        labeling{k,tag_id} = correct_tag;
        fprintf('Point tagged at iteration: %d\n',k);
    elseif isnan(ref_point(1))
        %Point lost
        fprintf('Point lost at iteration: %d\n',k);
        is_corrected = false;
    end
    
    invalid = isnan(person);
    invalid = invalid(1,:);
    
    p = person(:,~invalid);
    mean_pos = mean(p,2);
    
    camtarget(mean_pos)
    
    span = toc(start);
    start = tic;
    freq = 1/span;
    
    jump = round(100/freq);
%     k=k+jump;
    k=k+1;
    
    hl(1) = text(cx,cy,3.2,sprintf('Vicon freq: %.2f',100));
    hl(2) = text(cx,cy,3,sprintf('Plot freq: %.2f jumping: %d',1/span,jump));
    hl(3) = text(cx,cy,3.400,sprintf('Frame: %d',k)); 
    
    pause(0.001)
end

%% Work in main poses, high visibility

high_visibility_iterations = [396 1270 2500 3400 4700 5700 7200 7800 9100 9600 10100 10600];

% Get labeling at high_visibility_iterations

close all

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

cx = 0;
cy = 2;
cz = 1;

cube_width = 6.5;

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])

cube_width = 6.0;

axis vis3d
grid on
view(3)

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 (m)')
ylabel('y (m)')
zlabel('z (m)')

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

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

trial_length = size(x,1);

k=1;

is_corrected = false;
tag_id = 2;

correct_tag = '';

for k = high_visibility_iterations 
    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);
        
    invalid = isnan(person);
    invalid = invalid(1,:);
    
    p = person(:,~invalid);
    mean_pos = mean(p,2);
    
    camtarget(mean_pos)
    
    span = toc(start);
    start = tic;
    freq = 1/span;
    
    jump = round(100/freq);
%     k=k+jump;
%     k=k+1;
    
    hl(1) = text(cx,cy,3.2,sprintf('Vicon freq: %.2f',100));
    hl(2) = text(cx,cy,3,sprintf('Plot freq: %.2f jumping: %d',1/span,jump));
    hl(3) = text(cx,cy,3.400,sprintf('Frame: %d',k)); 
    
    pause
%     pause(0.001)
end

%% Replacements

replacements_all = {};

% Replacements for k=396

replacements = {'L15','rtoe'};
replacements = [replacements; {'L20','rank'}];
replacements = [replacements; {'L30','rhee'}];
replacements = [replacements; {'L11','ranki'}];

replacements = [replacements; {'L10','ltoe'}];
replacements = [replacements; {'L23','lank'}];
replacements = [replacements; {'L31','lhee'}];
replacements = [replacements; {'L25','lanki'}];

replacements = [replacements; {'L1','rkne'}];
replacements = [replacements; {'L12','rknei'}];

replacements = [replacements; {'L13','lkne'}];
replacements = [replacements; {'L5','lknei'}];

replacements = [replacements; {'L19','rfhd'}];
replacements = [replacements; {'L24','lfhd'}];
replacements = [replacements; {'L22','rbhd'}];
replacements = [replacements; {'L26','lbhd'}];

replacements = [replacements; {'L8','rsho'}];
replacements = [replacements; {'L9','lsho'}];

replacements = [replacements; {'L18','relb'}];
replacements = [replacements; {'L16','lelb'}];

replacements = [replacements; {'L4','rasi'}];
replacements = [replacements; {'L7','lasi'}];
replacements = [replacements; {'L27','rpsi'}];
replacements = [replacements; {'L21','lpsi'}];

replacements = [replacements; {'L6','rfin'}];
replacements = [replacements; {'L17','rwra'}];
replacements = [replacements; {'L29','rwrb'}];

replacements = [replacements; {'L2','lfin'}];
replacements = [replacements; {'L3','lwra'}];
replacements = [replacements; {'L14','lwrb'}];

replacements_all = [replacements_all; {replacements}];

% Replacements for k=1270

replacements = {'L13','rtoe'};
replacements = [replacements; {'L27','rank'}];
replacements = [replacements; {'L30','rhee'}];
replacements = [replacements; {'L19','ranki'}];

replacements = [replacements; {'L17','ltoe'}];
replacements = [replacements; {'L24','lank'}];
replacements = [replacements; {'L25','lhee'}];
replacements = [replacements; {'L21','lanki'}];

replacements = [replacements; {'L6','rkne'}];
replacements = [replacements; {'L3','rknei'}];

replacements = [replacements; {'L8','lkne'}];
replacements = [replacements; {'L14','lknei'}];

replacements = [replacements; {'L12','rfhd'}];
replacements = [replacements; {'L11','lfhd'}];
replacements = [replacements; {'L22','rbhd'}];
replacements = [replacements; {'L33','lbhd'}];

replacements = [replacements; {'L10','rsho'}];
replacements = [replacements; {'L1','lsho'}];

replacements = [replacements; {'L4','relb'}];
replacements = [replacements; {'L18','lelb'}];

replacements = [replacements; {'L9','rasi'}];
replacements = [replacements; {'L7','lasi'}];
replacements = [replacements; {'L29','rpsi'}];
replacements = [replacements; {'L26','lpsi'}];

replacements = [replacements; {'L2','rfin'}];
replacements = [replacements; {'L5','rwra'}];
replacements = [replacements; {'L23','rwrb'}];

replacements = [replacements; {'L16','lfin'}];
replacements = [replacements; {'L20','lwra'}];
replacements = [replacements; {'L15','lwrb'}];

replacements_all = [replacements_all; {replacements}];

% Replacements for k=2500

replacements = {'L20','rtoe'};
replacements = [replacements; {'L18','rank'}];
replacements = [replacements; {'L29','rhee'}];
replacements = [replacements; {'L21','ranki'}];

replacements = [replacements; {'L4','ltoe'}];
replacements = [replacements; {'L9','lank'}];
replacements = [replacements; {'L26','lhee'}];
replacements = [replacements; {'L19','lanki'}];

replacements = [replacements; {'L22','rkne'}];
replacements = [replacements; {'L5','rknei'}];

replacements = [replacements; {'L3','lkne'}];
replacements = [replacements; {'L27','lknei'}];

replacements = [replacements; {'L14','rfhd'}];
replacements = [replacements; {'L11','lfhd'}];
replacements = [replacements; {'L25','rbhd'}];
replacements = [replacements; {'L23','lbhd'}];

replacements = [replacements; {'L7','rsho'}];
replacements = [replacements; {'L1','lsho'}];

replacements = [replacements; {'L17','relb'}];
replacements = [replacements; {'L8','lelb'}];

replacements = [replacements; {'L10','rasi'}];
replacements = [replacements; {'L2','lasi'}];
replacements = [replacements; {'L32','rpsi'}];
replacements = [replacements; {'L30','lpsi'}];

replacements = [replacements; {'L13','rfin'}];
replacements = [replacements; {'L10','rwra'}];
replacements = [replacements; {'L31','rwrb'}];

replacements = [replacements; {'L16','lfin'}];
replacements = [replacements; {'L6','lwra'}];
replacements = [replacements; {'L15','lwrb'}];

replacements_all = [replacements_all; {replacements}];

%% Create the new tags
replacements = replacements_all{1};

for t=1:size(replacements,1)
    eval(sprintf('%s = nan(3,%d);',char(replacements{t,2}),trial_length));
end

for ra=1:size(replacements_all,1)
    %Apply replacements
    
    k = high_visibility_iterations(ra);
    replacements = replacements_all{ra};

    back_iterations_mask = ones(1,size(replacements,1));

    for ki=k:-1:1
        fprintf('ki: %d\n',ki);
        for r=1:size(replacements,1)
            eval(sprintf('original = %s(:,%d);',replacements{r,1},ki))

            is_valid = ~isnan(original(1));

            if is_valid && back_iterations_mask(r)
                %The point is valid and the mask is also valid
                eval(sprintf('%s(:,%d) = original;',replacements{r,2},ki))
            elseif ~is_valid && back_iterations_mask(r)
                back_iterations_mask(r) = 0;
            elseif back_iterations_mask(r)==0
                %Mask is invalid
            end
        end

        if sum(back_iterations_mask)==0
            fprintf('All labels lost at iteration: %d\n',ki);
            break
        end
    end

    front_iterations_mask = ones(1,size(replacements,1));

    for ki=k:1:trial_length
        fprintf('ki: %d\n',ki);

        for r=1:size(replacements,1)
            eval(sprintf('original = %s(:,%d);',replacements{r,1},ki))

            is_valid = ~isnan(original(1));

            if is_valid && front_iterations_mask(r)
                %The point is valid and the mask is also valid
                eval(sprintf('%s(:,%d) = original;',replacements{r,2},ki))
            elseif ~is_valid && front_iterations_mask(r)
                front_iterations_mask(r) = 0;
            elseif front_iterations_mask(r)==0
                %Mask is invalid
            end
        end

        if sum(front_iterations_mask)==0
            fprintf('All labels lost at iteration: %d\n',ki);
            break
        end
    end
end
%%

%% New tags

replacements = replacements_all{1};

n_tags={};
for r=1:size(replacements,1)
    n_tags{r} = replacements{r,2};
end

if size(n_tags,1)~=size(unique(n_tags),1)
    error('There is a repeated tag, check the replacements variable')
end

%% Define the connections

connections = {'rsho','lsho'};

connections = [connections; {'rfhd','lfhd'}];
connections = [connections; {'rfhd','rbhd'}];
connections = [connections; {'lfhd','lbhd'}];
connections = [connections; {'rbhd','lbhd'}];

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

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

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

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

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

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

connections = [connections; {'rtoe','ranki'}];
connections = [connections; {'ranki','rhee'}];
connections = [connections; {'rhee','rank'}];
connections = [connections; {'rank','rtoe'}];
connections = [connections; {'rank','ranki'}];

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

connections = [connections; {'rkne','rank'}];
connections = [connections; {'rknei','ranki'}];

connections = [connections; {'lkne','lank'}];
connections = [connections; {'lknei','lanki'}];

%% Plot the new tags
close all

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

cx = 0;
cy = 2;
cz = 1;

cube_width = 6.5;

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 vis3d
grid on
view(3)

cube_width = 6.0;

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)')

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

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

trial_length = size(x,1);

colors = [21, 72, 144
          21, 72, 144
          255, 102, 0]/255;
colors2 =[255, 0, 0
          255, 0, 0
          255, 102, 0]/255;
k=1;
while k<trial_length
    delete(hl)
    
    for i=1:length(n_tags)
        eval(sprintf('person(1,%d) = %s(1,%d);',i,char(n_tags{i}),k));
        eval(sprintf('person(2,%d) = %s(2,%d);',i,char(n_tags{i}),k));
        eval(sprintf('person(3,%d) = %s(3,%d);',i,char(n_tags{i}),k));
    end
    
    for i=1:length(tags)
        eval(sprintf('person_o(1,%d) = %s(1,%d);',i,char(tags{i}),k));
        eval(sprintf('person_o(2,%d) = %s(2,%d);',i,char(tags{i}),k));
        eval(sprintf('person_o(3,%d) = %s(3,%d);',i,char(tags{i}),k));
    end
    
    h2 = plotPerson(person_o,connections,tags,h2,colors2);
    h = plotPerson(person,connections,n_tags,h,colors);
    
    invalid = isnan(person);
    invalid = invalid(1,:);
    
    p = person(:,~invalid);
    mean_pos = mean(p,2);
    
    camtarget(mean_pos)
    
    span = toc(start);
    start = tic;
    freq = 1/span;
    
    jump = round(100/freq);
%     k=k+1;
    k=k+jump;
    
    hl(1) = text(cx,cy,3.2,sprintf('Vicon freq: %.2f',100));
    hl(2) = text(cx,cy,3,sprintf('Plot freq: %.2f jumping: %d',1/span,jump));
    hl(3) = text(cx,cy,3.400,sprintf('Frame: %d',k));
    
%     pause
    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');
