clc 
clear all

warning off

%% Input Subject Information 
%Subject Number and Paretic arm in order to collect correct sheet with rawdata

prompt = {'Subject Number:','Affected Arm (1) for L, (2) for R', ...
    'Severity of Paresis (1) for mild, (2) for severe'};
dlg_title = 'Input';
num_lines = 1;
defaultans = {'S01','2','1'};
answer = inputdlg(prompt,dlg_title,num_lines,defaultans);    
arm = str2double(answer(2,1));
severity = str2double(answer(3,1));

%% Control Baseline Movement
%n = 10 movements by the control subject, same arm as the subject
%Currently not controlling for dominance

if arm == 1
    [NUM] = xlsread('S00_SRT', 'Left');
    [NUMR] = xlsread('S00_SRT_Rapid','Left');
else
    [NUM] = xlsread('S00_SRT','Right');
    [NUMR] = xlsread('S00_SRT_Rapid','Right');
end

[PKS,LOCS]=findpeaks(NUM(:,3),'MinPeakProminence',300);

FrC = [LOCS(1) LOCS(2)-LOCS(1) LOCS(3)-LOCS(2) LOCS(4)-LOCS(3) ...
    LOCS(5)-LOCS(4) LOCS(6)-LOCS(5) LOCS(7)-LOCS(6) LOCS(8)-LOCS(7) ...
     LOCS(9)-LOCS(8) LOCS(10)-LOCS(9)];
Avg_TC = mean(FrC)*(10/3);
DimC = round(mean(FrC));

% PKS are the max y values signifying completion of reach movement
% LOCS are the location in time of the max y values

CON{1} = zeros(max(FrC),4); 
CON{1}(1:LOCS(1),1:size(NUM,2)) = NUM(1:LOCS(1),1:size(NUM,2));
for i = 2:size(LOCS,1)-1
    CON{i} = zeros(max(FrC),4); 
    CON{i}(1:LOCS(i)-LOCS(i-1)+1,1:size(NUM,2)) = NUM(LOCS(i-1):LOCS(i),1:size(NUM,2));
end
CON{10} = zeros(max(FrC),4); 
CON{10}(1:LOCS(10)-LOCS(9),1:size(NUM,2)) = NUM(LOCS(9):LOCS(10)-1,1:size(NUM,2));

for i = 1:10
    CON{i}(1:50,:) = [];
end

if arm == 1
    CON{4}(end,:) = [];
end

StackC = cat(3,CON{1},CON{2},CON{3},CON{4},CON{5},CON{6},CON{7},CON{8},CON{9},CON{10});

% Stacked matrix containing each full reach in the z-dimension

% figure(1)
% plot(NUM(:,1),NUM(:,3),'b*')
% hold on
% plot(CON{1}(:,1),CON{1}(:,3),'ro')
% plot(CON{4}(:,1),CON{4}(:,3),'ko')
% hold off

StackC(StackC == 0) = NaN;
MMC = nanmean(StackC,3);
SDMC = std(StackC,[],3);

XC = [MMC(:,2)]; % Averaged reach curve, x-coordinate
YC = [MMC(:,3)]; % Averaged reach curve, y-coordinate
ZC = [MMC(:,4)]; % Averaged reach curve, z-coordinate

FrameC = linspace(0,Avg_TC,length(XC));

clear PKS LOCS

% When using Control movement, use XC, YC, and ZC for the averaged reach,
% and individual slices of the StackC concatenated matrix for each
% individual reaching curve

%% Control Rapid Movement

%Use with Right Arm Data

