% This script reads a MAT file containing the data structure converted from
% JSON and wired a _headPose file, containing a matrix of head poses.

clear all; close all; clc
addpath('../HeadPoseAnalysis/Quaternions/');
addpath('../HeadPoseAnalysis/Network/UDP_Receiver/');
mat_filename = '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF/20150217_111609_00.mat'; % MK in rectangle
% mat_filename = '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF/20150217_112107_00.mat'; % MK in lines
% 20150217_111155_00.mat'; % Katherine
% 20150217_112354_00.mat'; % Mark
load(mat_filename);
[file_pathstr,filename,file_ext] = fileparts(mat_filename);

headPoses = getHeadPoseVector(json2data);

peopleIDs = unique(headPoses(:,2));
peopleIDs = peopleIDs(2:end);
nPeople = length(peopleIDs);
colours = [0,0,0; jet(nPeople-1)];

% Indices of poses that are far enough form the kinect sensor:
dist2kinect = sqrt(sum(headPoses(:,4:6).^2,2));
badPoses = (dist2kinect<.1); % if the head is less than 10cm from the sensor, it's not good.
figure;
for p=1:nPeople,
    currPersonIdx = (headPoses(:,2)==peopleIDs(p));
    currPersonIdx(badPoses) = false;
    plot3(headPoses(currPersonIdx,4), headPoses(currPersonIdx,5), headPoses(currPersonIdx,6), '*','color', colours(p,:));
    hold on
end
axis([-2 2, -1.2 1.2, 0 5])
xlabel('X');
ylabel('Y');
zlabel('Z');
view([180,-90]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');

grid on
hold off;
headPosesFileName = [file_pathstr, filesep, filename, '_headPoses.mat'];
if ~exist(headPosesFileName, 'file'),
    save([file_pathstr, filesep, filename, '_headPoses.mat'], 'headPoses');
end
% savefig([file_pathstr, filesep. filename])

if 0,
    files = dir('*.fig');
    for f=1:length(files),
        openfig(files(f).name);
        view([180,-90]);
        cameratoolbar('Show');
        cameratoolbar('SetMode', 'orbit');
        cameratoolbar('SetCoordSys','y');
    end
end

fprintf('#detections=%d,  #visible_faces=%d,  #frames=%d,  #good_poses=%d\n', size(headPoses,1), sum(headPoses(:,3)), length(unique(headPoses(:,1))), sum(~badPoses));


% Plot to show that data is acquired in a linear time:
figure;
[uniqueTimeStamps, iA, iC]= unique(headPoses(:,1));
for p=1:nPeople,
    currPersonIdx = (headPoses(:,2)==peopleIDs(p));
    plot(iC(currPersonIdx), headPoses(currPersonIdx,1), '-*','color', colours(p,:))
    hold on
end

