% GetDataOfCurrentFrame
% [time_stamp person_ID good_head x y z  Qr Qx Qy Qz]

if ~exist('currentIndex','var')
    currentIndex = 1; % An auxilary parameter to synchronise the times stamp and the kinect detection
    tempTimeStamp = headPoses(currentIndex,1);
    currentTimeStamp = allTimeStamp(k);
    while tempTimeStamp~=currentTimeStamp,
        currentIndex = currentIndex+1;
        tempTimeStamp = headPoses(currentIndex,1);
    end
    Z_pre = [];
    previouseHeadpose = [];
    IDstatus = [];
    colours = [0,0,0; jet(6)];
    
    %     axislim = [-2 2, -0.5 1.5, 0 5];
    %     axislim = [-2 2, 0 1, 0 4];
end

Z = [];
currentTimeStamp = allTimeStamp(k);
tempTimeStamp = headPoses(currentIndex,1);
while tempTimeStamp==currentTimeStamp,
    Z = [Z;headPoses(currentIndex,:)];
    currentIndex = currentIndex+1;
    tempTimeStamp = headPoses(currentIndex,1);
end

Z = Z(:,[4 5 6]); Z = Z'; % [x,y,z]
m_k = size(Z,2); % length of current data
Z_pre = Z;

% now calculate the clutter intensity for each measurements 
if m_k>0
    k_z = 0.2*ones(1,m_k);
    % https://msdn.microsoft.com/en-us/library/hh973074.aspx
    % Kinect horizontal Field of View in default range
    z_range = [1.2,4.5];
    nFOVid = [];
    for i = 1:m_k,
        if Z(3,:)<0, % must be in front of the kinect!
            k_z(i) = 2;
            nFOVid = [nFOVid,i];
        else
            range = norm(Z(:,i),'fro');
            if range>z_range(2) || range<z_range(1),
                k_z(i) = k_z(i) + min((range>z_range(2))*(range-z_range(2))*2,2) + (range<z_range(1))*(z_range(1)-range)*2/z_range(1);
                nFOVid = [nFOVid,i];
            end
            
            if abs(Z(1,i))>Z(3,i)*tand(35.3) || Z(2,i)>Z(3,i)*tand(30),
                k_z(i) = k_z(i)+1;
                nFOVid = [nFOVid,i];
            end
        end
    end
    
    nFOVid = unique(nFOVid);
    % in the kinect field;
    Idfield = setdiff([1:m_k],nFOVid);
    if sum(Idfield)>1 % at least two detections in the field
        [~,Idx] = sort(Z(3,Idfield),'ascend');
        
        for i = 1:length(Idx)-1,
            frontIndx = Idx(i);
            for j = i+1:length(Idx),
                backIndex = Idx(j);
                sigma_occ = 0.2*Z(3,backIndex)/Z(3,frontIndx);
                x_map = Z(1,frontIndx)*Z(3,backIndex)/Z(3,frontIndx);
                dis = x_map-Z(1,backIndex);
                k_z(backIndex) = k_z(backIndex) +  0.3*exp(-dis^2/(2*sigma_occ^2));
            end
        end
        
    end
    
    %     Id = Z(3,:)>z_range(2) | Z(3,:)<z_range(1) | Z(2,:)<0;
    %     if any(Id)
    %         k_z(Id) = 2;
    %     end
    %   Kinect vertical Field of View in default range
    %     Id2 = abs(Z(1,:))>Z(3,:)*tand(35.3) | Z(2,:)>Z(3,:)*tand(30); % view angle 70.6 x 60 degrees
    %     Id2 = Id2 & ~Id;
    %     if any(Id2)
    %         k_z(Id2) = 0.2+1;
    %     end
    
    %     % in the kinect field;
    %     Idfield = find(~Id & ~Id2);
    %     if sum(Idfield)>1 % at least two detections in the field
    %         [~,Idx] = sort(Z(3,Idfield),'ascend');
    %
    %         for i = 1:length(Idx)-1,
    %             frontIndx = Idx(i);
    %             for j = i+1:length(Idx),
    %                 backIndex = Idx(j);
    %                 sigma_occ = 0.2*Z(3,backIndex)/Z(3,frontIndx);
    %                 x_map = Z(1,frontIndx)*Z(3,backIndex)/Z(3,frontIndx);
    %                 dis = x_map-Z(1,backIndex);
    %                 k_z(backIndex) = k_z(backIndex) +  0.3*exp(-dis^2/(2*sigma_occ^2));
    %             end
    %         end
    %
    %     end
    
    
end

disp(['At time frame ',num2str(k),', the Detected # person is ',num2str(m_k),'==========']);