function [headPoses,allTimeStamp] = getHeadPoseVector(json2data)
% headPose = [time_stamp person_ID good_head x y z  Qw Qx Qy Qz]
% good_head is a flag that is set to 1 if the head was actually measured
% and 0 if the head pose was determined based on the neck (skeleton).

% extract all the time stamps, including these frames without any people
% detection
allTimeStamp = zeros(length(json2data),1);
for c=1:length(json2data)
    allTimeStamp(c) = json2data{c}.nTime;
end
% figure;plot(diff(allTimeStamp)); % to find the frame that takes much
% longer than the other frames

% First, count how many people were detected to be able 
% to pre-alocate headPoses:
nPeople = 0;
for c=1:length(json2data)
    if isfield(json2data{c}, 'People'),
        nPeople = nPeople + length(json2data{c}.People);
    end
end

headPoses = -ones(nPeople,10); % This is a matrix where each row is a 9d vector descriping the pose of a person in one instant:
% [time_stamp person_ID good_head x y z  Qr Qx Qy Qz]
% good_head is a flag that is set to 1 if the head was actually measured
% and 0 if the head pose was determined based on the neck (skeleton).
headCount = 0;
for c=1:length(json2data)
    if isfield(json2data{c}, 'People'),
        for p=1:length(json2data{c}.People),
            headCount = headCount + 1;
            % headPoses(headCount,:) = getHeadPoseVector(json2data{c}.People{p});

            good_head = str2num(json2data{c}.People{p}.validFace);
            [X,Y,Z] = jsonStruct2link(json2data{c}.People{p}.joints.Head, json2data{c}.People{p}.joints.Head);
            x = double([X(1), Y(1), Z(1)]);

            if good_head
                Q = double([json2data{c}.People{p}.faceRotation.w, json2data{c}.People{p}.faceRotation.x, json2data{c}.People{p}.faceRotation.y, json2data{c}.People{p}.faceRotation.z]);
            else
                % The lines below would be much nicer if I was using the quaternion
                % functions from the Aerospace Toolbox:
                RotMatrix = quaternion(double([json2data{c}.People{p}.jointOrientations.Neck.w, json2data{c}.People{p}.jointOrientations.Neck.x, json2data{c}.People{p}.jointOrientations.Neck.y, json2data{c}.People{p}.jointOrientations.Neck.z]));
                RotMatrix = RotMatrix * [1 0 0; 0 -1 0; 0 0 -1]; % Rotate 180 degrees around the X axis
                Q = quaternion(RotMatrix)';
            end

            headPoses(headCount,:) = [double(json2data{c}.nTime), double(json2data{c}.People{p}.iFace), double(good_head), x, Q];
        end
    else
        headCount = headCount+1;
        headPoses(headCount,1) = double(json2data{c}.nTime);
    end
end
