% This script reads a MAT file containing the data structure converted from
% JSON and wired a _headPose file, containing a matrix of head poses.

addpath('../../../shared/Code/MatLab/Quaternions/');
addpath('../../../shared/Code/MatLab/QuaternionAverage/');
addpath('../../../shared/Code/MatLab/Network/UDP_Receiver/');
addpath('../../../shared/Code/MatLab/Tools/Plotting/');
addpath('../../../shared/Code/MatLab/Tools/');

pathstr = '/vol/vssp/datasets/vid+depth01/S3A_XEF_02_2015';
    % '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF';
    % 'C:\Users\s3a\Videos\Kinect';
    % '/vol/vssp/ucdatasets/s3a/vml/2015_02_17_XEF' 
filename = '20150923_125248_00';
         % '20150217_111609_00';
         % '20150217_111609_00';
         % '20150217_112354_00';
         % '20150217_112107_00';
         % '20150217_111155_00'
         % '../../../shared/Code/MatLab/Network/UDP_Receiver/received_data.mat';    
mat_filename = [pathstr, filesep, filename];

load(mat_filename);
% Load annotation:
annotation_filename = [pathstr, filesep, filename, '_annotation.mat'];
if exist(annotation_filename,'file')
    load(annotation_filename);
end

% Converting JSON file into a matrix

nFrames = length(json2data);
droppedFrames = false(nFrames,1);

dummy = -ones(nFrames,1);
detected_as = -ones(nFrames, 4);
missing = false(nFrames, 4);
bad_position = false(nFrames, 4);
bad_orientation = false(nFrames, 4);
validFace = false(nFrames, 4);
is_present = false(nFrames, 4);

for t=1:nFrames
    droppedFrames(t) = ~isfield(json2data{t}, 'People');
    if ~droppedFrames(t),
        nDetectedPeople = length(json2data{t}.People);
        for p=1:6,
            vf = false;
            da = -1;
            pID = -1; % Determined by iFace (if detected) or by annotated new_label
            for pd=1:nDetectedPeople,
                if json2data{t}.People{pd}.iFace == p-1;
                    pID = p-1;
                    da = p;
                    vf = strcmpi(json2data{t}.People{pd}.validFace, 'True');
                end
            end
            correct_ID = pID;
            if pID >= 0, % Person has been detected
                if annotation.person(t,pID+1).new_label >= 0,
                    % Person not detected and with wrong label
                    correct_ID = annotation.person(t,pID+1).new_label;
                end
            else % pID < 0, % If pID is still -1, it's possible that the person was not
                % detected but its label is correct.
                if annotation.person(t,p).new_label >=0
                    correct_ID = annotation.person(t,p).new_label;
                elseif annotation.person(t,p).bad_orientation || annotation.person(t,p).bad_position || annotation.person(t,p).not_detected || annotation.person(t,p).dummy,
                    correct_ID = p-1;
                end
            end
            if correct_ID >= 0,
                validFace(t,correct_ID+1) = vf;
                is_present(t,correct_ID+1) = 1;
                detected_as(t,correct_ID+1) = da;
                missing(t,correct_ID+1) = annotation.person(t,p).not_detected;
                bad_position(t,correct_ID+1) = annotation.person(t,p).bad_position;
                bad_orientation(t,correct_ID+1) = annotation.person(t,p).bad_orientation;
                if annotation.person(t,p).dummy,
                    dummy(t) = p;
                end
            end 
        end
    end
end

% Visualisations:
figure; imagesc(is_present); colorbar
figure; imagesc(missing); colorbar
figure; imagesc(bad_position); colorbar
figure; imagesc(bad_orientation); colorbar
good_frame_idx = find(~droppedFrames);