% [PKS,LOCS]=findpeaks(NUMR(:,3),'MinPeakProminence',300);
% 
% FrCR = [LOCS(1) LOCS(2)-LOCS(1) LOCS(3)-LOCS(2) LOCS(4)-LOCS(3) ...
%     LOCS(5)-LOCS(4) LOCS(6)-LOCS(5) LOCS(7)-LOCS(6) LOCS(8)-LOCS(7) ...
%      LOCS(9)-LOCS(8) LOCS(10)-LOCS(9)];
% Avg_TCR = mean(FrCR)*(10/3);
% DimCR = round(mean(FrCR));
% 
% CONR{1} = zeros(max(FrCR),4); 
% CONR{1}(1:LOCS(1),1:size(NUMR,2)) = NUMR(1:LOCS(1),1:size(NUMR,2));
% for i = 2:size(LOCS,1)-1
%     CONR{i} = zeros(max(FrCR),4); 
%     CONR{i}(1:LOCS(i)-LOCS(i-1)+1,1:size(NUMR,2)) = NUMR(LOCS(i-1):LOCS(i),1:size(NUMR,2));
% end
% CONR{10} = zeros(max(FrCR),4); 
% CONR{10}(1:LOCS(10)-LOCS(9),1:size(NUMR,2)) = NUMR(LOCS(9):LOCS(10)-1,1:size(NUMR,2));
% 
% CONR{7}(end,:)=[];
% 
% StackCR = cat(3,CONR{1},CONR{2},CONR{3},CONR{4},CONR{5},...
%     CONR{6},CONR{7},CONR{8},CONR{9},CONR{10});
% 
% % figure(2)
% % plot(NUMR(:,1),NUMR(:,3),'b*')
% % hold on
% % plot(CONR{1}(:,1),CONR{1}(:,3),'ro')
% % plot(CONR{4}(:,1),CONR{4}(:,3),'ko')
% % hold off
% 
% StackCR(StackCR == 0) = NaN;
% MMCR = nanmean(StackCR,3);
% SDMCR = std(StackCR,[],3);
% 
% XCR = [MMCR(:,2)];
% YCR = [MMCR(:,3)];
% ZCR = [MMCR(:,4)];
% 
% FrameCR = linspace(0,Avg_TCR,length(XCR));
% 
% clear PKS LOCS
% 
% % When using Rapid movement, use XCR, YCR, and ZCR for the averaged reach,
% % and individual slices of the StackCR concatenated matrix for each
% % individual reaching curve

%% Extract Subject Reaches

if isfile(char(strcat(answer(1,1),'_SRT_P2.xlsx')))
    [NUM1] = xlsread(char(strcat(answer(1,1),'_SRT_P1')));
    [NUM2] = xlsread(char(strcat(answer(1,1),'_SRT_P2')));
    [NUMS] = [NUM1 ; NUM2];
else
    [NUMS] = xlsread(char(strcat(answer(1,1),'_SRT_P1')));
end


% For S08, S16, S17 replace above with line below
%[NUMS] = xlsread(char(strcat(answer(1,1),'_SRT_P2')));

NUMS(:,1) = linspace(1,size(NUMS,1),size(NUMS,1));

figure(1)
if arm == 1
    plot(NUMS(:,1),NUMS(:,3))
else
    plot(NUMS(:,1),NUMS(:,6))
end

% For S01, use MinPeakProminence of 300, L = 350
% For S02, use MinPeakHeight of 100, L = 350
% For S04, use MinPeakDistance of 160, L = 200
% For S05, use MinPeakHeight of 100, L = 100
% For S06, use MinPeakDistance of 100, L = 200
% For S08, use 'MinPeakHeight',250,'MinPeakDistance',100, L = 250
% For S12, use 'MinPeakHeight',100,'MinPeakProminence',100, L = 200
% For S16, use 'MinPeakHeight' of 150, L = 250
% For S17, use 'MinPeakHeight',150,'MinPeakDistance',1500, L = 170
% For S18, use 'MinPeakHeight',100,'MinPeakProminence',100, L = 200
% For S21, use 'MinPeakHeight',100, 'MinPeakProminence',150, L = 170
% For S24, use 'MinPeakDistance',2000, L = 300;
% For S27, use 'MinPeakHeight', 100, 'MinPeakDistance',1000, L = 150
% For S28, use 'MinPeakHeight', 100, L = 250
% For S29, use 'MinPeakHeight', 20, 'MinPeakDistance',2000, L = 80
% For S30, use 'MinPeakHeight', 20, 'MinPeakDistance',2000, L = 100
% For S31, use 'MinPeakHeight', 20, 'MinPeakDistance',300, L = 50
% For S33, use 'MinPeakHeight', 100, 'MinPeakDistance',200, L = 200
% For S34, use 'MinPeakHeight', 150, 'MinPeakDistance',500, L = 250
% For S36, use 'MinPeakHeight', 150, 'MinPeakDistance',500, L = 350
% For S38, use 'MinPeakHeight', 100, 'MinPeakDistance',500, L = 250
% For S40, use 'MinPeakHeight', 250, 'MinPeakDistance',500, L = 350
% For S41, use 'MinPeakHeight', 200, 'MinPeakDistance',500, L = 250
% For S42, use 'MinPeakHeight', 200, 'MinPeakDistance',500, L = 250
% For S46, use 'MinPeakHeight', 200, 'MinPeakDistance',500, L = 250
% For S47, use 'MinPeakHeight', 200, 'MinPeakDistance',500, L = 300
% For S48, use 'MinPeakHeight',30,'MinPeakDistance',1500,'MinPeakProminence',10, L = 200
% For S51, use 'MinPeakHeight', 50, 'MinPeakDistance',2000, L = 150
% For S53, use 'MinPeakHeight', 20, 'MinPeakDistance',2000, L = 100

