
%%%%%%%%%%%%%%%%%%%% points-data order %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%    Hips-1 LeftUpLeg-2 LeftLowLeg-3 LeftFoot-4 ltoes-5 RightUpLeg-6 RightLowLeg-7
%    RightFoot-8 rtoes-9 upperback-10 Chest-11 Neck-12 Head-13 UChest-14 LeftUpArm-15 LeftLowArm-16
%    LeftHand-17 lfingers-18 Chest-19 RightUpArm-20 RightLowArm-21 RightHand-22 rfingers-23 
%
%%%%%%%%%%%%%%%%%%%% frame data [993 frames]:  23 points (69 columns)  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear;
close all;

loaddata;           % Loads the data into a matrix x (993x69) (993 frames and 69 body positions)
[r,c]=size(x);

% PARAMETERS
% ===========
% from              --- The first frame to be plotted
% to                --- The last frame to be plotted
% interval          --- The increment of frame
% delay             --- Delay plotting (Used ONLY when reading input from user)
% kernelFunction    --- The kernelFunction to be use
% kernelPar         --- The kernelFunction's parameter
% error             --- Error between prediction and data
from = 1;
to = r;
interval = 10;
delay = 0.4;
kernelFunction = 'expK';
kernelPar = 1;
offset = 5; % this is used to plot both the true walk and the predicted- one offset from it.
testSize = 300;           % Number of test data.
trainSize = r - testSize; % Define number of train data.

% COMPUTE PCA
% ============
[U,S,V] = svd(x);
Uk = ???
Sk = ???
pc = ???

% FIT ZERO MEAN GAUSSIAN PROCESS MEAN 
% ===================================
% Make predictions both on the train and test datasets:
yPredTrain = ???
yPredTest  = ???
prediction = [yPredTrain;yPredTest];
prediction = prediction + offset;    % Move away from original figure for visualization.

% SETUP FIGURE PARAMETERS
% =========================
figure(1)
clf;
hold on;
set(gcf, 'MenuBar', 'none');
set(gcf, 'DoubleBuffer', 'on');
set(gca, 'Visible', 'off');

% PLOT PRINCIPAL COMPONENTS
% =========================
PCA = axes('position',[.1 .6 .8 .35]);
hold on;
axis([-5 40 -15 35]);
plot(pc(from:trainSize,1),pc(from:trainSize,2),'*b',pc(trainSize+1:to,1),pc(trainSize+1:to,2),'*g');  % Plotting all points [in between the specified range]
CURRENT = plot(pc(from,1), pc(from,2), 'ro');                   % Plot A red circle to indicate current frame in PCA Plot
currentText = text(25,32, ['Current Frame: ' num2str(from)] );  % Display current frame number
currentText2 = text(25,22,'Train data');  % Display when train/test data

% PLOT INITIAL BODIES:
% ====================
FIGURE = axes('position',[0 0 1 .6]);
axis([-6 14 -10 20 -5 5]);
view(-12,84);
set(FIGURE, 'Visible', 'off');
rotate3d(FIGURE, 'on');
hold on;

currFrame = x(from,:);       % Frame coordinates of all points (1x69)
y = reshape(currFrame,3,23); % converts (1x69) to (3,23) - i.e. 23 points in 3D.
currFrame2 = prediction(from,:);
y2 = reshape(currFrame2,3,23);  % These are the predictions.
body = plot3(y(1,:),y(2,:),y(3,:),'ro','linewidth',5);    % Plot of Data body points.
body2= plot3(y2(1,:),y2(2,:),y2(3,:),'yo','linewidth',5); % Plot of Prediction body points.

% PLOT ALL LINES THAT FORM PART OF THE BODIES
% =============================================
[rShoulder,lShoulder,lowBack,upBack,neck,rUpArm,rLowArm,rHand,lUpArm,lLowArm,lHand,lHip,lUpLeg,lLowLeg,lFoot,rHip,rUpLeg,rLowLeg,rFoot]=defineLines(y);
[rShoulder2,lShoulder2,lowBack2,upBack2,neck2,rUpArm2,rLowArm2,rHand2,lUpArm2,lLowArm2,lHand2,lHip2,lUpLeg2,lLowLeg2,lFoot2,rHip2,rUpLeg2,rLowLeg2,rFoot2]=defineLines(y2);