figure;plot(good_frame_idx,is_present(~droppedFrames,:)'); legend('dummy','Katherine', 'Mark', 'Teo')

figure;plot(good_frame_idx, dummy(~droppedFrames),'-*');
title('ID of the dummy head (assigned by the detector)')

%% 
%%%%%%%%%% Indentities
figure;
hold on
markers = {'k-', 'r--', 'b-.', 'm-', 'g-+', 'y-*'};
for p=1:4
    plot(good_frame_idx, detected_as(~droppedFrames,p) ,markers{p}, 'LineWidth', 3);
end
% legend('dummy','Katherine', 'Mark', 'Teo')
big_legend(12, 0, 'Dummy head','Actress A', 'Actor B', 'Clapper')
big_xlabel(14,'Frame number');
big_ylabel(14, 'Label assigned by Kinect2');
big_axes(12)
% title('New labels (colour code shows which labels they were assigned by the detector)')

%%%%%%%%%% Indentities
n_people_per_frame = sum(is_present,2);
n_people_per_valid_frame = sum(is_present(~droppedFrames,:),2);
figure; plot(good_frame_idx, n_people_per_valid_frame) % There should be not more than 4 people!

% Real frame rate: total number of detected frames during the time interval
% (which is in units of 0.1 microseconds)
real_fps = (10000000* (sum(~droppedFrames)))/double(json2data{end}.nTime - json2data{1}.nTime);

n_present_people = sum(is_present(~droppedFrames,:));

detections = []

% Now that I have all ground truth, I can compute stats:
fprintf('Dropped frames: %d out of %d (%2.3f%%)\n',sum(droppedFrames), nFrames, 100*sum(droppedFrames)/nFrames);
fprintf('Actual frame rate (taking dropped frames into accont): %2.3f\n', real_fps);
fprintf('People actually present in valid frames:\n\tTotal: %d\n\taverage per frame: %1.3f\n\tminimum in a frame: %d\n\tmaximum in a frame: %d\n', sum(sum(is_present(~droppedFrames,:))), mean(n_people_per_valid_frame), min(n_people_per_valid_frame), max(n_people_per_valid_frame));
fprintf('Missed detections in valid frames:\n\tTotal: %d (%02.3f%%)\n', sum(sum(missing(~droppedFrames,:))), 100*mean(mean(missing(~droppedFrames,:))));
fprintf('\tDummy: %d/%d (%2.3f%%)\n',  sum(missing(~droppedFrames,1)), n_present_people(1), 100*sum(missing(~droppedFrames,1))/n_present_people(1));
fprintf('\tKatherine: %d/%d (%2.3f%%)\n',  sum(missing(~droppedFrames,2)), n_present_people(2), 100*sum(missing(~droppedFrames,2))/n_present_people(2));
fprintf('\tMark: %d/%d (%2.3f%%)\n',  sum(missing(~droppedFrames,3)), n_present_people(3), 100*sum(missing(~droppedFrames,3))/n_present_people(3));
fprintf('\tTeo: %d/%d (%2.3f%%)\n',  sum(missing(~droppedFrames,4)), n_present_people(4), 100*sum(missing(~droppedFrames,4))/n_present_people(4));
% False positive detections have not occured.
fprintf('----------------\nTracking results\nNumber of bad head orientation estimations (out of the total of detected people):\n');
bo = bad_orientation & is_present;
bo(droppedFrames,:) = 0;
fprintf('\tTotal: %d (%2.3f%%)\n', sum(bo(:)), 100*sum(bo(:))/sum(n_present_people))
fprintf('\tDummy: %d (%2.3f%%)\n', sum(bo(:,1)), 100*sum(bo(:,1))/n_present_people(1))
fprintf('\tKatherine: %d (%2.3f%%)\n', sum(bo(:,2)), 100*sum(bo(:,2))/n_present_people(2))
fprintf('\tMark: %d (%2.3f%%)\n', sum(bo(:,3)),  100*sum(bo(:,3))/n_present_people(3))
fprintf('\tTeo: %d (%2.3f%%)\n', sum(bo(:,4)),  100*sum(bo(:,4))/n_present_people(4))
fprintf('Number of bad head position estimations (out of the total of detected people):\n');
bo = bad_position & is_present;
bo(droppedFrames,:) = 0;
fprintf('\tTotal: %d (%2.3f%%)\n', sum(bo(:)), 100*sum(bo(:))/sum(n_present_people))
fprintf('\tDummy: %d (%2.3f%%)\n', sum(bo(:,1)), 100*sum(bo(:,1))/n_present_people(1))
fprintf('\tKatherine: %d (%2.3f%%)\n', sum(bo(:,2)), 100*sum(bo(:,2))/n_present_people(2))
fprintf('\tMark: %d (%2.3f%%)\n', sum(bo(:,3)),  100*sum(bo(:,3))/n_present_people(3))
fprintf('\tTeo: %d (%2.3f%%)\n', sum(bo(:,4)),  100*sum(bo(:,4))/n_present_people(4))


fprintf('----------------\nSame as above, but in a positive way (true detections, rather than missing):');
n_true_detections = (n_present_people - sum(missing(~droppedFrames,:)))
fprintf('True positive person detections:\n');
fprintf('\tTotal: %d (%2.3f%%)\n', sum(n_true_detections), 100*sum(n_true_detections)/sum(n_present_people));
fprintf('\tDummy: %d (%2.3f%%)\n', n_true_detections(1), 100*n_true_detections(1)/n_present_people(1));
fprintf('\tKatherine: %d (%2.3f%%)\n', n_true_detections(2), 100*n_true_detections(2)/n_present_people(2));
fprintf('\tMark: %d (%2.3f%%)\n', n_true_detections(3), 100*n_true_detections(3)/n_present_people(3));
fprintf('\tTeo: %d (%2.3f%%)\n', n_true_detections(4), 100*n_true_detections(4)/n_present_people(4));

fprintf('Number of good head position estimations (out of the total of detected people):\n');
bo = (~bad_position) & is_present & (~missing);
bo(droppedFrames,:) = 0;
fprintf('\tTotal: %d (%2.3f%%)\n', sum(bo(:)), 100*sum(bo(:))/sum(n_true_detections))
fprintf('\tDummy: %d (%2.3f%%)\n', sum(bo(:,1)), 100*sum(bo(:,1))/n_true_detections(1))
fprintf('\tKatherine: %d (%2.3f%%)\n', sum(bo(:,2)), 100*sum(bo(:,2))/n_true_detections(2))
fprintf('\tMark: %d (%2.3f%%)\n', sum(bo(:,3)),  100*sum(bo(:,3))/n_true_detections(3))
fprintf('\tTeo: %d (%2.3f%%)\n', sum(bo(:,4)),  100*sum(bo(:,4))/n_true_detections(4))

fprintf('\nNumber of good head orientation estimations (out of the total of detected people):\n');
bo = (~bad_orientation) & is_present & (~missing);
bo(droppedFrames,:) = 0;
fprintf('\tTotal: %d (%2.3f%%)\n', sum(bo(:)), 100*sum(bo(:))/sum(n_true_detections))
fprintf('\tDummy: %d (%2.3f%%)\n', sum(bo(:,1)), 100*sum(bo(:,1))/n_true_detections(1))
fprintf('\tKatherine: %d (%2.3f%%)\n', sum(bo(:,2)), 100*sum(bo(:,2))/n_true_detections(2))
fprintf('\tMark: %d (%2.3f%%)\n', sum(bo(:,3)),  100*sum(bo(:,3))/n_true_detections(3))
fprintf('\tTeo: %d (%2.3f%%)\n', sum(bo(:,4)),  100*sum(bo(:,4))/n_true_detections(4))
fprintf('\tTotal good orientation rate, excluding the dummy: %2.2f%%\n', 100*sum(sum(bo(:,2:end)))/sum(n_true_detections(2:4)))

% Compute number of label changes
label_changes = zeros(1,4);
figure;
for p=1:4
    gfi = find(is_present(:,p) & ~droppedFrames & (detected_as(:,p) ~= -1));
    changes= detected_as(gfi(2:end),p)~=detected_as(gfi(1:end-1),p);
    label_changes(p) = sum(changes);
    subplot(8,1,2*p-1)
    plot(detected_as(gfi,p))
    subplot(8,1,2*p)
    plot(changes)
end

fprintf('----------------\nLabel changes\n');
fprintf('\tTotal: %d\n\tDummy: %d\n\tKatherine: %d\n\tMark: %d\n\tTeo: %d\n', sum(label_changes), label_changes(1), label_changes(2), label_changes(3), label_changes(4))

%%
headPoses = getHeadPoseVector(json2data);

% Indices of poses that are far enough form the kinect sensor:
figure;
plot_head_trajectories(headPoses)

headPosesFileName = [pathstr, filesep, filename, '_headPoses.mat'];
if ~exist(headPosesFileName, 'file'),
    save([pathstr, filesep, filename, '_headPoses.mat'], 'headPoses');
end
% savefig([file_pathstr, filesep. filename])

if 0,
    files = dir('*.fig');
    for f=1:length(files),
        openfig(files(f).name);
        view([180,-90]);
        cameratoolbar('Show');
        cameratoolbar('SetMode', 'orbit');
        cameratoolbar('SetCoordSys','y');
    end
end

fprintf('#detections=%d,  #visible_faces=%d,  #frames=%d\n', size(headPoses,1), sum(headPoses(:,3)), length(unique(headPoses(:,1))));
n_dummies_detected = n_present_people(1)-sum(missing(~droppedFrames,1));
fprintf('Percentage of faces detected out of the total that excludes the dummy=%2.2f\n', 100*sum(headPoses(:,3))/(size(headPoses,1) -n_dummies_detected));



% Plot to show that data is acquired in a linear time:
figure;
[uniqueTimeStamps, iA, iC]= unique(headPoses(:,1));
peopleIDs = unique(headPoses(:,2));
peopleIDs = peopleIDs(2:end);
nPeople = length(peopleIDs);
colours = [0,0,0; jet(nPeople-1)];
for p=1:nPeople,
    currPersonIdx = (headPoses(:,2)==peopleIDs(p));
    plot(iC(currPersonIdx), headPoses(currPersonIdx,1), '-*','color', colours(p,:))
    hold on
end

%%
% Now use annotation to generate clean head poses
% headPose = [time_stamp person_ID good_head x y z  Qw Qx Qy Qz]
headPosesGT = getHeadPoseVector(json2data, detected_as, ~bad_position & ~bad_orientation & is_present);
dummy_idx = headPosesGT(:,2)==0;
good_poseGT = headPosesGT(dummy_idx,3)>0;
headPosesGTa = getHeadPoseVector(json2data, detected_as, ~bad_position & is_present);
good_positionGT = headPosesGTa(dummy_idx,3)>0;

figure;
plot_head_trajectories(headPosesGT)

headPosesGTFileName = [pathstr, filesep, filename, '_headPosesGT.mat'];
if ~exist(headPosesFileName, 'file'),
    save([pathstr, filesep, filename, '_headPosesGT.mat'], 'headPosesGT');
end

fprintf('----------------\nFurther analysis\nSTD of dummy head position in 3D, when annotated as "good position" (in cm):\n');
% Relevant head positions
dummyHeadPositions = headPosesGT(dummy_idx,4:6);
dummyHeadOrientations = headPosesGT(dummy_idx,7:end);

% quaternion2euler converts quaternions to Euler angles in XYZ, i.e., roll pitch and yaw.
% But our coordinate system is ZXY
dummyHeadAnglesTmp = zeros(size(dummyHeadOrientations,1), 3);
for r=1:size(dummyHeadOrientations,1),
    dummyHeadAnglesTmp(r,:) = quaternion2euler(dummyHeadOrientations(r,:), 1);
end
valid_angles = all(~isnan(dummyHeadAnglesTmp),2);
mean_angles_original = mean(dummyHeadAnglesTmp(valid_angles,:));

% Making ungles more readable (and keeping range around zero)
big_angles = abs(dummyHeadAnglesTmp)>90;
dummyHeadAnglesTmp(big_angles) = sign(dummyHeadAnglesTmp(big_angles)).*(180-abs(dummyHeadAnglesTmp(big_angles)));
% "Converting" to yaw, pitch, roll:
dummyHeadAngles = [dummyHeadAnglesTmp(:,2) dummyHeadAnglesTmp(:,1) dummyHeadAnglesTmp(:,3)];


mean_angles = mean(dummyHeadAngles(valid_angles&good_poseGT,:))

% if the head is less than 10cm from the sensor, it's not good.
dist2kinect = sqrt(sum(dummyHeadPositions.^2,2));
not_too_close = dist2kinect>.25;
good_poseGT = good_poseGT & not_too_close & valid_angles; 
good_positionGT = good_positionGT & not_too_close; 

disp('Standard deviation of the pose estimate of the dummy head, in milimeters.')
disp('In good frames')
1000*std(dummyHeadPositions(good_poseGT,:))
mean_good = mean(dummyHeadPositions(good_poseGT,:)); 
disp('In good position frames')
1000*my_std(dummyHeadPositions(good_positionGT,:), mean_good)

disp('Taking all frames into account')
1000*my_std(dummyHeadPositions(not_too_close,:), mean_good)
disp('In bad pose frames')
1000*my_std(dummyHeadPositions((~good_poseGT) & not_too_close,:), mean_good)
disp('In bad position frames')
1000*my_std(dummyHeadPositions((~good_positionGT & ~good_poseGT) & not_too_close,:), mean_good)
disp('Worst distance to mean')
good_mean = repmat(mean_good, size(dummyHeadPositions,1),1);
1000*sqrt(max((dummyHeadPositions-good_mean).^2))

dist2mean = sqrt(sum((dummyHeadPositions-good_mean).^2,2));
disp('Standard deviation of the distance of the dummy head, in milimeters.')

1000*mean(dist2mean(good_poseGT))
1000*mean(dist2mean(good_positionGT))
1000*mean(dist2mean(not_too_close))
1000*mean(dist2mean((~good_poseGT) & not_too_close))
1000*mean(dist2mean((~good_positionGT & ~good_poseGT) & not_too_close))
1000*max(dist2mean)


disp('Standard deviation of the angle estimate of the dummy head, in degrees.')
disp('In good frames')
std(dummyHeadAngles(good_poseGT,:))
mean_good = mean(dummyHeadAngles(good_poseGT,:)); 
disp('In good position frames')
my_std(dummyHeadAngles(good_positionGT&valid_angles,:), mean_good)

disp('Taking all frames into account')
my_std(dummyHeadAngles(not_too_close&valid_angles,:), mean_good)
disp('In bad pose frames')
my_std(dummyHeadAngles((~good_poseGT) & not_too_close & valid_angles,:), mean_good)
disp('In bad position frames')
my_std(dummyHeadAngles((~good_positionGT & ~good_poseGT) & not_too_close & valid_angles,:), mean_good)
disp('Worst distance to mean')
good_mean = repmat(mean_good, size(dummyHeadAngles,1),1);
sqrt(max((dummyHeadAngles-good_mean).^2))
max(abs(dummyHeadAngles-good_mean))

%%
disp('Computing angle changes properly, through quaternions, rather than Euler.')

% Average quaternion:
Qavg = avg_quaternion_markley(dummyHeadOrientations)';
% quaternion2euler(Qavg,1)

% Projected angles, based on this :
% http://math.stackexchange.com/questions/167827/compute-angle-between-quaternions-in-matlab

conjQavg = [Qavg(1), -Qavg(2:end)];
angles = zeros(size(dummyHeadOrientations,1),1);
for r=1:size(dummyHeadOrientations,1)
    z = quaternionproduct(dummyHeadOrientations(r,:),conjQavg);
    angles(r) = 180*acos(z(1))/pi;
end

angles(angles>90) = 180-angles(angles>90);

mean(angles(good_poseGT&valid_angles& not_too_close))
mean(angles(good_positionGT&valid_angles& not_too_close))
mean(angles(valid_angles& not_too_close))
mean(angles(~good_poseGT&valid_angles& not_too_close))
mean(angles(~good_positionGT&~good_poseGT&valid_angles&not_too_close))
max(angles(valid_angles& not_too_close))

%%
disp('Percentage of frames that meet the requirements')
%%
disp('Percentage of frames that meet the requirements')
100*mean(abs(angles(good_poseGT&valid_angles& not_too_close))<=2)
100*mean(abs(angles(good_positionGT&valid_angles& not_too_close))<=2)
100*mean(abs(angles(valid_angles& not_too_close))<=2)
100*mean(abs(angles(~good_poseGT&valid_angles& not_too_close))<=2)
100*mean(abs(angles(~good_positionGT&~good_poseGT&valid_angles&not_too_close))<=2)



%%
disp('Correlation coefficients for position')
corrcoef(dummyHeadPositions(not_too_close,:))
cov(dummyHeadPositions(not_too_close,:))