L = 100;
maxreach = 84.32;

if arm == 1   
    [PKS,LOCS]=findpeaks(NUMS(:,3), 'MinPeakHeight', 100);
else
    [PKS,LOCS]=findpeaks(NUMS(:,6), 'MinPeakHeight', 100);
end

Peaks = size(LOCS,1);FrS = [LOCS(1)];

for i = 2:Peaks
    itt = [LOCS(Peaks)-LOCS(Peaks - 1)];
    FrS = [FrS itt];
end

Avg_TS = mean(FrS)*(10/3);
DimS = round(mean(FrS));

% PKS are the max y values signifying completion of reach movement
% LOCS are the location in time of the max y values

figure(2)
plot(NUMS(:,1),NUMS(:,3),'b-')
hold on
plot(LOCS,PKS,'r*')
hold off

if arm == 1
    for i = 1:size(LOCS,1)
        SUB{i} = zeros(max(FrS),4);
        SUB{i}(1:L,1) = NUMS(((LOCS(i)-(L-1)):LOCS(i)),1);
        SUB{i}(1:L,2:4) = NUMS(((LOCS(i)-(L-1)):LOCS(i)),2:4);
    end
else
    for i = 1:size(LOCS,1)
        SUB{i} = zeros(max(FrS),4);
        SUB{i}(1:L,1) = NUMS(((LOCS(i)-(L-1)):LOCS(i)),1);
        SUB{i}(1:L,2:4) = NUMS(((LOCS(i)-(L-1)):LOCS(i)),5:7);
    end
end

StackS = cat(3,SUB{1:size(LOCS,1)});

% Stacked matrix containing each full reach in the z-dimension

% figure(2)
% plot(NUMS(:,1),NUMS(:,6),'b-')
% hold on
% plot(SUB{1}(:,1),SUB{1}(:,3),'ro')
% plot(SUB{9}(:,1),SUB{9}(:,3),'ko')
% hold off

StackS(StackS == 0) = NaN;
MMS = nanmean(StackS,3);
SDMS = std(StackS,[],3);

XS = [MMS(:,2)]; % Averaged reach curve, x-coordinate
YS = [MMS(:,3)]; % Averaged reach curve, y-coordinate
ZS = [MMS(:,4)]; % Averaged reach curve, z-coordinate

FrameS = linspace(0,Avg_TS,length(XS));

for i = 1:Peaks
    error(i) = ((maxreach - PKS(i))/maxreach)*100;
end

meanerror = mean(error); stderror = std(error);

disp('Number of Peaks is')
disp(Peaks)
disp('Target inaccuracy is')
disp(meanerror)
disp('+/-')
disp(stderror)

clear PKS LOCS

%% Orientation Profile

for T = 1:size(StackS,3)
    for i = 1:length(SUB{T})-5
        x1 = SUB{T}(i,2:4)';
        b1 = SUB{T}(i+1,2:4)';
        x2 = SUB{T}(i+2,2:4)';
        b2 = SUB{T}(i+3,2:4)';
        x3 = SUB{T}(i+4,2:4)';
        b3 = SUB{T}(i+5,2:4)';
        
        X = [x1 x2 x3];
        B = [b1 b2 b3];
        A = B * inv(X);
        
        yaw(i) = atan(A(2,1)/A(1,1));
        pitch(i) = -asin(A(3,1));
        roll(i) = atan(A(3,2)/A(3,3));
    end
    Yaw(:,T) = yaw';
    Pitch(:,T) = pitch';
    Roll(:,T)= roll';
end

fc = 50;
fs = 300;
[b,a] = butter(50,fc/(fs/2));
Yaw = filter(b,a,Yaw);
Pitch = filter(b,a,Pitch);
Roll = filter(b,a,Roll);

