function resultsVisualisation_exp2
% Plot mean and standard deviation of the tactile bracelet and sEMG sensors
% for the full activation of each movement (thumb, index and little finger
% flexion, wirst flexion, extension and supination).
% Animation showing the ground truth and the predicted activation level of
% each degree-of-freedom for all levels of intermediate activation and each
% movement. The prediction is loaded from the results files generated by
% MLmethods_multimodal_exp2.m
%
%
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please cite the related publications.
% 
% @article{Jaquier17MDPI,
%   author="Jaquier, N. and Connan, M. and Castellini, C. and Calinon, S.",
%   title="Combining electro- and tactile myography to improve hand and
% 		wrist activity detection in prostheses",
%   year="2017",
% 	journal = "Technologies",
% 	volume = "",
% 	number = "",
% 	article number = "",
% }
% 
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Noémie Jaquier
% 
% This code is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
% 
% This code is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
% 
% You should have received a copy of the GNU General Public License
% along with this code. If not, see <http://www.gnu.org/licenses/>.


%% Parameters
subjectId = 2;

dataStr = sprintf('data/exp2/data_S%d.mat',subjectId);
predictionStr = sprintf('results/MULTIMODALpredictions%d.mat',subjectId);
% predictionStr = sprintf('results/EMGpredictions%d.mat',subjectId);
% predictionStr = sprintf('results/TMGpredictions%d.mat',subjectId);

saveOpt = true;
%% Extract data corresponding to activation indexes for rest and the six movements
load(dataStr);

stim(stim(:,7)== -1,10) = -pow2(6);
stim(stim(:,9)== -1,10) = -pow2(8);

activeIdx = find(stim(:,12)==1);
dof0Idx = find(stim(:,10)==     0 ); dof0ActiveIdx = intersect(activeIdx,dof0Idx);
dof1Idx = find(stim(:,10)==pow2(0)); dof1ActiveIdx = intersect(activeIdx,dof1Idx);
dof3Idx = find(stim(:,10)==pow2(2)); dof3ActiveIdx = intersect(activeIdx,dof3Idx);
dof6Idx = find(stim(:,10)==pow2(5)); dof6ActiveIdx = intersect(activeIdx,dof6Idx);
dof7Idx = find(stim(:,10)==-pow2(6)); dof7ActiveIdx = intersect(activeIdx,dof7Idx);
dof8Idx = find(stim(:,10)==pow2(6)); dof8ActiveIdx = intersect(activeIdx,dof8Idx);
dof9Idx = find(stim(:,10)==-pow2(8)); dof9ActiveIdx = intersect(activeIdx,dof9Idx);

tmgTrainVec = brac([dof0ActiveIdx;dof1ActiveIdx;dof3ActiveIdx;dof6ActiveIdx;dof7ActiveIdx;dof8ActiveIdx;dof9ActiveIdx],:);
emgTrain = emg([dof0ActiveIdx;dof1ActiveIdx;dof3ActiveIdx;dof6ActiveIdx;dof7ActiveIdx;dof8ActiveIdx;dof9ActiveIdx],:);
Y = stim([dof0ActiveIdx;dof1ActiveIdx;dof3ActiveIdx;dof6ActiveIdx;dof7ActiveIdx;dof8ActiveIdx;dof9ActiveIdx],[1;3;6;7;8;9]);

titleT = {'Rest','Thumb flexion','Index flexion','Little flexion',...
	'Wrist flexion','Wrist extension','Wrist supination'};

%% Input data in matrix form
% Tactile bracelet of 8 x 40 sensors
N = size(tmgTrainVec,1);
tmgTrain = zeros(8,40,N);
for i=1:size(tmgTrainVec,1)
	tmgTrain(:,:,i) = reshape(tmgTrainVec(i,:),8,40);
end

%% Indexes corresponding to movement in extracted data
dofIdx{1,1} = 1:length(dof0ActiveIdx);
dofIdx{1,2} = dofIdx{1,1}(end)+1:dofIdx{1,1}(end)+length(dof1ActiveIdx);
dofIdx{1,3} = dofIdx{1,2}(end)+1:dofIdx{1,2}(end)+length(dof3ActiveIdx);
dofIdx{1,4} = dofIdx{1,3}(end)+1:dofIdx{1,3}(end)+length(dof6ActiveIdx);
dofIdx{1,5} = dofIdx{1,4}(end)+1:dofIdx{1,4}(end)+length(dof7ActiveIdx);
dofIdx{1,6} = dofIdx{1,5}(end)+1:dofIdx{1,5}(end)+length(dof8ActiveIdx);
dofIdx{1,7} = dofIdx{1,6}(end)+1:dofIdx{1,6}(end)+length(dof9ActiveIdx);

%% Compute means and standard deviation of classes
meanTMG = cell(1,7);
stdTMG = cell(1,7);
meanEMG = cell(1,7);
stdEMG = cell(1,7);
for i = 1:7
	meanTMG{1,i} = mean(tmgTrain(:,:,dofIdx{1,i}),3);
	stdTMG{1,i} = std(tmgTrain(:,:,dofIdx{1,i}),0,3);
	meanEMG{1,i} = mean(emgTrain(dofIdx{1,i},:));
	stdEMG{1,i} = std(emgTrain(dofIdx{1,i},:));
end

