clear all;close all;clc
%% For One Speaker and one Dummy head
%% Get position of the speaker we want, the plain parameter for the speaker path and the dummy head position
% mat_filename = 'Katherine_headPoses.mat';% Katherine
mat_filename = 'Mark_headPoses.mat';% Mark

% addpath('../../../shared/Code/MatLab/Tools/');
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
dummyID = peopleIDs(1); % there might be more than one ID associated with the dummy head
% however, since the dummy head is static, we just need one of these IDs to
% estimate accurately the dummy head location.
minVar = 100000;
dummyPos = [0 0 0];

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

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]);
        mean_data = mean(data);
        data_mean = data-repmat(mean_data,[L 1]);
        % keep 90 percent of the data;
        tempvar = sum(data_mean.^2,2); % distance to the centre of this group
        [var_sort,Index] = sort(tempvar,'ascend');
        data_use = data(Index(1:round(L*0.9)),:);
        pos = mean(data_use);
        data_use_mean = data_use-repmat(pos,[round(L*0.9) 1]);
        tempvar = sum(sum(data_use_mean.^2));
        if tempvar<minVar
            dummyID = peopleIDs(p);
            dummyPos = pos;
        end
    end
end
fprintf('Dummy head ID = %d at the 3D position of [%0.3g %0.3g %0.3g]\n', dummyID,dummyPos);

% Now find the single speaker ID
speakerID = []; % actually only one speaker
for p=1:nPeople,
    if peopleIDs(p)~=dummyID,
        currPersonIdx = (headPoses(:,2)==peopleIDs(p));
        currPersonIdx(badPoses) = false;
        currPersonIdx = find(currPersonIdx);
        L = length(currPersonIdx);
        if L>500,
            % to detect if it is the dummy head
            data = headPoses(currPersonIdx,[4:6]);
            data_mean = data-repmat(dummyPos,[L 1]); % distance to the dummy head
            dis2 = sum(data_mean.^2,2);
            
            NbigDis2 = sum(dis2>0.5);
            if NbigDis2 >0.5*L, % 80 percent of these samples are beyond the distance of sqrt(0.5) to the dummy
                speakerID = [speakerID,peopleIDs(p)];
            end
        end
    end
end


% plot the useful detected point

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);
    hh = plot3(headPoses(currPersonIdx,4), headPoses(currPersonIdx,5), headPoses(currPersonIdx,6), '*','color', colours(p,:));
end
axis([-2 2, -1.2 1.2, 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');



% find the plane of the speaker trajectory with linear programming
currPersonIdx = (headPoses(:,2)==speakerID);
currPersonIdx(badPoses) = false;
currPersonIdx = find(currPersonIdx);
L = length(currPersonIdx);
data = headPoses(currPersonIdx,[4:6]);
useIndex = true(1,L);
threshold = 0.2;
flag = 1;
hplane = [];
varValue_pre = 10000;
while (flag)
    Xcolv = data(useIndex,1); % Make X a column vector
    Ycolv = data(useIndex,2); % Make Y a column vector
    Zcolv = data(useIndex,3); % Make Z a column vector
    Const = ones(size(Xcolv)); % Vector of ones for constant term
    
    Coefficients = [Xcolv Ycolv Const]\Zcolv; % Find the coefficients fot the plane
    
    %The plain is z = XCoeff*x + XCoeff*x + CCoeff
    % or XCoeff*x + XCoeff*x + (-1)*z + CCoeff = 0
    XCoeff = Coefficients(1); % X coefficient
    YCoeff = Coefficients(2); % X coefficient
    CCoeff = Coefficients(3); % constant term
    delete(hplane);
    hplane = plotPlane([XCoeff,YCoeff,-1],CCoeff,[-2 2],[0 5]);
    %         pause(3)
    
    % distance from the pont 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;
    
    varValue = var(dis(useIndex));
    
    useIndex = true(1,L);
    useIndex(dis>threshold) = false;
    threshold = threshold*0.8;
    if abs(varValue-varValue_pre)<0.01,
        flag = 0;
    end
    
    varValue_pre = varValue;
end

figure;plot(dis) % this is to choose the theshold for useful deteaction
threshold = 0.15;
useIndex = true(1,L);
useIndex(dis>threshold) = false;

usePersonIdx = currPersonIdx(useIndex);
% [time_stamp person_ID good_head x y z  Qr Qx Qy Qz]
% keep the time stamp for each frame, and then the location information
headPose = headPoses(usePersonIdx,[1 4 5 6]);

%% position of the speaker we want, the plain parameter for the speaker path, the dummy head position
% save KatherineData.mat headPose Coefficients dummyID dummyPos allTimeStamp
% save MarkData.mat headPose Coefficients dummyPos allTimeStamp