avgyaw = nanmean(Yaw,2);
avgyaw(~any(~isnan(avgyaw), 2),:) = 0;
%avgyaw(~any(~isnan(avgyaw), 2),:)=[];
avgpitch = nanmean(Pitch,2);
avgpitch(~any(~isnan(avgpitch), 2),:) = 0;
%avgpitch(~any(~isnan(avgpitch), 2),:)=[];
avgroll = nanmean(Roll,2);
avgroll(~any(~isnan(avgroll), 2),:) = 0;
%avgroll(~any(~isnan(avgroll), 2),:)=[];

YS = 100 * YS./max(YS);

figure(2)
subplot(411)
plot(YS,(-1)*XS,'k')
xlabel('% of Forward Reaching')
ylabel('Displacement (mm)')
xlim([0 100])
legend(char(strcat('N = ',{' '}, num2str(size(StackS,3)))))

if severity == 1
    if arm == 1
        title(char(strcat(answer(1,1),{' '},'averaged',{' '},...
        'left reach path,',{' '},'paretic left arm and',{' '},'mild impairment')))
    else
        title(char(strcat(answer(1,1),{' '},'averaged',{' '},...
        'right reach path,',{' '},'paretic right arm and',{' '},'mild impairment')))
    end
else
    if arm == 1
        title(char(strcat(answer(1,1),{' '},'averaged',{' '},...
        'left reach path,',{' '},'paretic left arm and',{' '},'severe impairment')))
    else
        title(char(strcat(answer(1,1),{' '},'averaged',{' '},...
        'right reach path,',{' '},'paretic right arm and',{' '},'severe impairment')))
    end
end

stdyaw = std(Yaw,1,2);
stdyaw(~any(~isnan(stdyaw), 2),:) = 0;
stdpitch = std(Pitch,1,2);
stdpitch(~any(~isnan(stdpitch), 2),:) = 0;
stdroll = std(Roll,1,2);
stdroll(~any(~isnan(stdroll), 2),:) = 0;
    
yup = avgpitch + stdpitch;
yup(~any(~isnan(yup), 2),:) = 0;
ylp = avgpitch - stdpitch;
ylp(~any(~isnan(ylp), 2),:) = 0;
    
yur = avgroll + stdroll;
yur(~any(~isnan(yur), 2),:) = 0;
ylr = avgroll - stdroll;
ylr(~any(~isnan(ylr), 2),:) = 0;
    
yuy = avgyaw + stdyaw;
yuy(~any(~isnan(yuy), 2),:) = 0;
yly = avgyaw - stdyaw;
yly(~any(~isnan(yly), 2),:) = 0;
    
XS(~any(~isnan(XS), 2),:) = 0;
YS(~any(~isnan(YS), 2),:) = 0;
ZS(~any(~isnan(ZS), 2),:) = 0;

