%% calculate the azimuth of the speakers relatively to the dummy head.
% multiple speakers
clear all;close all;clc
% position of the speaker, its plain parameter, the dummy head position, allTimeStamp
mat_filename = 'MarkKatherineData.mat';% Mark
load(mat_filename)

figure;
% plot the dummy head
plot3(dummyPos(1), dummyPos(2), dummyPos(3), 'o','MarkerFaceColor','g','MarkerSize',8);
hold on
% the origin
plot3(0, 0, 0, 's','MarkerFaceColor','r','MarkerSize',8);
% plot the spearker
plot3(headPose(:,2), headPose(:,3), headPose(:,4), 'b*');
axis([-2 2, -0.2 1.5, -1 5]);grid on;
% plot the plane
plotPlane([Coefficients(1),Coefficients(2),-1],Coefficients(3),[-2 2],[-1 5]);
xlabel('X');ylabel('Y');zlabel('Z');
% view([180,-90]);
view([-37.5,30]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');


% map every points into the plane

%The projection of a point q = (x, y, z) onto a plane given by a point p = (a, b, c)
%and a normal n = (d, e, f) is q_proj = q - dot(q - p, n) * n
n = [Coefficients(1),Coefficients(2),-1];
n = n/norm(n,'fro'); % the normal, passes point [0 0 Coefficients(3)]
tempdata = [headPose(:,[2:4]);[0 0 0];dummyPos]; % q
temp = tempdata;
temp(:,3) = tempdata(:,3)-Coefficients(3); % q-p
dot = temp*n(:); %dot(q - p, n)
mapPoints = tempdata - dot*n;
figure; hold on;
% the mapped data
plot3(mapPoints(1:end-2,1), mapPoints(1:end-2,2), mapPoints(1:end-2,3), 'c*');
% plot3(mapPoints(1:1000,1), mapPoints(1:1000,2), mapPoints(1:1000,3), 'c*');
% the mapped origin
plot3(mapPoints(end-1,1), mapPoints(end-1,2), mapPoints(end-1,3), 's','MarkerFaceColor','r','MarkerSize',8);
% the mapped dummy head
plot3(mapPoints(end,1), mapPoints(end,2), mapPoints(end,3), 'o','MarkerFaceColor','g','MarkerSize',8);
axis([-2 2, -0.2 1.5, -1 5]);grid on;
% plot the plane
plotPlane([Coefficients(1),Coefficients(2),-1],Coefficients(3),[-2 2],[-1 5]);
xlabel('X');ylabel('Y');zlabel('Z');
% view([180,-90]);
% view([-37.5,30]);
view(n); 
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');

% the mapped origin
origin = [mapPoints(end-1,1), mapPoints(end-1,2), mapPoints(end-1,3)];
% the mapped dummy head
dummyPos = [mapPoints(end,1), mapPoints(end,2), mapPoints(end,3)];
% the mapped data;
mapPoints = mapPoints(1:end-2,:);

unitx = (origin-dummyPos); unitx = unitx/norm(unitx,'fro');
LL = 3.4;
quiver3(dummyPos(1),dummyPos(2),dummyPos(3),unitx(1)*LL,unitx(2)*LL,unitx(3)*LL,'LineWidth',2,'Color','k');
quiver3(dummyPos(1),dummyPos(2),dummyPos(3),-unitx(1)*LL,-unitx(2)*LL,-unitx(3)*LL,'Color','k');
% find the 3d vector perpenticular to unitx and parallel to the plane
xx = -1; % the first element of the vector
unity = -xx*pinv([unitx(2),unitx(3);Coefficients(2),-1])*[unitx(1);Coefficients(1)];
unity = [xx;unity];unity = unity/norm(unity,'fro');
LL = 2;
quiver3(dummyPos(1),dummyPos(2),dummyPos(3),unity(1)*LL,unity(2)*LL,unity(3)*LL,'LineWidth',2,'Color','k');
LL = 3;
quiver3(dummyPos(1),dummyPos(2),dummyPos(3),-unity(1)*LL,-unity(2)*LL,-unity(3)*LL,'Color','k');

dir = mapPoints-repmat(dummyPos,[length(mapPoints),1]);
cosv = dir*unitx(:)./sqrt(sum(dir.^2,2));
sinv = dir*unity(:);
indexNeg = sinv<0;
azimuth = acosd(cosv);
if ~isempty(indexNeg)
    azimuth(indexNeg) = -azimuth(indexNeg);
end



[~,locb] = ismember(headPose(:,1),allTimeStamp);

figure;plot(allTimeStamp(locb),azimuth,'*');ylim([-190 190]);
set(gca,'Ytick',[-180:20:180]);grid on;

save MarkKatherineAzimuth.mat headPose azimuth Coefficients dummyPos allTimeStamp