%% Plot the mean and standart deviation of each TMG and sEMG sensor of each movement
ySizeTMG = 1:8;
xSizeTMG = 1:40;
figure('position',[100,200,1600,450]); hold on;
for i=1:7
	% plot TMG data
	subplot(4,7,[i i+7]); hold on;
	surf(xSizeTMG,ySizeTMG,meanTMG{1,i});
	for y = 1:length(ySizeTMG)
		for x = 1:length(xSizeTMG)
			plot3d_errorbars(xSizeTMG(x), ySizeTMG(y), meanTMG{1,i}(y,x), [0,0], [0,0], [stdTMG{1,i}(y,x),stdTMG{1,i}(y,x)]);
		end
	end
	xticks([1 20 40])
	ylim([1 8])
	yticks([1 8])
	zlim([-0.5 2.5])
	colormap jet
	caxis([-.2 1])
	if i == 1
		zlabel('TMG output [V]');
	end
	title(titleT{1,i},'Fontsize',14);
	grid on
	daspect([6 3 1])
	view(-10,21)
	
	% plot sEMG data
	subplot(4,7,i+14); hold on;
	errorbar(meanEMG{1,i},stdEMG{1,i});
	xlim([1,10]);
	ylim([-0.1 3]);
	if i==4
		xlabel('Channels');
	end
	if i == 1
		ylabel('sEMG output [V]');
	end
	grid on
end

if saveOpt
	str = sprintf('graphs/S%d_fullActivationsPlot.png',subjectId);
	print('-dpng',str);
end

%% Video of the prediction vs ground truth
load(predictionStr);
predictionData = yhat_tmgemg_SPD_logSPD; % change this variable if you want to plot another method

% Store ground truth data in a new vector
y = zeros(size(yTest,1),6);
y(:,[1 2 3 5 6]) = yTest(:,[1 3 6 7 9]);
% Separate wrist extension and flexion and store them as positive
% activations
y(y(:,5)<0,4) = -y(y(:,5)<0,5);
y(y(:,5)<0,5) = 0;
% Wrist supination as positive activation
y(:,6) = -y(:,6);

% Store prediction data in a new vector
yhat = zeros(size(predictionData,1),6);
yhat(:,[1 2 3 5 6]) = predictionData(:,[1 3 6 7 9]);
% Separate wrist extension and flexion and store them as positive
% activations
yhat(yhat(:,5)<0,4) = -yhat(yhat(:,5)<0,5);
yhat(yhat(:,5)<0,5) = 0;
% Wrist supination as positive activation
yhat(:,6) = -yhat(:,6);

tmgTest = permute(tmgTest,[2,3,1]);

yT = 1:8;
xT = 1:40;

titleT = {'Thumb flex.','Index flex.','Little flex.',...
	'Wrist flex.','Wrist ext.','Wrist sup.'};

fig = figure('PaperPosition',[0 0 8 8],'position',[450,150,1150,850]);
pause();
n = 0;
for t = 320:3:2000 % show only the first repetition, do not show the first (long) rest part
	n = n+1;
	% Plot TMG data
	subplot(5,6,1:12)
	surf(xT,yT,tmgTest(:,:,t)-mean(tmgTest,3));
	zlim([-.3 .6]);
	set(gca,'Fontsize',14);
	zlabel('TMG','Fontsize',16);
	ylabh = get(gca,'YLabel');
	set(ylabh,'Position',get(ylabh,'Position')+ [8 2 0]);
	
	% Plot sEMG data
	subplot(5,6,13:18)
	plot(emgTest(t,:),'Linewidth',3);
	xlim([1,10]);
	ylim([-0.1,4]);
	set(gca,'Fontsize',14);
	ylabel('sEMG','Fontsize',16);
	
	% Plot the ground truth
	for i = 1:6
		subplot(5,6,18+i); hold on;
		xStim = [0 1 1 0];
		yStim = [0 0 y(t,i) y(t,i)];
		patch(xStim,[0 0 1 1],[1 1 1]);
		patch(xStim,yStim,'g');
		ylim([0 1])
		set(gca,'Fontsize',14);
		set(gca,'xtick',[])
		if i == 1
			ylabel('Ground truth');
		end
	end
	
	% Plot the prediction
	for i = 1:6
		subplot(5,6,24+i); hold on;
		xStim = [0 1 1 0];
		yStim = [0 0 yhat(t,i) yhat(t,i)];
		patch(xStim,[0 0 1 1],[1 1 1]);
		patch(xStim,yStim,[1 .5 0]);
		ylim([0 1])
		set(gca,'Fontsize',14);
		xlabel(titleT{i},'Fontsize',16);
		set(gca,'xtick',[])
		if i == 1
			ylabel('Prediction')
		end
	end
	
	set(gcf, 'PaperUnits', 'centimeters');
	set(gcf, 'PaperPosition', [0 0 27 20]);
	drawnow;
	if saveOpt
		str = sprintf('graphs/videos/videoS%d_%.4d.png',subjectId,n);
		saveas(fig, str);
	end
end

end

function [h]=plot3d_errorbars(x, y, z, ex, ey, ez, varargin)
% Draw a vertical errorbar for each point x, y, z in 3 dimensions.

for i=1:length(x)
	xV = [x(i); x(i)];
	yV = [y(i); y(i)];
	zV = [z(i); z(i)];
	
	xMin = x(i) + ex(i);
	xMax = x(i) - ex(i);
	yMin = y(i) + ey(i);
	yMax = y(i) - ey(i);
	zMin = z(i) + ez(i);
	zMax = z(i) - ez(i);
	
	xB = [xMin, xMax];
	yB = [yMin, yMax];
	zB = [zMin, zMax];
	
	h=plot3(xV, yV, zB, '-k');
	set(h, 'LineWidth', 1);
	h=plot3(xB, yV, zV, '-k');
	set(h, 'LineWidth', 1);
	h=plot3(xV, yB, zV, '-k');
	set(h, 'LineWidth', 1);
end
end