if size(YS,1)<size(stdyaw,1)
    subplot(412)
    fill([YS fliplr(YS)],[yuy(1:size(YS,1)) fliplr(yly(1:size(YS,1)))],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS,avgyaw(1:size(YS,1)),'k')
    %plot(YS,yuy(1:size(YS,1)),'k')
    %plot(YS,yly(1:size(YS,1)),'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdyaw(1:size(YS,1),:))
    ylabel('Shoulder ROM')
    xlabel('% of Forward Reaching')
    title('Joint Contribution to Endpoint Orientation During Reaching Task')
    xlim([0 100])
    ylim([-1 1])
    
    subplot(413)
    fill([YS fliplr(YS)],[yup(1:size(YS,1)) fliplr(ylp(1:size(YS,1)))],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS,avgpitch(1:size(YS,1)),'k')
    %plot(YS,yup(1:size(YS,1)),'k')
    %plot(YS,ylp(1:size(YS,1)),'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdpitch(1:size(YS,1),:))
    ylabel('Elbow ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-1 1])
    
    subplot(414)
    
    fill([YS fliplr(YS)],[yur(1:size(YS,1)) fliplr(ylr(1:size(YS,1)))],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS,avgroll(1:size(YS,1)),'k')
    %plot(YS,yur(1:size(YS,1)),'k')
    %plot(YS,ylr(1:size(YS,1)),'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdroll(1:size(YS,1),:))
    ylabel('Wrist ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-1 1])
else
    subplot(412)
    fill([YS(1:size(avgyaw,1)) fliplr(YS(1:size(avgyaw,1)))],[yuy fliplr(yly)],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS(1:size(avgyaw,1)),avgyaw,'k')
    %plot(YS(1:size(avgyaw,1)),yuy,'k')
    %plot(YS(1:size(avgyaw,1)),yly,'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdyaw(1:size(YS,1),:))
    ylabel('Shoulder ROM')
    xlabel('% of Forward Reaching')
    title('Joint Contribution to Endpoint Orientation During Reaching Task')
    xlim([0 100])
    ylim([-2 2])
    
    subplot(413)
    fill([YS(1:size(avgpitch,1)) fliplr(YS(1:size(avgpitch,1)))],[yup fliplr(ylp)],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS(1:size(avgpitch,1)),avgpitch,'k')
    %plot(YS(1:size(avgpitch,1)),yup,'k')
    %plot(YS(1:size(avgpitch,1)),ylp,'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdpitch(1:size(YS,1),:))
    ylabel('Elbow ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-2 2])
    
    subplot(414)
    
    fill([YS(1:size(avgroll,1)) fliplr(YS(1:size(avgroll,1)))],[yur fliplr(ylr)],[.8 .9 1], 'linestyle', 'none')
    hold on
    plot(YS(1:size(avgroll,1)),avgroll,'k')
    %plot(YS(1:size(avgroll,1)),yur,'k')
    %plot(YS(1:size(avgroll,1)),ylr,'k')
    hold off
    %plot(YS(1:size(YS,1),:),stdroll(1:size(YS,1),:))
    ylabel('Wrist ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-2 2]) 
end

%print(gcf, 'temp.pdf', '-dpdf')

%% Velocity Profiles
AC = [];
A = [];

FrameC = 100 * FrameC./max(FrameC);
Frame = linspace(0,L,L);
Frame = 100 * Frame./max(Frame);

for i = 1:size(StackS,3)
    A(:,i,1) = SUB{i}(1:L,2); %The x-coordinate in each reach
    A(:,i,2) = SUB{i}(1:L,3); %The y-coordinate in each reach
    A(:,i,3) = SUB{i}(1:L,4); %The z-coordinate in each reach
end

for i = 1:size(StackC,3)
    AC(:,i,1) = CON{i}(:,2); %The x-coordinate in each reach
    AC(:,i,2) = CON{i}(:,3); %The y-coordinate in each reach
    AC(:,i,3) = CON{i}(:,4); %The z-coordinate in each reach
end

AC(isnan(AC)) = 0;
A(isnan(A)) = 0;

for d = 1:size(AC,3)
    for c = 1:size(AC,2)
        for r = 2:size(AC,1)
            if AC(r,c,d) == 0
                AC(r,c,d) = AC(r-1,c,d);
            end
        end
    end
end

for d = 1:size(A,3)
    for c = 1:size(A,2)
        for r = 2:size(A,1)
            if A(r,c,d) == 0
                A(r,c,d) = A(r-1,c,d);
            end
        end
    end
end

ACvel = [];
Avel = [];

for k = 1:3
    for j = 1:size(StackC,3)
        ACvel(:,j,k) = pos2vel(AC(:,j,k),300);
    end
end

for k = 1:3
    for j = 1:size(StackS,3)
        Avel(:,j,k) = pos2vel(A(:,j,k),300);
    end
end

AvgACvel = nanmean(ACvel,2);
AvgAvel = nanmean(Avel,2);
stdACvel = std(ACvel,0,2);
stdAvel = std(Avel,0,2);

yuC = (AvgACvel(:,:,2) + stdACvel(:,:,2))';
ylC = (AvgACvel(:,:,2) - stdACvel(:,:,2))';
yu = (AvgAvel(:,:,2) + stdAvel(:,:,2))';
yl = (AvgAvel(:,:,2) - stdAvel(:,:,2))';

[MC,IC] = max(AvgACvel(:,:,2));
[MS,IS] = max(AvgAvel(:,:,2));
IC = 100 * IC/(size(AvgACvel,1));
IS = 100 * IS/(size(AvgAvel,1));


figure(3)
subplot(211)
fill([FrameC fliplr(FrameC)],[yuC fliplr(ylC)],[.8 .9 1], 'linestyle', 'none')
hold on
plot(FrameC,AvgACvel(:,:,2),'k')
xline(IC,'r.','LineWidth',1.5)
%plot(FrameC,(AvgACvel(:,:,2) + stdACvel(:,:,2)),'k.')
%plot(FrameC,(AvgACvel(:,:,2) - stdACvel(:,:,2)),'k.')
hold off
title('Control Velocity Profile for Forward Displacement')
xlabel('% of Reaching Movement')
ylabel('Velocity dy/dt (mm/s)')
xlim([0 100])

subplot(212)
fill([Frame fliplr(Frame)],[yu fliplr(yl)],[.8 .9 1], 'linestyle', 'none')
hold on
plot(Frame,AvgAvel(:,:,2),'k')
xline(IS,'r.','LineWidth',1.5)
%plot(FrameS,(AvgAvel(:,:,2) + stdAvel(:,:,2)),'k.')
%plot(FrameS,(AvgAvel(:,:,2) - stdAvel(:,:,2)),'k.')
hold off
title('Subject Velocity Profile for Forward Displacement')
xlabel('% of Reaching Movement')
ylabel('Velocity dy/dt (mm/s)')
xlim([0 100])

%% Procrustes Method

% Sliding Procrustes - 
% Compare each subject curve against control baseline curve to receive
% dissimilarity scores. This will serve the basis for identifying which
% section of the subject curves consistently show similar movement to the
% control while accomodating for fatigue 

for t = 1:size(StackS,3)
    Xaxis(:,t) = SUB{t}(1:L,2);
    Yaxis(:,t) = SUB{t}(1:L,3);
    Zaxis(:,t) = SUB{t}(1:L,4);
end

for t = 1:size(StackC,3)
    Xaxis0(:,t) = CON{t}(:,2);
    Yaxis0(:,t) = CON{t}(:,3);
    Zaxis0(:,t) = CON{t}(:,4);
end

Xaxis = nanmean(Xaxis,2);Xaxis = Xaxis - (Xaxis(1));Xaxis(~any(~isnan(Xaxis), 2),:)=[];
Yaxis = nanmean(Yaxis,2);Yaxis = Yaxis - (Yaxis(1));Yaxis(~any(~isnan(Yaxis), 2),:)=[];
Zaxis = nanmean(Zaxis,2);Zaxis = Zaxis - (Zaxis(1));Zaxis(~any(~isnan(Zaxis), 2),:)=[];

Xaxis0 = mean(Xaxis0,2);Xaxis0 = Xaxis0 - (Xaxis0(1));Xaxis0(~any(~isnan(Xaxis0), 2),:)=[];
Yaxis0 = mean(Yaxis0,2);Yaxis0 = Yaxis0 - (Yaxis0(1));Yaxis0(~any(~isnan(Yaxis0), 2),:)=[];
Zaxis0 = mean(Zaxis0,2);Zaxis0 = Zaxis0 - (Zaxis0(1));Zaxis0(~any(~isnan(Zaxis0), 2),:)=[];

Yaxis = 100 * Yaxis./max(Yaxis);
Yaxis0 = 100 * Yaxis0./max(Yaxis0);


S = [Xaxis Yaxis Zaxis];
% For S08, replace above with line below:
%S = [Xaxis(1:100) Yaxis Zaxis(1:100)];
% For S12, replace above with line below:
%S = [Xaxis(1:155) Yaxis Zaxis(1:155)];
% For S17, replace above with line below:
%S = [Xaxis Yaxis(1:82) Zaxis(1:82)];
C = [Xaxis0 Yaxis0 Zaxis0];

figure(4)
subplot(122)
plot3(S(:,1),S(:,2),S(:,3),'r')
hold on
plot3(C(:,1),C(:,2),C(:,3),'b')
%ylabel('% of Forward Reaching')
xlabel('Lateral Displacement (mm)')
zlabel('Vertical Displacement (mm)')

if severity == 1
    if arm == 1
        sgtitle(char(strcat(answer(1,1),{' '},'and Control',{' '},...
        'left reach path,',{' '},'paretic left arm and',{' '},'mild impairment')))
    else
        sgtitle(char(strcat(answer(1,1),{' '},'and Control',{' '},...
        'right reach path,',{' '},'paretic right arm and',{' '},'mild impairment')))
    end
else
    if arm == 1
        sgtitle(char(strcat(answer(1,1),{' '},'and Control',{' '},...
        'left reach path,',{' '},'paretic left arm and',{' '},'severe impairment')))
    else
        sgtitle(char(strcat(answer(1,1),{' '},'and Control',{' '},...
        'right reach path,',{' '},'paretic right arm and',{' '},'severe impairment')))
    end
end

Slider_Size = 35;
i = 1;
dIndex = [];

for s_counter = 1:(size(S,1)-Slider_Size)
    Sub = S(s_counter:s_counter+Slider_Size,:);
    for c_counter = 1:(size(C,1)-Slider_Size)
        Con = C(c_counter:c_counter+Slider_Size,:);
        [d Z] = procrustes(Sub,Con);
        dIndex(i,:) = [s_counter c_counter d];
        i = i + 1;
    end
end

% Sig_dI = dIndex( dIndex(:,3)<0.005, :);
Sig_dI = sortrows(dIndex,3);
Sig_dI = Sig_dI(1:10,:);
Sig_dI = sortrows(Sig_dI,2);
Sig_dI = Sig_dI(1:5,:);
Sig_dI = sortrows(Sig_dI,1);
Sig_dI = Sig_dI(1,:);
d1 = round(Sig_dI(3),4);

Subj = S(Sig_dI(1):(Sig_dI(1)+Slider_Size),:);
Cont = C(Sig_dI(2):(Sig_dI(2)+Slider_Size),:);

plot3(Subj(:,1),Subj(:,2),Subj(:,3),'r*')
plot3(Cont(:,1),Cont(:,2),Cont(:,3),'b*')
%legend('Subject','Control')
axis([-150 50 0 100 0 200])
plot(linspace(-300,100),zeros(1,100),'k');
plot(-150*ones(1,100),linspace(0,100),'k');
plot3(-150*ones(1,100),zeros(1,100),linspace(0,200),'k');
hold off
txt1 = strcat('Dissimilarity index =',{' '},num2str(d1));
%view([142 18])
text(-50,100,60,txt1)

subplot(121)

plot(S(:,1),S(:,2),'r')
hold on
plot(C(:,1),C(:,2),'b')
ylabel('% of Forward Reaching')
xlabel('Lateral Displacement (mm)')

% if severity == 1
%     if arm == 1
%         title(char(strcat(answer(1,1),{' '},'and Control',{' '},...
%         'left reach path,',{' '},'paretic left arm and',{' '},'mild impairment')))
%     else
%         title(char(strcat(answer(1,1),{' '},'and Control',{' '},...
%         'right reach path,',{' '},'paretic right arm and',{' '},'mild impairment')))
%     end
% else
%     if arm == 1
%         title(char(strcat(answer(1,1),{' '},'and Control',{' '},...
%         'left reach path,',{' '},'paretic left arm and',{' '},'severe impairment')))
%     else
%         title(char(strcat(answer(1,1),{' '},'and Control',{' '},...
%         'right reach path,',{' '},'paretic right arm and',{' '},'severe impairment')))
%     end
% end

plot(Subj(:,1),Subj(:,2),'r*')
plot(Cont(:,1),Cont(:,2),'b*')
legend('Subject','Control')
axis([-150 50 0 100])
hold off
txt2 = strcat('Dissimilarity index =',{' '},num2str(d1));
text(-50,80,txt1)

S10 = S(1:(size(S,1)/10),:);
S20 = S((size(S,1)/10):2*(size(S,1)/10),:);
S30 = S(2*(size(S,1)/10):3*(size(S,1)/10),:);
S40 = S(3*(size(S,1)/10):4*(size(S,1)/10),:);
S50 = S(4*(size(S,1)/10):5*(size(S,1)/10),:);
S60 = S(5*(size(S,1)/10):6*(size(S,1)/10),:);
S70 = S(6*(size(S,1)/10):7*(size(S,1)/10),:);
S80 = S(7*(size(S,1)/10):8*(size(S,1)/10),:);
S90 = S(8*(size(S,1)/10):9*(size(S,1)/10),:);
S100 = S(9*(size(S,1)/10):end,:);

C10 = C(1:size(S10),:);
C20 = C((size(C,1)/10):(size(C,1)/10)+size(S10,1),:);
C30 = C(2*(size(C,1)/10):2*(size(C,1)/10)+size(S10,1),:);
C40 = C(3*(size(C,1)/10):3*(size(C,1)/10)+size(S10,1),:);
C50 = C(4*(size(C,1)/10):4*(size(C,1)/10)+size(S10,1),:);
C60 = C(5*(size(C,1)/10):5*(size(C,1)/10)+size(S10,1),:);
C70 = C(6*(size(C,1)/10):6*(size(C,1)/10)+size(S10,1),:);
C80 = C(7*(size(C,1)/10):7*(size(C,1)/10)+size(S10,1),:);
C90 = C(8*(size(C,1)/10):8*(size(C,1)/10)+size(S10,1),:);
C100 = C(9*(size(C,1)/10):9*(size(C,1)/10)+size(S10,1),:);

% C10 = C(1:(size(C,1)/10),:);
% C20 = C((size(C,1)/10):2*(size(C,1)/10),:);
% C30 = C(2*(size(C,1)/10):3*(size(C,1)/10),:);
% C40 = C(3*(size(C,1)/10):4*(size(C,1)/10),:);
% C50 = C(4*(size(C,1)/10):5*(size(C,1)/10),:);
% C60 = C(5*(size(C,1)/10):6*(size(C,1)/10),:);
% C70 = C(6*(size(C,1)/10):7*(size(C,1)/10),:);
% C80 = C(7*(size(C,1)/10):8*(size(C,1)/10),:);
% C90 = C(8*(size(C,1)/10):9*(size(C,1)/10),:);
% C100 = C(9*(size(C,1)/10):end,:);
% 
% S10 = S(1:(size(C10)),:);
% S20 = S((2*(size(S,1)/10)-(size(C10))):2*(size(S,1)/10),:);
% S30 = S((3*(size(S,1)/10)-(size(C10))):3*(size(S,1)/10),:);
% S40 = S((4*(size(S,1)/10)-(size(C10))):4*(size(S,1)/10),:);
% S50 = S((5*(size(S,1)/10)-(size(C10))):5*(size(S,1)/10),:);
% S60 = S((6*(size(S,1)/10)-(size(C10))):6*(size(S,1)/10),:);
% S70 = S((7*(size(S,1)/10)-(size(C10))):7*(size(S,1)/10),:);
% S80 = S((8*(size(S,1)/10)-(size(C10))):8*(size(S,1)/10),:);
% S90 = S((9*(size(S,1)/10)-(size(C10))):9*(size(S,1)/10),:);
% S100 = S(end-(size(C10)):end,:);

disp('Dissimilarity Profile is')

D10 = procrustes(S10,C10);
D20 = procrustes(S20,C20);
D30 = procrustes(S30,C30);
D40 = procrustes(S40,C40);
D50 = procrustes(S50,C50);
D60 = procrustes(S60,C60);
D70 = procrustes(S70,C70);
D80 = procrustes(S80,C80);
D90 = procrustes(S90,C90);
D100 = procrustes(S100,C100);

D = [D10 D20 D30 D40 D50 D60 D70 D80 D90 D100]

%Stretching Procrustes - 
%Compare each control curve to each control curve to find breakdown of the
%reach into subsections
%Compare each subject curve to each subject curve to find locations where
%kinematics change significantly to show if subsections are retained, any introduced

%Stretching Procrustes
%Fatigue, other kinematics
%Demonstrate effects of arm dominance
%Demonstrate emergence of synergies and compensation by looking at
%individual joints

%% Outputs

figure(5)
if size(YS,1)<size(stdyaw,1)
    subplot(311)
    hold on
    findpeaks(stdyaw)
    plot(YS,avgyaw(1:size(YS,1)),'k')
    hold off
    ylabel('Shoulder ROM')
    xlabel('% of Forward Reaching')
    title('Incidence of High Deviation in Joint ROM')
    xlim([0 100])
    ylim([-1 1])
    
    subplot(312)
    hold on
    findpeaks(stdpitch)
    plot(YS,avgpitch(1:size(YS,1)),'k')
    hold off
    ylabel('Elbow ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-1 1])
    
    subplot(313)
    hold on
    findpeaks(stdroll)
    plot(YS,avgroll(1:size(YS,1)),'k')
    hold off
    ylabel('Wrist ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-1 1])
else
    subplot(311)
    hold on
    plot(YS(1:size(avgyaw,1)),avgyaw,'k')
    findpeaks(stdyaw)
    hold off
    ylabel('Shoulder ROM')
    xlabel('% of Forward Reaching')
    title('Incidence of High Deviation in Joint ROM')
    xlim([0 100])
    ylim([-2 2])
    
    subplot(312)
    hold on
    findpeaks(stdpitch)
    plot(YS(1:size(avgpitch,1)),avgpitch,'k')
    hold off
    ylabel('Elbow ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-2 2])
    
    subplot(313)
    hold on
    findpeaks(stdroll)
    plot(YS(1:size(avgroll,1)),avgroll,'k')
    hold off
    ylabel('Wrist ROM')
    xlabel('% of Forward Reaching')
    xlim([0 100])
    ylim([-2 2]) 
end

disp('Peak Velocity of Control')
disp(MC)
disp('Peak Velocity of Subject')
disp(MS)
disp('Subject Peak Velocity is % of Control')
disp((MS/MC)*100)

disp('Peak Velocity location')
disp(IS)