% Data processing, PCA and model training
clear all;
%% 1 - Gather and concatenate training 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

%% 2 - Window Definition (winStruct) and preprocessing

% 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);
prompt = 'Introduce window size= ';
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


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

%% 4 - Model training

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

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

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
%%
%--- Weight vector for reducing "Rest" pose influence
reducedWeight=0.5;
reduFaFa=ones(size(labelVec));
for fix=1:length(labelVec)
    if(labelVec(fix)==1)
        reduFaFa(fix)=reduFaFa(fix)*reducedWeight;
    end
end
%%  6 - Train Model and SAVE

DateString = datestr(now,'yyyy mmmm dd HHMMSS');

% 
% %-------------------KNN
% KNN=fitcknn(score,labelVec,'NumNeighbors',3,'Weights',reduFaFa);
% modelREC = strcat('data/MODELS/',DateString,' - K5NN w', num2str(winSize)) ;
% save(modelREC,'KNN')

%-------------------Random Forest
X=score;
Y=labelVec;
NumTrees=6;
Rtree=TreeBagger(NumTrees,X,Y,'MinLeaf',1);
% Rtree=TreeBagger(NumTrees,X,Y);
modelREC = strcat('data/MODELS/',DateString,' - RTree w', num2str(winSize),' T',num2str(NumTrees)) ;
save(modelREC,'Rtree')

% -----------ECOC 
% t = templateSVM('Standardize',1);
% Mdl = fitcecoc(score,labelVec,'Learners',t,...
%     'ClassNames',{'1','2','3','4','5','8'});
% modelREC = strcat('data/MODELS/',DateString,' - ECOC w', num2str(winSize)) ;
% save(modelREC,'Mdl')




