% Model Testing
clear all;
close all;
%% 1 - Pick Model to test
disp('Select Model to evaluate')
pickModel = uigetfile;
pickModel = strcat('data/MODELS/',pickModel) ;
load(pickModel);

%% 2 - Gather and concatenate test data
disp('Select Folder containing Test data')
directory_name = uigetdir;
files = dir(directory_name);
fileIndex = find(~[files.isdir]);
core={};

for i = 1:length(fileIndex)
if(i==1)
    fileName = files(fileIndex(i)).name;
    tempName = strcat(directory_name,'/',fileName);  
	tempFile=load(tempName);
    core.emg=[tempFile.f.emgData];  
    core.label=[tempFile.f.label]; 
else
    fileName = files(fileIndex(i)).name;
    tempName = strcat(directory_name,'/',fileName);  
	tempFile=load(tempName);
    core.emg=[core.emg;tempFile.f.emgData]; 
    core.label=[core.label;tempFile.f.label];
end

end

%% 3 - Window Definition  (winStruct)

% Row|             Window 1           - - -    Window n=numTrainSamples
%-------------------------------------------------------------------
%  1  |         EMG   (winsizex8)          |           ...
%--------------------------------------------------------------
%  2  |  Integrated EMG              (1x8) |        ...
%  3  |  Mean Absolute Value         (1x8) |      ...
%  4  |  Simple Squared Integral     (1x8) |    ...
%  5  |  Variance of EMG             (1x8) |  ...
%  6  |  Root Mean Square            (1x8) |  ...
%  7  |  Waveform Length             (1x8) |  ...
%  8  |  Zero Crossing               (1x8) |  ...
%  9  |  Label [1=Rest(cyan); 2=Fist(blue); 3=WaveLeft(green); 4=WaveRight(red); 5=SpreadFingers(Magenta)]
%---------------------------------------------------------

datLength=length(core.emg);
pickModel
prompt = 'Introduce same window size used for training= ';
winSize = input(prompt);

winIncrement=winSize;       % Both values are equal for disjoint windowing [j1]
numTrainSamples=(datLength-winSize)/winIncrement+1;

% if( mod(datLength,winSize)~=0)
%     errordlg('WARNING: change window size to have Integer number of Train Samples')
%     numTrainSamples
% end

winStruct={};
winlabel={};

for w=1:numTrainSamples
   winStruct=[winStruct core.emg(1+winSize*(w-1):winSize*w,:)]; 
   winlabel=[winlabel core.label(1+winSize*(w-1):winSize*w,:)];
end


%% 4 - Data calculations
[winRow,winCol]=size(winStruct{1,1});
IEMG=[];
MAV=[];
SSQ=[];
VAR=[];
RMS=[];
WL=[];
ZC=[];
AR=[];      %Autoregressive Coefficients
featMatrixToPCA=[];
labelVec=[];
for w=1:numTrainSamples
    for i = 1:winCol;
        wIEMG(1,i)=sum(abs(winStruct{1,w}(:,i)));            % Integrated EMG    
        wMAV(1,i)= wIEMG(1,i)/winRow;                        % Mean Absolute Value  
        wSSQ(1,i)=sum((abs(winStruct{1,w}(:,i))).^2);        % Simple Squared Integral
        wVAR(1,i)=sum(winStruct{1,w}(:,i).^2)/(winRow-1);    % Variance of EMG
        wRMS(1,i)=sqrt(sum(winStruct{1,w}(:,i).^2)/(winRow));    % Root Mean Square
             

        wWLtemp=0;
        wZCtemp=0;
        for i2=1:winRow-1
            wWLtemp=wWLtemp+abs(winStruct{1,w}(i2+1,i)-winStruct{1,w}(i2,i));
            wSigntemp=winStruct{1,w}(i2+1,i)*winStruct{1,w}(i2,i);
            if (wSigntemp<0)        % Keep in mind the Threshold instead of 0 to account signal noise
                wZCtemp=wZCtemp+1;
            end 
            
        end
        wWL(1,i)=wWLtemp;                                    % Waveform Length
        wZC(1,i)=wZCtemp;                                    % Zero Crossing
    end
    