% REPEAT PLOTS FOR ALL FRAMES:
% ============================
for frame=from:interval:to          % Frame number to be plotted.
    currFrame = x(frame,:);         
    y = reshape(currFrame,3,23);    % Actual data
    currFrame2 = prediction(frame,:);
    y2 = reshape(currFrame2,3,23);  % Prediction
 
    set(body,'xdata',y(1,:),'ydata',y(2,:),'zdata',y(3,:));
    set(body2,'xdata',y2(1,:),'ydata',y2(2,:),'zdata',y2(3,:));
    
    % UPDATE BODY POSITION (ACTUAL DATA)
    % ==================================
    set(rShoulder, 'xdata',[y(1,12) y(1,20)], 'ydata',[y(2,12) y(2,20)], 'zdata',[y(3,12) y(3,20)]);
    set(lShoulder, 'xdata',[y(1,12) y(1,15)], 'ydata',[y(2,12) y(2,15)], 'zdata',[y(3,12) y(3,15)]);
    set(lowBack,   'xdata',[y(1,10) y(1,11)], 'ydata',[y(2,10) y(2,11)], 'zdata',[y(3,10) y(3,11)]);
    set(upBack,    'xdata',[y(1,11) y(1,12)], 'ydata',[y(2,11) y(2,12)], 'zdata',[y(3,11) y(3,12)]);
    set(neck,      'xdata',[y(1,12) y(1,13)], 'ydata',[y(2,12) y(2,13)], 'zdata',[y(3,12) y(3,13)]);
    
    set(rUpArm,    'xdata',[y(1,20) y(1,21)], 'ydata',[y(2,20) y(2,21)], 'zdata',[y(3,20) y(3,21)]);
    set(rLowArm,   'xdata',[y(1,21) y(1,22)], 'ydata',[y(2,21) y(2,22)], 'zdata',[y(3,21) y(3,22)]);
    set(rHand,     'xdata',[y(1,22) y(1,23)], 'ydata',[y(2,22) y(2,23)], 'zdata',[y(3,22) y(3,23)]);
    set(lUpArm,    'xdata',[y(1,15) y(1,16)], 'ydata',[y(2,15) y(2,16)], 'zdata',[y(3,15) y(3,16)]);
    set(lLowArm,   'xdata',[y(1,16) y(1,17)], 'ydata',[y(2,16) y(2,17)], 'zdata',[y(3,16) y(3,17)]);
    set(lHand,     'xdata',[y(1,17) y(1,18)], 'ydata',[y(2,17) y(2,18)], 'zdata',[y(3,17) y(3,18)]);
    
    set(lHip,      'xdata',[y(1,1) y(1,2)], 'ydata',[y(2,1) y(2,2)], 'zdata',[y(3,1) y(3,2)]);
    set(lUpLeg,    'xdata',[y(1,2) y(1,3)], 'ydata',[y(2,2) y(2,3)], 'zdata',[y(3,2) y(3,3)]);
    set(lLowLeg,   'xdata',[y(1,3) y(1,4)], 'ydata',[y(2,3) y(2,4)], 'zdata',[y(3,3) y(3,4)]);
    set(lFoot,     'xdata',[y(1,4) y(1,5)], 'ydata',[y(2,4) y(2,5)], 'zdata',[y(3,4) y(3,5)]);
    set(rHip,      'xdata',[y(1,1) y(1,6)], 'ydata',[y(2,1) y(2,6)], 'zdata',[y(3,1) y(3,6)]);
    set(rUpLeg,    'xdata',[y(1,6) y(1,7)], 'ydata',[y(2,6) y(2,7)], 'zdata',[y(3,6) y(3,7)]);
    set(rLowLeg,   'xdata',[y(1,7) y(1,8)], 'ydata',[y(2,7) y(2,8)], 'zdata',[y(3,7) y(3,8)]);
    set(rFoot,     'xdata',[y(1,8) y(1,9)], 'ydata',[y(2,8) y(2,9)], 'zdata',[y(3,8) y(3,9)]);

 
    % UPDATE BODY POSITION (PREDICTED DATA)
    % =====================================
    set(rShoulder2, 'xdata',[y2(1,12) y2(1,20)], 'ydata',[y2(2,12) y2(2,20)], 'zdata',[y2(3,12) y2(3,20)]);
    set(lShoulder2, 'xdata',[y2(1,12) y2(1,15)], 'ydata',[y2(2,12) y2(2,15)], 'zdata',[y2(3,12) y2(3,15)]);
    set(lowBack2,   'xdata',[y2(1,10) y2(1,11)], 'ydata',[y2(2,10) y2(2,11)], 'zdata',[y2(3,10) y2(3,11)]);
    set(upBack2,    'xdata',[y2(1,11) y2(1,12)], 'ydata',[y2(2,11) y2(2,12)], 'zdata',[y2(3,11) y2(3,12)]);
    set(neck2,      'xdata',[y2(1,12) y2(1,13)], 'ydata',[y2(2,12) y2(2,13)], 'zdata',[y2(3,12) y2(3,13)]);
    
    set(rUpArm2,    'xdata',[y2(1,20) y2(1,21)], 'ydata',[y2(2,20) y2(2,21)], 'zdata',[y2(3,20) y2(3,21)]);
    set(rLowArm2,   'xdata',[y2(1,21) y2(1,22)], 'ydata',[y2(2,21) y2(2,22)], 'zdata',[y2(3,21) y2(3,22)]);
    set(rHand2,     'xdata',[y2(1,22) y2(1,23)], 'ydata',[y2(2,22) y2(2,23)], 'zdata',[y2(3,22) y2(3,23)]);
    set(lUpArm2,    'xdata',[y2(1,15) y2(1,16)], 'ydata',[y2(2,15) y2(2,16)], 'zdata',[y2(3,15) y2(3,16)]);
    set(lLowArm2,   'xdata',[y2(1,16) y2(1,17)], 'ydata',[y2(2,16) y2(2,17)], 'zdata',[y2(3,16) y2(3,17)]);
    set(lHand2,     'xdata',[y2(1,17) y2(1,18)], 'ydata',[y2(2,17) y2(2,18)], 'zdata',[y2(3,17) y2(3,18)]);
    
    set(lHip2,      'xdata',[y2(1,1) y2(1,2)], 'ydata',[y2(2,1) y2(2,2)], 'zdata',[y2(3,1) y2(3,2)]);
    set(lUpLeg2,    'xdata',[y2(1,2) y2(1,3)], 'ydata',[y2(2,2) y2(2,3)], 'zdata',[y2(3,2) y2(3,3)]);
    set(lLowLeg2,   'xdata',[y2(1,3) y2(1,4)], 'ydata',[y2(2,3) y2(2,4)], 'zdata',[y2(3,3) y2(3,4)]);
    set(lFoot2,     'xdata',[y2(1,4) y2(1,5)], 'ydata',[y2(2,4) y2(2,5)], 'zdata',[y2(3,4) y2(3,5)]);
    set(rHip2,      'xdata',[y2(1,1) y2(1,6)], 'ydata',[y2(2,1) y2(2,6)], 'zdata',[y2(3,1) y2(3,6)]);
    set(rUpLeg2,    'xdata',[y2(1,6) y2(1,7)], 'ydata',[y2(2,6) y2(2,7)], 'zdata',[y2(3,6) y2(3,7)]);
    set(rLowLeg2,   'xdata',[y2(1,7) y2(1,8)], 'ydata',[y2(2,7) y2(2,8)], 'zdata',[y2(3,7) y2(3,8)]);
    set(rFoot2,     'xdata',[y2(1,8) y2(1,9)], 'ydata',[y2(2,8) y2(2,9)], 'zdata',[y2(3,8) y2(3,9)]);
    
    % UPDATE RED CIRCLE AND TEXT ON PCA PLOT
    % =========================================
    set(CURRENT, 'xdata', pc(frame,1), 'ydata', pc(frame,2));
    set(currentText, 'String', ['Current Frame: ' num2str(frame)]);
    if frame>trainSize,
      set(currentText2, 'String','Test data');
    end;
    drawnow;
    rotate3d(FIGURE, 'on');
end