% %% 5 - Graphics
% %------------- 3D PCA
% figure
% grid on
% 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')
% hold off
% 
% %---------- EMG coefficients
% figure
% 
% subplot(3,2,1)
% hold on
% title('Integrated EMG')
% xlabel('Window')
% ylabel('Muscle Activity')
% plot(IEMG(:,1),'-xr')
% plot(IEMG(:,2),'-xb')
% plot(IEMG(:,3),'-xc')
% plot(IEMG(:,4),'-xy')
% plot(IEMG(:,5),'-xm')
% plot(IEMG(:,6),'-xg')
% plot(IEMG(:,7),'-xk')
% plot(IEMG(:,8),'--or')
% %legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% 
% 
% subplot(3,2,2)
% hold on
% title('Simple Squared Integral')
% xlabel('Window')
% ylabel('EMG Energy')
% plot(SSQ(:,1),'-xr')
% plot(SSQ(:,2),'-xb')
% plot(SSQ(:,3),'-xc')
% plot(SSQ(:,4),'-xy')
% plot(SSQ(:,5),'-xm')
% plot(SSQ(:,6),'-xg')
% plot(SSQ(:,7),'-xk')
% plot(SSQ(:,8),'--or')
% %legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% 
% 
% subplot(3,2,3)            
% hold on
% title('Root Mean Square')
% xlabel('Window')
% ylabel('Constant Muscle Force')
% plot(RMS(:,1),'-xr')
% plot(RMS(:,2),'-xb')
% plot(RMS(:,3),'-xc')
% plot(RMS(:,4),'-xy')
% plot(RMS(:,5),'-xm')
% plot(RMS(:,6),'-xg')
% plot(RMS(:,7),'-xk')
% plot(RMS(:,8),'--or')
% %legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% 
% subplot(3,2,4)
% hold on
% title('Waveform Length')
% xlabel('Window')
% ylabel('Cummulative Length')
% plot(WL(:,1),'-xr')
% plot(WL(:,2),'-xb')
% plot(WL(:,3),'-xc')
% plot(WL(:,4),'-xy')
% plot(WL(:,5),'-xm')
% plot(WL(:,6),'-xg')
% plot(WL(:,7),'-xk')
% plot(WL(:,8),'--or')
% %legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% 
% subplot(3,2,5)
% hold on
% title('Zero Crossings')
% xlabel('Window')
% ylabel('Number of ZC')
% plot(ZC(:,1),'-xr')
% plot(ZC(:,2),'-xb')
% plot(ZC(:,3),'-xc')
% plot(ZC(:,4),'-xy')
% plot(ZC(:,5),'-xm')
% plot(ZC(:,6),'-xg')
% plot(ZC(:,7),'-xk')
% plot(ZC(:,8),'--or')
% %legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% 
% subplot(3,2,6)      %-----------MASK Legend
% hold on
% xlabel('Amplitude-related features for EMG analisys     | LEGEND')
% ylim([0 1000])
% plot(ZC(:,1),'-xr')
% plot(ZC(:,2),'-xb')
% plot(ZC(:,3),'-xc')
% plot(ZC(:,4),'-xy')
% plot(ZC(:,5),'-xm')
% plot(ZC(:,6),'-xg')
% plot(ZC(:,7),'-xk')
% plot(ZC(:,8),'--or')
% legend('Pod 1','Pod 2','Pod 3','Pod 4','Pod 5','Pod 6','Pod 7','Pod 8')
% hold off
% %----------------------------------------EMG RAW DATA
% figure
% restplot=zeros(size(core.emg));
% fistplot=zeros(size(core.emg));
% WLplot=zeros(size(core.emg));
% WRplot=zeros(size(core.emg));
% spreadplot=zeros(size(core.emg));
% pinchplot=zeros(size(core.emg));
% RLplot=zeros(size(core.emg));
% RRplot=zeros(size(core.emg));
% Tplot=zeros(size(core.emg));
% deadplot=zeros(size(core.emg));
% 
% % Feature |   Pose   |  Color  |
% %---------*----------*---------|
% %    1    |   Rest   |  Cyan   |
% %    2    |   Fist   |  Blue   |
% %    3    |   WavL   | Magenta |
% %    4    |   WavR   |   Red   |
% %    5    |  Spread  |  Green  |
% %    6    |  Pinch   |  Green  |
% %    7    |  RotL    |  Green  |
% %    8    |  RotR    |  Green  |
% %    9    |  Thumb  |  Green  |
% %   10    |Corrupted |  Green  |
% for gr=1:length(core.emg)
%     if(core.label(gr)==1)
%         restplot(gr,:)=core.emg(gr,:);   
%     elseif(core.label(gr)==2)
%         fistplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==3)
%         WLplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==4)    
%         WRplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==5)
%         spreadplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==6)
%         pinchplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==7)    
%         RLplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==8)    
%         RRplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==9)    
%         Tplot(gr,:)=core.emg(gr,:);
%     elseif(core.label(gr)==10)    
%         deadplot(gr,:)=core.emg(gr,:);
%     else 
%        errordlg('ERROR: Unexpected label') 
%     end
%     
%     
%     
% end
% 
% for pl=1:8
%   
%   str=sprintf('POD  %d',pl); 
%   subplot(4,2,pl)
%   hold on
%   if(sum(sum(restplot))~=0)
%     plot(restplot(:,pl),'c')
%   end
%   if(sum(sum(fistplot))~=0)
%     plot(fistplot(:,pl),'b')
%   end
%   if(sum(sum(WLplot))~=0)
%     plot(WLplot(:,pl),'m')
%   end
%   if(sum(sum(WRplot))~=0)
%     plot(WRplot(:,pl),'r')
%   end
%   if(sum(sum(spreadplot))~=0)
%     plot(spreadplot(:,pl),'g') 
%   end
%   if(sum(sum(pinchplot))~=0)
%     plot(pinchplot(:,pl),'-xc') 
%   end
%   if(sum(sum(RLplot))~=0)
%     plot(RLplot(:,pl),'-xb') 
%   end
%   if(sum(sum(RRplot))~=0)
%     plot(RRplot(:,pl),'-xm') 
%   end
%   if(sum(sum(Tplot))~=0)
%     plot(Tplot(:,pl),'-xk') 
%   end
%   if(sum(sum(deadplot))~=0)
%     plot(deadplot(:,pl),'k') 
%   end
%   
%   hold off
%   title(str)
%     
% end
% 