%-------------Labeling windows with majority Vote-----------------
        labelcounter=zeros(10,1);
        for lab=1:length(winlabel{w})
            if(winlabel{w}(lab)==1)
                labelcounter(1)=labelcounter(1)+1;
            elseif(winlabel{w}(lab)==2)
                labelcounter(2)=labelcounter(2)+1;
            elseif(winlabel{w}(lab)==3)
                labelcounter(3)=labelcounter(3)+1;
            elseif(winlabel{w}(lab)==4)    
                labelcounter(4)=labelcounter(4)+1;
            elseif(winlabel{w}(lab)==5)
                labelcounter(5)=labelcounter(5)+1;
            elseif(winlabel{w}(lab)==6)
                labelcounter(6)=labelcounter(6)+1;
            elseif(winlabel{w}(lab)==7)
                labelcounter(7)=labelcounter(7)+1;
            elseif(winlabel{w}(lab)==8)
                labelcounter(8)=labelcounter(8)+1;
            elseif(winlabel{w}(lab)==9)
                labelcounter(9)=labelcounter(9)+1;
            elseif(winlabel{w}(lab)==10)
                labelcounter(10)=labelcounter(10)+1;
            end
        end
        [maxlab,indexlab]=max(labelcounter);
%-----------------------------------------------------------------
        
    
    winStruct{2,w}=wIEMG;
    winStruct{3,w}=wMAV;
    winStruct{4,w}=wSSQ;
    winStruct{5,w}=wVAR;
    winStruct{6,w}=wRMS;
    winStruct{7,w}=wWL;
    winStruct{8,w}=wZC;
    winStruct{9,w}=indexlab;
    labelVec=[labelVec;indexlab];
    
    IEMG=[IEMG;wIEMG];
    MAV=[MAV;wMAV];
    SSQ=[SSQ;wSSQ];
    VAR=[VAR;wVAR];
    RMS=[RMS;wRMS];
    WL=[WL;wWL];
    ZC=[ZC;wZC];
    
     featVecToPCA=[];
    for const=1:8
        featVecToPCA=[featVecToPCA wIEMG(const) wSSQ(const) wRMS(const) wWL(const) wZC(const)];
    end 
    featMatrixToPCA=[featMatrixToPCA;featVecToPCA];
    
end

%% 5 - PCA
[coeff,score,latent]=pca(featMatrixToPCA);     %Number of windows reduced

feat1=[];
feat2=[];
feat3=[];
feat4=[];
feat5=[];
feat6=[];
feat7=[];
feat8=[];
feat9=[];
 
[rPca,cPca]=size(score);

% Feature |   Pose   |  Color  |
%---------*----------*---------|
%    1    |   Rest   |  Cyan   |
%    2    |   Fist   |  Blue   |
%    3    |   WavL   |  Green  |
%    4    |   WavR   |   Red   |
%    5    |  Spread  | Magenta |
% Label [1=Rest(cyan); 2=Fist(blue); 3=WaveLeft(green); 4=WaveRight(red); 5=SpreadFingers(Magenta)]

for as=1:rPca
    if(labelVec(as)==1)
        feat1=[feat1;score(as,:)];
    elseif(labelVec(as)==2)
        feat2=[feat2;score(as,:)];
    elseif(labelVec(as)==3)
        feat3=[feat3;score(as,:)];
    elseif(labelVec(as)==4)
        feat4=[feat4;score(as,:)];
    elseif(labelVec(as)==5)
        feat5=[feat5;score(as,:)];
    elseif(labelVec(as)==6)
        feat6=[feat6;score(as,:)];
    elseif(labelVec(as)==7)
        feat7=[feat7;score(as,:)];
    elseif(labelVec(as)==8)
        feat8=[feat8;score(as,:)];        
     elseif(labelVec(as)==9)
        feat9=[feat9;score(as,:)];  
    
    end
    
