clear all;close all;clc

% disply the Kinect skelton tracking results (the head positions are saved in the mat file, their IDs are swapped)

% mat_filename = '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF/20150217_111609_00.mat'; % MK in rectangle
% load(mat_filename);
% [headPoses,allTimeStamp] = getHeadPoseVector(json2data);
% save MKrectangleKinect_headPose.mat headPoses allTimeStamp


% mat_filename = '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF/20150217_112107_00.mat'; % MK in lines
% load(mat_filename);
% [headPoses,allTimeStamp] = getHeadPoseVector(json2data);
% save MKlinesKinect_headPose.mat headPoses allTimeStamp

%% For multiple Speakers and one Dummy head Get position of the speakerS we want, the plain parameter for the speakerS' paths and the dummy head position
% [time_stamp person_ID good_head x y z  Qr Qx Qy Qz]

% mat_filename = '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF/20150217_111609_00_headPosesGT.mat';  % Mark and Katherine
% load(mat_filename);
% headPoses = headPosesGT;

mat_filename = 'MKrectangleKinect_headPose.mat'; 
% mat_filename = 'MKlinesKinect_headPose.mat';
load(mat_filename);


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


% find the dummy head ID
% Based on the assuption that the dummy is static

% 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.

clusters = [];
vari = [];
for p=1:nPeople,
    currPersonIdx = (headPoses(:,2)==peopleIDs(p));
    currPersonIdx(badPoses) = false;
    currPersonIdx = find(currPersonIdx);
    L = length(currPersonIdx);
    if L>200,
        data = headPoses(currPersonIdx,[4:6]);
        % divide each long sequence into floor(L/200) segments
        for seg = 1:min(floor(L/200),20),
            tempData = data((seg-1)*200+1:seg*200,:);
            mean_data = mean(tempData);
            data_mean = tempData-repmat(mean_data,[200 1]);
            % keep 90 percent of the data;
            tempvar = mean(sum(data_mean.^2,2)); % distance to the centre of this group
            if tempvar<0.1,
                vari = [vari;tempvar];
                clusters = [clusters;mean_data];
            end
        end
    end
end


N = 1;
cclusters = clusters(1,:);
ncclusters = 1;
for i = 2:size(clusters,1),
    newFlag=1;
    for n = 1:N,
        dis = clusters(i,:)-cclusters(n,:);
        dis = sum(abs(dis));
        if dis <0.1
            cclusters(n,:) = cclusters(n,:)*ncclusters(n)+clusters(i,:);
            ncclusters(n) = ncclusters(n) +1;
            cclusters(n,:) = cclusters(n,:)/ncclusters(n);
            newFlag=0;
            break
            
        end
    end
    
    if newFlag
        cclusters = [cclusters;clusters(i,:)];
        ncclusters = [ncclusters,1];
    end
    
    N = length(ncclusters);
end
[~,idx] = max(ncclusters);
dummyPos = cclusters(idx,:);

fprintf('Dummy head at the 3D position of [%0.3g %0.3g %0.3g]\n', dummyPos);

% remove the outliers
% remove the dummy head detection


speakerID = 0:5;
for p = 1:length(speakerID),
    currPersonIdx = (headPoses(:,2)==speakerID(p));
    currPersonIdx(badPoses) = false;
    currPersonIdx = find(currPersonIdx);
    dis = repmat(dummyPos,[length(currPersonIdx),1]) - headPoses(currPersonIdx,4:6);
    dis = sum(abs(dis),2);
    removeFlag = dis<0.7;
    badPoses(currPersonIdx) = removeFlag;
end

% plot the useful detected point
speakerID = 0:5;
figure;
% plot the dummy head
plot3(dummyPos(1), dummyPos(2), dummyPos(3), 'o','MarkerFaceColor','g','MarkerSize',8);
hold on
% plot the spearker
for p = 1:length(speakerID),
    currPersonIdx = (headPoses(:,2)==speakerID(p));
    currPersonIdx(badPoses) = false;
    currPersonIdx = find(currPersonIdx);
    if ~isempty(currPersonIdx)
        hh = plot3(headPoses(currPersonIdx,4), headPoses(currPersonIdx,5), headPoses(currPersonIdx,6), '*','color', colours(p,:));
    end
end
axis([-2 2, -0.5 1.5, 0 5]);grid on;
xlabel('X');ylabel('Y');zlabel('Z');
% view([180,-90]);
view([-37.5,30]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');

% from three points (0 0 0), dummyPos, (2 0 0) to  calculate the plane
% coefficients
Coefficients = [0 0 1;dummyPos(1:2) 1;2 0 1]\[0;dummyPos(3);0];
%The plain is z = XCoeff*x + YCoeff*y + CCoeff
XCoeff = Coefficients(1); % X coefficient
YCoeff = Coefficients(2); % X coefficient
CCoeff = Coefficients(3); % constant term
hplane = plotPlane([XCoeff,YCoeff,-1],CCoeff,[-2 2],[0 5]);




% find the plane of the speaker trajectory with linear programming
dataIndex = find(~badPoses);
data = headPoses(dataIndex,[4:6]);
L = length(data);
threshold = 0.35;

% distance from the point to the plane;
% equation 9 http://mathworld.wolfram.com/Point-PlaneDistance.html
denom = sqrt(XCoeff^2 + YCoeff^2 +1);
dis = abs( XCoeff*data(:,1) + YCoeff*data(:,2) + (-1)*data(:,3) + CCoeff )/denom;
useIndex = false(1,L);
useIndex(dis>threshold) = true;
badPoses(dataIndex(useIndex)) = true;

% distance from the point to the origin;
dataIndex = find(~badPoses);
data = headPoses(dataIndex,[4:6]);
L = length(data);
threshold = 0.5;
dis = sqrt(data(:,1).^2 + data(:,2).^2 + data(:,3).^2);
useIndex = false(1,L);
useIndex(dis<threshold) = true;
badPoses(dataIndex(useIndex)) = true;


% plot the useful detected point
speakerID = 0:5;
figure;
% plot the dummy head
plot3(dummyPos(1), dummyPos(2), dummyPos(3), 'o','MarkerFaceColor','g','MarkerSize',8);
hold on
% plot the spearker
for p = 1:length(speakerID),
    currPersonIdx = (headPoses(:,2)==speakerID(p));
    currPersonIdx(badPoses) = false;
    currPersonIdx = find(currPersonIdx);
    if ~isempty(currPersonIdx)
        hh = plot3(headPoses(currPersonIdx,4), headPoses(currPersonIdx,5), headPoses(currPersonIdx,6), '*','color', colours(p,:));
    end
end
axis([-2 2, -0.5 1.5, 0 5]);grid on;
xlabel('X');ylabel('Y');zlabel('Z');
view([-37.5,30]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');