end

%% 6 - TEST Model


% [measuredKnn, knnScore]= predict(KNN, score);  % KNN
% [measuredKnn, knnScore]= predict(Rtree, score);   % Decision Tree

%------Random Forest
[cellResult, knnScore]= predict(Rtree, score);   % Random Forest
measuredKnn=zeros(size(cellResult));
for aj=1:length(measuredKnn)
    measuredKnn(aj)=cellResult{aj}-48;
end
% %----------------

%------ECOC 

% [cellResult, knnScore]= predict(Mdl, score);  
% measuredKnn=zeros(size(cellResult));
% for aj=1:length(measuredKnn)
%     measuredKnn(aj)=cellResult{aj}-48;
% end
%----------------


fine=0;
f_1=zeros(1,9);
f_2=zeros(1,9);
f_3=zeros(1,9);
f_4=zeros(1,9);
f_5=zeros(1,9);
f_6=zeros(1,9);
f_7=zeros(1,9);
f_8=zeros(1,9);
f_9=zeros(1,9);

f1ok=0;
f2ok=0;
f3ok=0;
f4ok=0;
f5ok=0;
f6ok=0;
f7ok=0;
f8ok=0;
f9ok=0;

for a=1:length(measuredKnn)
    if(measuredKnn(a)==labelVec(a))
        fine=fine+1;
        
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==1)
        f_1(1,measuredKnn(a))=f_1(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==2)
        f_2(1,measuredKnn(a))=f_2(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==3)
        f_3(1,measuredKnn(a))=f_3(1,measuredKnn(a))+1;    
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==4)
        f_4(1,measuredKnn(a))=f_4(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==5)
        f_5(1,measuredKnn(a))=f_5(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==6)
        f_6(1,measuredKnn(a))=f_6(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==7)
        f_7(1,measuredKnn(a))=f_7(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==8)
        f_8(1,measuredKnn(a))=f_8(1,measuredKnn(a))+1;
    elseif(measuredKnn(a)~=labelVec(a)&&labelVec(a)==9)
        f_9(1,measuredKnn(a))=f_9(1,measuredKnn(a))+1;
      
    end
    
    if(measuredKnn(a)==labelVec(a)&&labelVec(a)==1)
        f1ok=f1ok+1;   
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==2)
        f2ok=f2ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==3)
        f3ok=f3ok+1;     
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==4)
        f4ok=f4ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==5)
        f5ok=f5ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==6)
        f6ok=f6ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==7)
        f7ok=f7ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==8)
        f8ok=f8ok+1; 
    elseif(measuredKnn(a)==labelVec(a)&&labelVec(a)==9)
        f9ok=f9ok+1; 
      
    end
end

 f_1=[f_1,sum(f_1)];
% f_2=[f_2,sum(f_2)]
% f_3=[f_3,sum(f_3)]
% f_4=[f_4,sum(f_4)]
% f_5=[f_5,sum(f_5)]
% f_6=[f_6,sum(f_6)]
% f_7=[f_7,sum(f_7)]
% f_8=[f_8,sum(f_8)]
% f_9=[f_9,sum(f_9)]
Accuracy=(fine/length(measuredKnn))*100
AccuracyNOrest=(f2ok+f3ok+f4ok+f5ok+f6ok+f7ok+f8ok+f9ok)/(length(measuredKnn)-f1ok-f_1(10))*100

%% Post processing
% Filtering with a sliding window performing a majority vote of the labels,
% since its not anatomically possible to jump from one pose to another in 1
% or 2 samples.
bestAccuracy=0;
for filtsize=5:40
filterBufferSize=filtsize;
filterBufer=ones(filterBufferSize,1);
filteredModel=ones(size(measuredKnn));
dataLength=length(measuredKnn);
filtwinIncrement=filterBufferSize;       % Both values are equal for disjoint windowing [j1]
numTrainSamples=(dataLength-filterBufferSize)/filtwinIncrement+1;


filtwinlabel={};

for w2=1:numTrainSamples
   filtwinlabel=[filtwinlabel measuredKnn(1+filterBufferSize*(w2-1):filterBufferSize*w2,:)];
end

for w=1:length(filtwinlabel)
labelcounter=zeros(10,1);
        for lab=1:length(filtwinlabel{w})
            if(filtwinlabel{w}(lab)==1)
                labelcounter(1)=labelcounter(1)+1;
            elseif(filtwinlabel{w}(lab)==2)
                labelcounter(2)=labelcounter(2)+1;
            elseif(filtwinlabel{w}(lab)==3)
                labelcounter(3)=labelcounter(3)+1;
            elseif(filtwinlabel{w}(lab)==4)    
                labelcounter(4)=labelcounter(4)+1;
            elseif(filtwinlabel{w}(lab)==5)
                labelcounter(5)=labelcounter(5)+1;
            elseif(filtwinlabel{w}(lab)==6)
                labelcounter(6)=labelcounter(6)+1;
            elseif(filtwinlabel{w}(lab)==7)
                labelcounter(7)=labelcounter(7)+1;
            elseif(filtwinlabel{w}(lab)==8)
                labelcounter(8)=labelcounter(8)+1;
            elseif(filtwinlabel{w}(lab)==9)
                labelcounter(9)=labelcounter(9)+1;
            elseif(filtwinlabel{w}(lab)==10)
                labelcounter(10)=labelcounter(10)+1;
            end
        end
        [maxlab,indexlab]=max(labelcounter);
        filteredModel(1+filterBufferSize*(w-1):filterBufferSize*w,:)=filterBufer*indexlab;
end



%--------------------------------

% New accuracy
fine=0;
f_1=zeros(1,9);
f_2=zeros(1,9);
f_3=zeros(1,9);
f_4=zeros(1,9);
f_5=zeros(1,9);
f_6=zeros(1,9);
f_7=zeros(1,9);
f_8=zeros(1,9);
f_9=zeros(1,9);

f1ok=0;
f2ok=0;
f3ok=0;
f4ok=0;
f5ok=0;
f6ok=0;
f7ok=0;
f8ok=0;
f9ok=0;
for a=1:length(filteredModel)
    if(filteredModel(a)==labelVec(a))
        fine=fine+1;
        
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==1)
        f_1(1,filteredModel(a))=f_1(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==2)
        f_2(1,filteredModel(a))=f_2(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==3)
        f_3(1,filteredModel(a))=f_3(1,filteredModel(a))+1;    
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==4)
        f_4(1,filteredModel(a))=f_4(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==5)
        f_5(1,filteredModel(a))=f_5(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==6)
        f_6(1,filteredModel(a))=f_6(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==7)
        f_7(1,filteredModel(a))=f_7(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==8)
        f_8(1,filteredModel(a))=f_8(1,filteredModel(a))+1;
    elseif(filteredModel(a)~=labelVec(a)&&labelVec(a)==9)
        f_9(1,filteredModel(a))=f_9(1,filteredModel(a))+1;
      
    end
    
    if(filteredModel(a)==labelVec(a)&&labelVec(a)==1)
        f1ok=f1ok+1;   
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==2)
        f2ok=f2ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==3)
        f3ok=f3ok+1;     
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==4)
        f4ok=f4ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==5)
        f5ok=f5ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==6)
        f6ok=f6ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==7)
        f7ok=f7ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==8)
        f8ok=f8ok+1; 
    elseif(filteredModel(a)==labelVec(a)&&labelVec(a)==9)
        f9ok=f9ok+1; 
      
    end
end
% 
 f_1=[f_1,sum(f_1)];
% f_2=[f_2,sum(f_2)]
% f_3=[f_3,sum(f_3)]
% f_4=[f_4,sum(f_4)]
% f_5=[f_5,sum(f_5)]
% f_6=[f_6,sum(f_6)]
% f_7=[f_7,sum(f_7)]
% f_8=[f_8,sum(f_8)]
% f_9=[f_9,sum(f_9)]
Accuracy_Filtered=(fine/length(filteredModel))*100;
AccuracyNOrest_Filtered=(f2ok+f3ok+f4ok+f5ok+f6ok+f7ok+f8ok+f9ok)/(length(filteredModel)-f1ok-f_1(10))*100;

if(AccuracyNOrest_Filtered>=bestAccuracy)
    bestAccuracy=AccuracyNOrest_Filtered;
    A1F=Accuracy_Filtered;
    bestwindowsize=filtsize;
    bestFilteredModel=filteredModel;
end


end
A1F
bestAccuracy

bestwindowsize
figure
subplot(3,1,1)
bar(labelVec)
xlim([0,length(labelVec)])
subplot(3,1,2)
bar(measuredKnn)
xlim([0,length(labelVec)])
subplot(3,1,3)
bar(filteredModel)
xlim([0,length(labelVec)])


%% 7 - PLOT
%------------- 3D PCA
% figure
% subplot(1,2,1)
% plot3(feat1(:,1),feat1(:,2),feat1(:,3),'c.')
% hold on
% 
% plot3(feat2(:,1),feat2(:,2),feat2(:,3),'b.')
% % 
% plot3(feat3(:,1),feat3(:,2),feat3(:,3),'m.')
% % 
% plot3(feat4(:,1),feat4(:,2),feat4(:,3),'r.')
% % 
% plot3(feat5(:,1),feat5(:,2),feat5(:,3),'g.')
% plot3(feat6(:,1),feat6(:,2),feat6(:,3),'k.')
% plot3(feat7(:,1),feat7(:,2),feat7(:,3),'bx')
% 
% plot3(feat8(:,1),feat8(:,2),feat8(:,3),'kx')
% plot3(feat9(:,1),feat9(:,2),feat9(:,3),'gx')
% grid on
% hold off
% title('Manual Labeling')
% 
% feat1=[];
% feat2=[];
% feat3=[];
% feat4=[];
% feat5=[];
% feat6=[];
% feat7=[];
%  
% [rPca,cPca]=size(score);
% 
% for as=1:rPca
%     if(measuredKnn(as)==1)
%         feat1=[feat1;score(as,:)];
%     elseif(measuredKnn(as)==2)
%         feat2=[feat2;score(as,:)];
%     elseif(measuredKnn(as)==3)
%         feat3=[feat3;score(as,:)];
%     elseif(measuredKnn(as)==4)
%         feat4=[feat4;score(as,:)];
%     elseif(measuredKnn(as)==5)
%         feat5=[feat5;score(as,:)];
%     elseif(measuredKnn(as)==6)
%         feat6=[feat6;score(as,:)];
%     elseif(measuredKnn(as)==7)
%         feat7=[feat7;score(as,:)];
%     elseif(measuredKnn(as)==8)
%         feat8=[feat8;score(as,:)];        
%      elseif(measuredKnn(as)==9)
%         feat9=[feat9;score(as,:)];  
%     
%     end
%     
% end
% subplot(1,2,2)
% plot3(feat1(:,1),feat1(:,2),feat1(:,3),'c.')
% hold on
% 
% plot3(feat2(:,1),feat2(:,2),feat2(:,3),'b.')
% % 
% plot3(feat3(:,1),feat3(:,2),feat3(:,3),'m.')
% % 
% plot3(feat4(:,1),feat4(:,2),feat4(:,3),'r.')
% % 
% plot3(feat5(:,1),feat5(:,2),feat5(:,3),'g.')
% plot3(feat6(:,1),feat6(:,2),feat6(:,3),'k.')
% plot3(feat7(:,1),feat7(:,2),feat7(:,3),'bx')
% 
% plot3(feat8(:,1),feat8(:,2),feat8(:,3),'kx')
% plot3(feat9(:,1),feat9(:,2),feat9(:,3),'gx')
% grid on
% hold off
% title('Model Result')
