% Author:               L.A.H. Schiks
% Affiliation:          Delft University of Technology
% Date:                 16 jan 2021
% Matlab Version:       R2020B
% Required Toolboxes:   DSP System Toolbox, Signal Processing Toolbox

%% Data Import script

close all
clear all
clc

% Some administration
acc_sensitivity     = 6.5;                      % mV/g --> datasheet ADXL377
gyr_sensitivity     = 0.1;                      % mV/g --> datasheet ADXRS649
g                   = 9.81;                     % Gravity acceleration
subjects            = [ 'SUBJ01'                % Participants
                        'SUBJ02'
                        'SUBJ03'
                        'SUBJ04'
                        'SUBJ05'
                        'SUBJ06'
                        'SUBJ07'
                        'SUBJ08'
                        'SUBJ09'
                        'SUBJ10'
                        'SUBJ11'
                        'SUBJ12'
                        'SUBJ13'
                        'SUBJ14'
                        'SUBJ15'
                        'SUBJ16'
                        'SUBJ17'
                        'SUBJ18'
                        'SUBJ19'
                        'SUBJ20'
                        'SUBJ21'
                        'SUBJ22'
                        'SUBJ23'
                        'SUBJ24'
                        'SUBJ25'
                        'SUBJ26'                        
                        'SUBJ27'
                        'SUBJ28'
                        'SUBJ29'];
dirname             = 'testdata\';              % Directory containing experimental data and calibration files
gender              = [0;0;1;0;0;0;0;1;1;0;0;0;1;1;0;0;0;0;0;0;0;1;1;1;0;0;0;0;0];


% Import sensor data 

for i=1:size(subjects,1)
           
    % Load calibration data
    cal_file_date       = xlsread([dirname subjects(i,:) '.csv'],1,'D1'); 
    cal_file_name       = strcat('calibration_',num2str(cal_file_date));
    fprintf('Initializing %s...\n', cal_file_name)
    [cal, labels]       = xlsread([dirname char(cal_file_name) '.csv']);
    labels              = labels(2,1:11);
    cal                 = cal(3:end,1:10);

    % Calculate calibration values and store them in a data structure using label names
    fprintf('Calculate calibration values...\n')
        for j = 1:length(labels)-1                                   % Minus 1 because not applicable to trigger signal
            c.(labels{j}) = mean(cal(100:end-1,j));
        end
    fprintf('Done\n')

    % Load excel data
    fprintf('Initializing %s sensor data...\n', subjects(i,:))

    [data, labels]  = xlsread([dirname subjects(i,:) '.csv']);       % Read excel sensor data and labels
    data            = data(3:end,1:11);
    labels          = labels(2,1:11);

    % Create data structure using label names
        for j = 1:length(labels)
            d.(subjects(i,:)).(labels{j}) = data(:,j);
        end
    
    % Calibrate raw data
    fprintf('Apply calibration values...\n')

        for j = 1:length(labels)-1                                   % Minus 1 because not applicable to trigger signal
            if j==1
                d.(subjects(i,:)).(labels{j}) = deg2rad((d.(subjects(i,:)).(labels{j})-c.(labels{j}))/(gyr_sensitivity/1000));               % [rad/s]
            else
                d.(subjects(i,:)).(labels{j}) = (d.(subjects(i,:)).(labels{j})-c.(labels{j}))/(acc_sensitivity/1000)*g;                      % [m/s^2]
            end
        end
    fprintf('Done\n')

    % Switch sensor orientation to dummy orientation and apply gravity
    w.(subjects(i,:)).gy    = - d.(subjects(i,:)).gy;
    a.(subjects(i,:)).tx    = -(d.(subjects(i,:)).tz)+g; 
    a.(subjects(i,:)).ty    = - d.(subjects(i,:)).ty;
    a.(subjects(i,:)).tz    = - d.(subjects(i,:)).tx;

    a.(subjects(i,:)).cx    =   d.(subjects(i,:)).cy;
    a.(subjects(i,:)).cy    =   d.(subjects(i,:)).cx;
    a.(subjects(i,:)).cz    = -(d.(subjects(i,:)).cz)-g;

    a.(subjects(i,:)).vx    = - d.(subjects(i,:)).vx;
    a.(subjects(i,:)).vy    =   d.(subjects(i,:)).vy;
    a.(subjects(i,:)).vz    = -(d.(subjects(i,:)).vz)-g;

    % Synchronize sensor data with trigger signal of motion capture system
    Fs      = xlsread([dirname subjects(i,:) '.csv'],1,'B1');           % Read sample frequency of the sensor data [Hz]
    dt      = 1/Fs;

    delay   = 0.02;                                                     % Delay between trigger and start of motion capture [s] (settings Qualisys)
    sync    = find(d.(subjects(i,:)).trig<0.1,1) + delay/dt;            % Find synchronization point; the first point at which trigger signal (5V) is under 0.1 volts

        for j = 1:length(labels)-1
            if j==1
                sen.(subjects(i,:)).w.(labels{j}) = w.(subjects(i,:)).gy(sync:end);                 % [rad/s]
            else
                sen.(subjects(i,:)).a.(labels{j}) = a.(subjects(i,:)).(labels{j})(sync:end);        % [m/s2]
            end
        end

    % Create time vector
    t.(subjects(i,:)) = linspace(0,length(sen.(subjects(i,:)).a.vx)/(Fs),length(sen.(subjects(i,:)).a.vx)); % [s]

    % Some administration for filter
    T  = dt*length(t.(subjects(i,:)));      % Obeservation time [s]
    N  = T/dt;                              % Number of samples [-]
    F  = (0:N-1)' / T;                      % Frequency vector [Hz]
    Fc_gyr = 50;                            % Cutoff frequency for gyroscope data [Hz]
    Fc_acc = 500;                           % Cutoff frequency for accelerometer data [Hz]

    % Plot the power spectral density
    
    ACC = fft(sen.(subjects(i,:)).a.vx);  % Fourier transform
    Pyy = ACC.*conj(ACC)/N;

    % Power spectral density plot [SUBJ17]
    if i == 17;
        figure()
        plot(F,mag2db(Pyy),'k')
        hold on
        title(strcat('Power spectral density ', num2str(subjects(i,:))))
        xlabel('Frequency Hz')
        ylabel('Magnitude dB')
        h1=line([Fc_acc,Fc_acc],ylim);
        set(h1,'Color','r','LineStyle','--','LineWidth',2);% Set properties of lines
        set(gca,'XMinorTick','on');
        hold off
        legend('a_{vx} unfiltered','Cutoff frequency @ 500 Hz')
        ylim([-100 150])
        xlim([0 2500])
    end
    
    % Apply low-pass filter on accelerometer and gyroscope signals
    [B,A]=butter(6,(Fc_gyr/(Fs/2)),'low');  %6th order butterworth filter results in
                                            %a 12th order filter
    [D,C]=butter(6,(Fc_acc/(Fs/2)),'low');

        for j = 1:length(labels)-1
            if j==1
                sen.(subjects(i,:)).w.(labels{j})= filtfilt(B,A,sen.(subjects(i,:)).w.(labels{j}));         % [rad/s]
            else
                sen.(subjects(i,:)).a.(labels{j}) = filtfilt(D,C,sen.(subjects(i,:)).a.(labels{j}));        % [m/s2]
            end
        end
end
    
clear ACC GYR Pyy Pgg h1 h2 A B C D Fc_gyr Fc_acc Fc_gyr F N T a acc_sensitivity c cal cal_file_date cal_file_name d data delay g gyr_sensitivity sync w i j


% Import motion tracking data

for i=1:size(subjects,1)
    load ([dirname subjects(i,:)])  %load test data for each subject
end

m.SUBJ01.v = SUBJ01.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ01.c = SUBJ01.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ02.v = SUBJ02.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ02.c = SUBJ02.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ03.v = SUBJ03.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ03.c = SUBJ03.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ04.v = SUBJ04.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ04.c = SUBJ04.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ05.v = SUBJ05.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ05.c = SUBJ05.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ06.v = SUBJ06.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ06.c = SUBJ06.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ07.v = SUBJ07.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ07.c = SUBJ07.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ08.v = SUBJ08.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ08.c = SUBJ08.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ09.v = SUBJ09.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ09.c = SUBJ09.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ10.v = SUBJ10.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ10.c = SUBJ10.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ11.v = SUBJ11.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ11.c = SUBJ11.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ12.v = SUBJ12.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ12.c = SUBJ12.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ13.v = SUBJ13.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ13.c = SUBJ13.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ14.v = SUBJ14.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ14.c = SUBJ14.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ15.v = SUBJ15.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ15.c = SUBJ15.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ16.v = SUBJ16.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ16.c = SUBJ16.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ17.v = SUBJ17.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ17.c = SUBJ17.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ18.v = SUBJ18.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ18.c = SUBJ18.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ19.v = SUBJ19.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ19.c = SUBJ19.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ20.v = SUBJ20.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ20.c = SUBJ20.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ21.v = SUBJ21.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ21.c = SUBJ21.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ22.v = SUBJ22.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ22.c = SUBJ22.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ23.v = SUBJ23.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ23.c = SUBJ23.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ24.v = SUBJ24.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ24.c = SUBJ24.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ25.v = SUBJ25.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ25.c = SUBJ25.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ26.v = SUBJ26.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ26.c = SUBJ26.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ27.v = SUBJ27.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ27.c = SUBJ27.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ28.v = SUBJ28.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ28.c = SUBJ28.Trajectories.Labeled.Data(2,:,:);    % Extract COG data
m.SUBJ29.v = SUBJ29.Trajectories.Labeled.Data(1,:,:);    % Extract vertex data
m.SUBJ29.c = SUBJ29.Trajectories.Labeled.Data(2,:,:);    % Extract COG data

for i=1:size(subjects,1)
    % Store directions separately, reshape and convert from milimeters to meters
    mot.(subjects(i,:)).s.cx = m.(subjects(i,:)).c(1,1,:)/1000; % [m]
    mot.(subjects(i,:)).s.cx = reshape(mot.(subjects(i,:)).s.cx,[1,length(mot.(subjects(i,:)).s.cx)]);
    mot.(subjects(i,:)).s.cy = m.(subjects(i,:)).c(1,2,:)/1000; % [m]
    mot.(subjects(i,:)).s.cy = reshape(mot.(subjects(i,:)).s.cy,[1,length(mot.(subjects(i,:)).s.cy)]);
    mot.(subjects(i,:)).s.cz = m.(subjects(i,:)).c(1,3,:)/1000; % [m]
    mot.(subjects(i,:)).s.cz = reshape(mot.(subjects(i,:)).s.cz,[1,length(mot.(subjects(i,:)).s.cz)]);

    mot.(subjects(i,:)).s.vx = m.(subjects(i,:)).v(1,1,:)/1000; % [m]
    mot.(subjects(i,:)).s.vx = reshape(mot.(subjects(i,:)).s.vx,[1,length(mot.(subjects(i,:)).s.vx)]);
    mot.(subjects(i,:)).s.vy = m.(subjects(i,:)).v(1,2,:)/1000; % [m]
    mot.(subjects(i,:)).s.vy = reshape(mot.(subjects(i,:)).s.vy,[1,length(mot.(subjects(i,:)).s.vy)]);
    mot.(subjects(i,:)).s.vz = m.(subjects(i,:)).v(1,3,:)/1000; % [m]
    mot.(subjects(i,:)).s.vz = reshape(mot.(subjects(i,:)).s.vz,[1,length(mot.(subjects(i,:)).s.vz)]);

    % Resample signals
    Fs_motion = 1000;
    mot.(subjects(i,:)).s.cx = interp(mot.(subjects(i,:)).s.cx,Fs/Fs_motion);
    mot.(subjects(i,:)).s.cy = interp(mot.(subjects(i,:)).s.cy,Fs/Fs_motion);
    mot.(subjects(i,:)).s.cz = interp(mot.(subjects(i,:)).s.cz,Fs/Fs_motion);
    mot.(subjects(i,:)).s.vx = interp(mot.(subjects(i,:)).s.vx,Fs/Fs_motion);
    mot.(subjects(i,:)).s.vy = interp(mot.(subjects(i,:)).s.vy,Fs/Fs_motion);
    mot.(subjects(i,:)).s.vz = interp(mot.(subjects(i,:)).s.vz,Fs/Fs_motion);
    time_motion = linspace(0,length(mot.(subjects(i,:)).s.cx)/(Fs),length(mot.(subjects(i,:)).s.cx)); % [s]
    

    % Some administration for filter
    Fc_mot = 10;

    % Apply low-pass filter
    [B,A]=butter(6,(Fc_mot/(Fs_motion/2)),'low');  %6th order butterworth filter results in
                                                   %a 12th order filter

    mot.(subjects(i,:)).s.cx = filtfilt(B,A,mot.(subjects(i,:)).s.cx);
    mot.(subjects(i,:)).s.cy = filtfilt(B,A,mot.(subjects(i,:)).s.cy);
    mot.(subjects(i,:)).s.cz = filtfilt(B,A,mot.(subjects(i,:)).s.cz);
    mot.(subjects(i,:)).s.vx = filtfilt(B,A,mot.(subjects(i,:)).s.vx);
    mot.(subjects(i,:)).s.vy = filtfilt(B,A,mot.(subjects(i,:)).s.vy);
    mot.(subjects(i,:)).s.vz = filtfilt(B,A,mot.(subjects(i,:)).s.vz);
  
    % Trim sensor data to the length of the motion data
    sen.(subjects(i,:)).a.tx = sen.(subjects(i,:)).a.tx(1:length(mot.(subjects(i,:)).s.cx)); 
    sen.(subjects(i,:)).a.ty = sen.(subjects(i,:)).a.ty(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.tz = sen.(subjects(i,:)).a.tz(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.cx = sen.(subjects(i,:)).a.cx(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.cy = sen.(subjects(i,:)).a.cy(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.cz = sen.(subjects(i,:)).a.cz(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.vx = sen.(subjects(i,:)).a.vx(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.vy = sen.(subjects(i,:)).a.vy(1:length(mot.(subjects(i,:)).s.cx));
    sen.(subjects(i,:)).a.vz = sen.(subjects(i,:)).a.vz(1:length(mot.(subjects(i,:)).s.cx));
    t.(subjects(i,:)) = t.(subjects(i,:))(1:length(mot.(subjects(i,:)).s.cx));

    for k=1:length(mot.(subjects(i,:)).s.vx)
    % Angle of dummy with z-axis (right positive w.r.t. inertial
    % ref frame. Range + and - 180.
        if      (mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))>=0 & (mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))>0 %eerste kwadrant
            theta.(subjects(i,:))(k) = atan((mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))/(mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k)));
        elseif  (mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))>0 & (mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))<=0 %tweede kwadrant
            theta.(subjects(i,:))(k) = -atan((mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))/(mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k)))+(pi()/2);
        elseif  (mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))<=0 & (mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))<0 %derde kwadrant
            theta.(subjects(i,:))(k) = -atan((mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))/(mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k)))-(pi()/2);
        elseif  (mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))<0 & (mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k))>=0 %vierde kwadrant
            theta.(subjects(i,:))(k) = atan((mot.(subjects(i,:)).s.vx(k)-mot.(subjects(i,:)).s.cx(k))/(mot.(subjects(i,:)).s.vz(k)-mot.(subjects(i,:)).s.cz(k)));
        else
            fprintf('Error in calculation of theta\n')
            pause;
        end
    end
    
    % Calculate angular data
    theta.(subjects(i,:))       = -theta.(subjects(i,:)); %Theta in moving B frame is in opposite direction to the inertial N frame (from which theta is derived)
    calc.(subjects(i,:)).w.gy   = gradient(theta.(subjects(i,:)),dt).';
    calc.(subjects(i,:)).alpha  = gradient(calc.(subjects(i,:)).w.gy,dt).';

end

clear A B N F Fc T dirname dt_motion Fs_motion SUBJ01 SUBJ02 SUBJ03 SUBJ04 SUBJ05 SUBJ06 SUBJ07 SUBJ08 SUBJ09 SUBJ10 SUBJ11 SUBJ12 SUBJ13 SUBJ14 SUBJ15 m SUBJ16 SUBJ17 SUBJ18 SUBJ19 SUBJ20 SUBJ21 SUBJ22 SUBJ24 SUBJ25 SUBJ26 SUBJ27 SUBJ28 SUBJ29

% Data analyse
for i=1:size(subjects,1)

 
    % Find maximum vertex acceleration in x-direction
    maximum=[max(sen.(subjects(i,:)).a.vx) min(sen.(subjects(i,:)).a.vx)];
    if abs(maximum(2))>=maximum(1)
        maximum=maximum(2);
    else
        maximum=maximum(1);
    end
    loc=find(sen.(subjects(i,:)).a.vx==maximum);
    
    % Calculate velocities
    mot.(subjects(i,:)).v.cx = gradient(mot.(subjects(i,:)).s.cx,dt);       % [m/s]
    mot.(subjects(i,:)).v.cz = gradient(mot.(subjects(i,:)).s.cz,dt);       % [m/s]
    mot.(subjects(i,:)).v.c_abs = sqrt(mot.(subjects(i,:)).v.cx.^2+mot.(subjects(i,:)).v.cz.^2);    % [m/s]
    v_cog_abs_max(i) = max(mot.(subjects(i,:)).v.c_abs);

    
    mot.(subjects(i,:)).v.vx = gradient(mot.(subjects(i,:)).s.vx,dt);       % [m/s]
    mot.(subjects(i,:)).v.vz = gradient(mot.(subjects(i,:)).s.vz,dt);       % [m/s]
    mot.(subjects(i,:)).v.v_abs = sqrt(mot.(subjects(i,:)).v.cx.^2+mot.(subjects(i,:)).v.cz.^2);    % [m/s]
    
    
    mot.(subjects(i,:)).a.vx = gradient(mot.(subjects(i,:)).v.vx,dt);       % [m/s]
    mot.(subjects(i,:)).a.vz = gradient(mot.(subjects(i,:)).v.vz,dt);       % [m/s]
    
    %Calculate absolute accelerations from motion data
    mot.(subjects(i,:)).a.v_abs = sqrt(mot.(subjects(i,:)).a.vx.^2+mot.(subjects(i,:)).a.vz.^2);
    % and from sensor data
    sen.(subjects(i,:)).a.t_abs = sqrt(sen.(subjects(i,:)).a.tx.^2+sen.(subjects(i,:)).a.tz.^2);    % [m/s]
    sen.(subjects(i,:)).a.c_abs = sqrt(sen.(subjects(i,:)).a.cx.^2+sen.(subjects(i,:)).a.cz.^2);    % [m/s]
    sen.(subjects(i,:)).a.v_abs = sqrt(sen.(subjects(i,:)).a.vx.^2+sen.(subjects(i,:)).a.vz.^2);    % [m/s]
    
    %Calculate maximum value for each participant
       % Calculate a_t_abs max
    if abs(min(sen.(subjects(i,:)).a.t_abs))>max(sen.(subjects(i,:)).a.t_abs)
        acc_t_max(i) = min(sen.(subjects(i,:)).a.t_abs);
    else
        acc_t_max(i) = max(sen.(subjects(i,:)).a.t_abs);
    end
    
    % Calculate a_c_abs max
    if abs(min(sen.(subjects(i,:)).a.c_abs))>max(sen.(subjects(i,:)).a.c_abs)
        acc_c_max(i) = min(sen.(subjects(i,:)).a.c_abs);
    else
        acc_c_max(i) = max(sen.(subjects(i,:)).a.c_abs);
    end
    
    % Calculate a_v_abs max
    if abs(min(sen.(subjects(i,:)).a.v_abs))>max(sen.(subjects(i,:)).a.v_abs)
        acc_v_max(i) = min(sen.(subjects(i,:)).a.v_abs);
    else
        acc_v_max(i) = max(sen.(subjects(i,:)).a.v_abs);
    end
    
    % Calculate omega max
    if abs(min(calc.(subjects(i,:)).w.gy))>max(calc.(subjects(i,:)).w.gy)
        omega_max(i) = min(calc.(subjects(i,:)).w.gy);
    else
        omega_max(i) = max(calc.(subjects(i,:)).w.gy);
    end
    
    % Calculate alpha max
    if abs(min(calc.(subjects(i,:)).alpha))>max(calc.(subjects(i,:)).alpha)
        alpha_max(i) = min(calc.(subjects(i,:)).alpha);
    else
        alpha_max(i) = max(calc.(subjects(i,:)).alpha);
    end
    
    % Calculate vertex tangential velocity max
    if abs(min(mot.(subjects(i,:)).v.vx))>max(mot.(subjects(i,:)).v.vx)
        v_ver_max(i) = min(mot.(subjects(i,:)).v.vx);
    else
        v_ver_max(i) = max(mot.(subjects(i,:)).v.vx);
    end
    
    % Calculate average shake frequency during the trial
    [pks,locs]    = findpeaks(sen.(subjects(i,:)).a.vx,'MinPeakHeight',0.4*max(sen.(subjects(i,:)).a.vx),'MinPeakDistance',100);   % Find other peaks 400
    for n=1:length(locs)-1
        intervals(n)=(locs(n+1)-locs(n))*dt;                            % Calculate the perdiod time of 1 shake cycle
    end
    freq_shake(i,:)=1/mean(intervals);                                  % Calculate average shaking frequency [Hz]
    clear intervals pks locs
end

% Calculate maximum value amongst all participants
    % Calculate a_t_abs max
    if abs(min(acc_t_max))>max(acc_t_max)
        acc_t_max_max = min(acc_t_max);
    else
        acc_t_max_max = max(acc_t_max);
    end
    
    % Calculate a_c_abs max
    if abs(min(acc_c_max))>max(acc_c_max)
        acc_c_max_max = min(acc_c_max);
    else
        acc_c_max_max = max(acc_c_max);
    end
    
    % Calculate a_v_abs max
    if abs(min(acc_t_max))>max(acc_t_max)
        acc_v_max_max = min(acc_v_max);
    else
        acc_v_max_max = max(acc_v_max);
    end
    
    % Calculate omega max
    if abs(min(omega_max))>max(omega_max)
        omega_max_max = min(omega_max);
    else
        omega_max_max = max(omega_max);
    end
    
    % Calculate alpha max
    if abs(min(alpha_max))>max(alpha_max)
        alpha_max_max = min(alpha_max);
    else
        alpha_max_max = max(alpha_max);
    end
    
    % Calculate tang ver vel max
    if abs(min(v_ver_max))>max(v_ver_max)
        v_ver_max_max = min(v_ver_max);
    else
        v_ver_max_max = max(v_ver_max);
    end

fprintf('End of script first part\n')

% Calculate means and std
acc_t_max(end+1)=acc_t_max_max;
acc_t_max(end+1)=mean(abs(acc_t_max(1:end-1)));
acc_t_max(end+1)=std(abs(acc_t_max(1:end-2)));

acc_c_max(end+1)=acc_c_max_max;
acc_c_max(end+1)=mean(abs(acc_c_max(1:end-1)));
acc_c_max(end+1)=std(abs(acc_c_max(1:end-2)));

acc_v_max(end+1)=acc_v_max_max;
acc_v_max(end+1)=mean(abs(acc_v_max(1:end-1)));
acc_v_max(end+1)=std(abs(acc_v_max(1:end-2)));

omega_max(end+1)=omega_max_max;
omega_max(end+1)=mean(abs(omega_max(1:end-1)));
omega_max(end+1)=std(abs(omega_max(1:end-2)));

alpha_max(end+1)=alpha_max_max;
alpha_max(end+1)=mean(abs(alpha_max(1:end-1)));
alpha_max(end+1)=std(abs(alpha_max(1:end-2)));

v_ver_max(end+1)=v_ver_max_max;
v_ver_max(end+1)=mean(abs(v_ver_max(1:end-1)));
v_ver_max(end+1)=std(abs(v_ver_max(1:end-2)));

freq_shake(end+1)= max(freq_shake);
freq_shake(end+1)=mean(freq_shake(1:end-1));
freq_shake(end+1)=std(freq_shake(1:end-2));

extralabels=[cellstr('Maximum')
            cellstr('Mean')
            cellstr('SD')];
rownames = [cellstr(subjects)    
    cellstr(extralabels)];

% % Write calculations and statistics to an Excel file
% fprintf('Maximum values:\n')
% T1= table(rownames); %subject names for excel table
% T = table(acc_t_max.',acc_c_max.',acc_v_max.',omega_max.',alpha_max.',v_ver_max.',freq_shake,'RowNames',rownames);
% T.Properties.VariableNames = {'AccTorABS' 'AccCOGABS' 'AccVerABS' 'Omega' 'Alpha' 'VtangVerMax' 'Fshake'};
% disp(T);
% filename = 'IHI-ST_Calculated_Parameters.xlsx';
% writetable(T1,filename,'Sheet',1,'Range','A1');
% writetable(T,filename,'Sheet',1,'Range','B1');

for k=1:size(subjects,1)
    if gender(k) == 0 %gender is male
        male.acc_t_max(k)=acc_t_max(k);
        male.acc_c_max(k)=acc_c_max(k);
        male.acc_v_max(k)=acc_v_max(k);
        male.omega_max(k)=abs(omega_max(k));
        male.alpha_max(k)=abs(alpha_max(k));
        male.v_ver_max(k)=abs(v_ver_max(k));
        male.freq_shake(k)=freq_shake(k);
        
    else %gender is female
        female.acc_t_max(k)=acc_t_max(k);
        female.acc_c_max(k)=acc_c_max(k);
        female.acc_v_max(k)=acc_v_max(k);
        female.omega_max(k)=abs(omega_max(k));
        female.alpha_max(k)=abs(alpha_max(k));
        female.v_ver_max(k)=abs(v_ver_max(k));
        female.freq_shake(k)=freq_shake(k);
    end
end
%remove zeros
male.acc_t_max(male.acc_t_max==0) = [];
male.acc_c_max(male.acc_c_max==0) = [];
male.acc_v_max(male.acc_v_max==0) = [];
male.omega_max(male.omega_max==0) = [];
male.alpha_max(male.alpha_max==0) = [];
male.v_ver_max(male.v_ver_max==0) = [];
male.freq_shake(male.freq_shake==0) = [];

female.acc_t_max(female.acc_t_max==0) = [];
female.acc_c_max(female.acc_c_max==0) = [];
female.acc_v_max(female.acc_v_max==0) = [];
female.omega_max(female.omega_max==0) = [];
female.alpha_max(female.alpha_max==0) = [];
female.v_ver_max(female.v_ver_max==0) = [];
female.freq_shake(female.freq_shake==0) = [];

clear acc_max  Fs maximum  T1 v_cog_abs_max v_ver_abs_max

%Typical vertex accelerations of the dummy’s head during violent shaking [SUBJ01].
figure()
plot(t.SUBJ01,sen.SUBJ01.a.vx,'k',t.SUBJ01,sen.SUBJ01.a.vy,'r',t.SUBJ01,sen.SUBJ01.a.vz,'g')
xlabel('Time (s)')
ylabel('Acceleration (m/s^2)')
grid on
xlim([14.35 14.7])
ylim([-250 250])
legend('x-direction','y-direction','z-direction')

%% Rotation axis calculation

close all
clc

g=9.81;
for i = 1:size(subjects,1)
    
% Trim trajectory to 1 cycle
[a_xv_max_pos, i_max_pos]   = max(sen.(subjects(i,:)).a.vx);                                                        % Maximum positive a_xv value
[a_xv_max_neg, i_max_neg]   = min(sen.(subjects(i,:)).a.vx);                                                        % Maximum negative a_xv value
[pospks,poslocs]    = findpeaks(sen.(subjects(i,:)).a.vx,'MinPeakHeight',0.3*a_xv_max_pos,'MinPeakDistance',500);   % Find other peaks 400
[negpks,neglocs]    = findpeaks(-sen.(subjects(i,:)).a.vx,'MinPeakHeight',0.3*-a_xv_max_neg,'MinPeakDistance',500); % Find other peaks

    if abs(a_xv_max_neg)>=a_xv_max_pos
        loc(1)  = i_max_neg;                                % Index of first minimum
        xv_max3 = negpks(find(-negpks==a_xv_max_neg)+1);    % Find NEXT minimum                                            
        loc(2)  = neglocs(find(negpks==xv_max3));           % Find index of NEXT minimum (used later on)     
    else
        loc(1)  = i_max_pos;                                % Index of first maximum
        xv_max3 = pospks(find(pospks==a_xv_max_pos)+1);   	% Find NEXT maximum
        loc(2)  = poslocs(find(pospks==xv_max3));           % Find index of NEXT maximum (used later on)
    end

clear a_xv_max_neg a_xv_max_pos i_max_neg i_max_pos pospks poslocs negpks neglocs xv_max3

% When generating data extra=0:
extra=0;

% When generating figures:
% Run script with parameter 'extra=25' to include additional datapoints at 
% the beginning and at the end of a shake cycle 
% to make sure that the 'round ends' are included in the figures.

mot.(subjects(i,:)).s.cx_trim       = mot.(subjects(i,:)).s.cx(loc(1)-extra:loc(2)+extra);
mot.(subjects(i,:)).s.cz_trim       = mot.(subjects(i,:)).s.cz(loc(1)-extra:loc(2)+extra);
mot.(subjects(i,:)).s.vx_trim       = mot.(subjects(i,:)).s.vx(loc(1)-extra:loc(2)+extra);
mot.(subjects(i,:)).s.vz_trim       = mot.(subjects(i,:)).s.vz(loc(1)-extra:loc(2)+extra);
mot.(subjects(i,:)).s.vy_trim       = mot.(subjects(i,:)).s.vy(loc(1)-extra:loc(2)+extra);
t_trim.(subjects(i,:))              = t.(subjects(i,:))(loc(1)-extra:loc(2)+extra);

sen.(subjects(i,:)).a.vx_trim       = sen.(subjects(i,:)).a.vx(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.vz_trim       = sen.(subjects(i,:)).a.vz(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.vy_trim       = sen.(subjects(i,:)).a.vy(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.cx_trim       = sen.(subjects(i,:)).a.cx(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.cz_trim       = sen.(subjects(i,:)).a.cz(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.tx_trim       = sen.(subjects(i,:)).a.tx(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.tz_trim       = sen.(subjects(i,:)).a.tz(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.c_abs_trim    = sen.(subjects(i,:)).a.c_abs(loc(1)-extra:loc(2)+extra);
sen.(subjects(i,:)).a.v_abs_trim    = sen.(subjects(i,:)).a.v_abs(loc(1)-extra:loc(2)+extra);

theta_trim.(subjects(i,:))          = theta.(subjects(i,:))(loc(1)-extra:loc(2)+extra);
calc.(subjects(i,:)).w.gy_trim      = calc.(subjects(i,:)).w.gy(loc(1)-extra:loc(2)+extra);
calc.(subjects(i,:)).alpha_trim     = calc.(subjects(i,:)).alpha(loc(1)-extra:loc(2)+extra).';

clear loc


    % Richtingscoefficienten bepalen raak en loodlijnen (voor ieder punt in de cycle)
    for k=1:length(mot.(subjects(i,:)).s.cx_trim)-1
        slope_cog_tang(k) = (mot.(subjects(i,:)).s.cz_trim(k+1) - mot.(subjects(i,:)).s.cz_trim(k)) / (mot.(subjects(i,:)).s.cx_trim(k+1) - mot.(subjects(i,:)).s.cx_trim(k)); %calculate slope COG tangentline
        slope_ver_tang(k) = (mot.(subjects(i,:)).s.vz_trim(k+1) - mot.(subjects(i,:)).s.vz_trim(k)) / (mot.(subjects(i,:)).s.vx_trim(k+1) - mot.(subjects(i,:)).s.vx_trim(k)); %calculate slope vertex tangentline
        slope_cog_perp(k) = -1/slope_cog_tang(k); %calculate slope COG perpline
        slope_ver_perp(k) = -1/slope_ver_tang(k); %calculate slope vertex perpline
    end
    fprintf('Calculating rotation centers... \n', subjects(i,:))
    % Determine intersection point of perpendicular lines for each point in the cycle
    threshold=1000000; %length setting of perpendicular lines, increase if no intersection was found.
    for k=1:length(slope_cog_tang) 
    
        x_axis_cog_tang = linspace(mot.(subjects(i,:)).s.cx_trim(k)-threshold,mot.(subjects(i,:)).s.cx_trim(k)+threshold);      %calculate an X-axis for COG tangentline
        x_axis_ver_tang = linspace(mot.(subjects(i,:)).s.vx_trim(k)-threshold,mot.(subjects(i,:)).s.vx_trim(k)+threshold);      %calculate an X-axis for vertex tangentline


        b_cog_tang=(mot.(subjects(i,:)).s.cz_trim(k)-(slope_cog_tang(k)*mot.(subjects(i,:)).s.cx_trim(k)));  %calculate b for COG tangentline
        b_ver_tang=(mot.(subjects(i,:)).s.vz_trim(k)-(slope_ver_tang(k)*mot.(subjects(i,:)).s.vx_trim(k)));  %calculate b for vertex tangentline
        b_cog_perp=(mot.(subjects(i,:)).s.cz_trim(k)-(slope_cog_perp(k)*mot.(subjects(i,:)).s.cx_trim(k)));  %calculate b for COG perpline
        b_ver_perp=(mot.(subjects(i,:)).s.vz_trim(k)-(slope_ver_perp(k)*mot.(subjects(i,:)).s.vx_trim(k)));  %calculate b for vertex perpline

        for p=1:length(x_axis_cog_tang)
            z_coordinate_cog_tang(p) = slope_cog_tang(k)*x_axis_cog_tang(p)+b_cog_tang;
            z_coordinate_cog_perp(p) = slope_cog_perp(k)*x_axis_cog_tang(p)+b_cog_perp;
        end
    
        for p=1:length(x_axis_ver_tang)
            z_coordinate_ver_tang(p) = slope_ver_tang(k)*x_axis_ver_tang(p)+b_ver_tang;
            z_coordinate_ver_perp(p) = slope_ver_perp(k)*x_axis_ver_tang(p)+b_ver_perp;
        end

        L1=[x_axis_cog_tang ;z_coordinate_cog_perp];    % Perpendicular line COG
        L2=[x_axis_ver_tang ;z_coordinate_ver_perp];    % Perpendicular line vertex
        P=InterX(L1,L2);                                % Intersection point between two perp lines = instantaneous center of rotation in world coordinates

            if isempty(P)==1 %check whether an intersection point was found. If P is empty, no intersection was found and NaN is assigned.
                fprintf('Error: somewhere at the interval no intersection point was found. Increase threshold!\n')
                fprintf('Current threshold: ')
                disp(threshold)
                pause;
            else
                IAOR.(subjects(i,:)).x(k)      = P(1);
                IAOR.(subjects(i,:)).z(k)      = P(2);
                ROC.(subjects(i,:))(k)         = sqrt((mot.(subjects(i,:)).s.cx_trim(k)-IAOR.(subjects(i,:)).x(k))^2 + (mot.(subjects(i,:)).s.cz_trim(k)-IAOR.(subjects(i,:)).z(k))^2);
            end         
    end
    
    

    for k=1:length(mot.(subjects(i,:)).s.vx_trim)-1
   
    % Vector from o to o prime expressed in
    % moving reference frame coordinates
    r_oox.(subjects(i,:))(k)     = (-cos(theta_trim.(subjects(i,:))(k))*(IAOR.(subjects(i,:)).x(k)))-(sin(theta_trim.(subjects(i,:))(k))*(IAOR.(subjects(i,:)).z(k)));
    r_ooz.(subjects(i,:))(k)     = (-sin(theta_trim.(subjects(i,:))(k))*(IAOR.(subjects(i,:)).x(k)))+(cos(theta_trim.(subjects(i,:))(k))*(IAOR.(subjects(i,:)).z(k)));
    
        
    % Vector from o prime to COG and Vertex expressed in
    % moving reference frame coordinates
    r_ocx.(subjects(i,:))(k)     = (-cos(theta_trim.(subjects(i,:))(k))*(mot.(subjects(i,:)).s.cx_trim(k)-IAOR.(subjects(i,:)).x(k)))-(sin(theta_trim.(subjects(i,:))(k))*(mot.(subjects(i,:)).s.cz_trim(k)-IAOR.(subjects(i,:)).z(k)));
    r_ocz.(subjects(i,:))(k)     = (-sin(theta_trim.(subjects(i,:))(k))*(mot.(subjects(i,:)).s.cx_trim(k)-IAOR.(subjects(i,:)).x(k)))+(cos(theta_trim.(subjects(i,:))(k))*(mot.(subjects(i,:)).s.cz_trim(k)-IAOR.(subjects(i,:)).z(k)));
    end
    
    % Vector from o prime to Vertex expressed in
    % moving reference frame coordinates
    r_ovx.(subjects(i,:))       = r_ocx.(subjects(i,:));
    r_ovz.(subjects(i,:))       = r_ocz.(subjects(i,:))+ 0.05982; %59.82mm offset between sensors COG and VERTEX
    
       
    % Velocity and acceleration vectors from o prime to COG and Vertex expressed in
    % moving reference frame coordinates
    r_ocx.dot.(subjects(i,:))       = gradient(r_ocx.(subjects(i,:)),dt);
    r_ocz.dot.(subjects(i,:))       = gradient(r_ocz.(subjects(i,:)),dt);
    r_ovx.dot.(subjects(i,:))       = gradient(r_ovx.(subjects(i,:)),dt);
    r_ovz.dot.(subjects(i,:))       = gradient(r_ovz.(subjects(i,:)),dt);
    r_ocx.dot.dot.(subjects(i,:))   = gradient(r_ocx.dot.(subjects(i,:)),dt);
    r_ocz.dot.dot.(subjects(i,:))   = gradient(r_ocz.dot.(subjects(i,:)),dt);
    r_ovx.dot.dot.(subjects(i,:))   = gradient(r_ovx.dot.(subjects(i,:)),dt);
    r_ovz.dot.dot.(subjects(i,:))   = gradient(r_ovz.dot.(subjects(i,:)),dt);
    
    r_oox.dot.(subjects(i,:))       = gradient(r_oox.(subjects(i,:)),dt);
    r_ooz.dot.(subjects(i,:))       = gradient(r_ooz.(subjects(i,:)),dt);
    
    % Calculate origin acceleration expressed in moving reference frame
    % coordinates. Calculation based on equations of motion.
    r_oox.dot.dot.(subjects(i,:))=sen.(subjects(i,:)).a.cx_trim(1:end-1) -   (calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ocz.(subjects(i,:)).')  + ((calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ocx.(subjects(i,:)).')- r_ocx.dot.dot.(subjects(i,:)).' -   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ocz.dot.(subjects(i,:)).') -g*sin(theta_trim.(subjects(i,:))(1:end-1)).';
    r_ooz.dot.dot.(subjects(i,:))=sen.(subjects(i,:)).a.cz_trim(1:end-1) +   (calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ocx.(subjects(i,:)).')  + ((calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ocz.(subjects(i,:)).')- r_ocz.dot.dot.(subjects(i,:)).' +   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ocx.dot.(subjects(i,:)).') +g*cos(theta_trim.(subjects(i,:))(1:end-1)).';
    
    % Note: in theory it is also possible to calculate the origin acceleration
    % using the gradient method, but this gives far less accurate results and
    % causes the model to have a realy bad fit.  
    %r_oox.dot.dot.(subjects(i,:))=gradient(r_oox.dot.(subjects(i,:)),dt).';
    %r_ooz.dot.dot.(subjects(i,:))=gradient(r_ooz.dot.(subjects(i,:)),dt).';  
     
     
    % Calculate VER en COG acceleration with equations of motion Moving ICOR
    % Model
    calc.(subjects(i,:)).a.vx = g*sin(theta_trim.(subjects(i,:))(1:end-1)).'+r_oox.dot.dot.(subjects(i,:))+(calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ovz.(subjects(i,:)).')  + (-(calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ovx.(subjects(i,:)).')+ r_ovx.dot.dot.(subjects(i,:)).' +   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ovz.dot.(subjects(i,:)).');
    calc.(subjects(i,:)).a.vz = -g*cos(theta_trim.(subjects(i,:))(1:end-1)).'+r_ooz.dot.dot.(subjects(i,:))-(calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ovx.(subjects(i,:)).')  + (-(calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ovz.(subjects(i,:)).')+ r_ovz.dot.dot.(subjects(i,:)).' -   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ovx.dot.(subjects(i,:)).');
    
    calc.(subjects(i,:)).a.cx = g*sin(theta_trim.(subjects(i,:))(1:end-1)).'+r_oox.dot.dot.(subjects(i,:))+(calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ocz.(subjects(i,:)).')  + (-(calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ocx.(subjects(i,:)).')+ r_ocx.dot.dot.(subjects(i,:)).' +   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ocz.dot.(subjects(i,:)).');
    calc.(subjects(i,:)).a.cz = -g*cos(theta_trim.(subjects(i,:))(1:end-1)).'+r_ooz.dot.dot.(subjects(i,:))-(calc.(subjects(i,:)).alpha_trim(1:end-1).*r_ocx.(subjects(i,:)).')  + (-(calc.(subjects(i,:)).w.gy_trim(1:end-1).^2).*r_ocz.(subjects(i,:)).')+ r_ocz.dot.dot.(subjects(i,:)).' -   ((2*calc.(subjects(i,:)).w.gy_trim(1:end-1)).*r_ocx.dot.(subjects(i,:)).');

    % Calculate RMSE and residual accelerations
    for k=1:length(calc.(subjects(i,:)).a.vx)
        RESIDUAL_VX.(subjects(i,:))(k) = (calc.(subjects(i,:)).a.vx(k) - sen.(subjects(i,:)).a.vx_trim(k));  % Root Mean Squared Error
        RESIDUAL_VZ.(subjects(i,:))(k) = (calc.(subjects(i,:)).a.vz(k) - sen.(subjects(i,:)).a.vz_trim(k));  % Root Mean Squared Error
    end
    RMSE_VX(i,:) = (sqrt(mean((sen.(subjects(i,:)).a.vx_trim(1:end-1) - calc.(subjects(i,:)).a.vx).^2)));
    RMSE_VZ(i,:) = (sqrt(mean((sen.(subjects(i,:)).a.vz_trim(1:end-1) - calc.(subjects(i,:)).a.vz).^2)));
    RMSE_CX(i,:) = (sqrt(mean((sen.(subjects(i,:)).a.cx_trim(1:end-1) - calc.(subjects(i,:)).a.cx).^2)));
    RMSE_CZ(i,:) = (sqrt(mean((sen.(subjects(i,:)).a.cz_trim(1:end-1) - calc.(subjects(i,:)).a.cz).^2)));
    
%Acceleration of the origin
vel_origin_x = gradient(IAOR.(subjects(i,:)).x,dt);
vel_origin_z = gradient(IAOR.(subjects(i,:)).z,dt);

acc.(subjects(i,:)).ox = gradient(vel_origin_x,dt);
acc.(subjects(i,:)).oz = gradient(vel_origin_z,dt);

clear vel_origin_x vel_origin_z

% Compare different model categories
r= 0.05982+0.027+0.04; % [m] distance VER to COG + COG to skull base + skull base to C5-C6
cat1.(subjects(i,:)).a.vx = r*calc.(subjects(i,:)).alpha_trim;
cat1.(subjects(i,:)).a.vz = - calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT1_VX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vx_trim - cat1.(subjects(i,:)).a.vx).^2));
RMSE_CAT1_VZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vz_trim - cat1.(subjects(i,:)).a.vz).^2));
RESIDUAL_CAT1_VX.(subjects(i,:))=(cat1.(subjects(i,:)).a.vx-sen.(subjects(i,:)).a.vx_trim).';
RESIDUAL_CAT1_VZ.(subjects(i,:))=(cat1.(subjects(i,:)).a.vz-sen.(subjects(i,:)).a.vz_trim).';

r= 0.027+0.04; % [m] distance COG to skull base + skull base to C5-C6
cat1.(subjects(i,:)).a.cx = r*calc.(subjects(i,:)).alpha_trim;
cat1.(subjects(i,:)).a.cz = - calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT1_CX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cx_trim - cat1.(subjects(i,:)).a.cx).^2));
RMSE_CAT1_CZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cz_trim - cat1.(subjects(i,:)).a.cz).^2));

cat2.(subjects(i,:)).a.vx = sen.(subjects(i,:)).a.tx_trim;
RMSE_CAT2_VX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vx_trim - cat2.(subjects(i,:)).a.vx).^2));
RESIDUAL_CAT2_VX.(subjects(i,:))=(cat2.(subjects(i,:)).a.vx-sen.(subjects(i,:)).a.vx_trim).';

cat2.(subjects(i,:)).a.cx = sen.(subjects(i,:)).a.tx_trim;
RMSE_CAT2_CX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cx_trim - cat2.(subjects(i,:)).a.cx).^2));

r= 0.05982+0.027+0.055; % [m] distance VER to COG + COG to skull base + skull base to neck base
cat3a.(subjects(i,:)).a.vx = (cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)-(sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)+ r*calc.(subjects(i,:)).alpha_trim;
cat3a.(subjects(i,:)).a.vz = (sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)+(cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)- calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT3A_VX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vx_trim - cat3a.(subjects(i,:)).a.vx).^2));
RMSE_CAT3A_VZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vz_trim - cat3a.(subjects(i,:)).a.vz).^2));
RESIDUAL_CAT3A_VX.(subjects(i,:))=(cat3a.(subjects(i,:)).a.vx-sen.(subjects(i,:)).a.vx_trim).';
RESIDUAL_CAT3A_VZ.(subjects(i,:))=(cat3a.(subjects(i,:)).a.vz-sen.(subjects(i,:)).a.vz_trim).';

r= 0.027+0.055; % [m] distance COG to skull base + skull base to neck base
cat3a.(subjects(i,:)).a.cx = (cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)-(sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)+ r*calc.(subjects(i,:)).alpha_trim;
cat3a.(subjects(i,:)).a.cz = (sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)+(cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)- calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT3A_CX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cx_trim - cat3a.(subjects(i,:)).a.cx).^2));
RMSE_CAT3A_CZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cz_trim - cat3a.(subjects(i,:)).a.cz).^2));

r= 0.05982+0.027; % [m] distance VER to COG + COG to skull base 
cat3b.(subjects(i,:)).a.vx = (cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)-(sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)+ r*calc.(subjects(i,:)).alpha_trim;
cat3b.(subjects(i,:)).a.vz = (sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)+(cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)- calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT3B_VX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vx_trim - cat3b.(subjects(i,:)).a.vx).^2));
RMSE_CAT3B_VZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.vz_trim - cat3b.(subjects(i,:)).a.vz).^2));
RESIDUAL_CAT3B_VX.(subjects(i,:))=(cat3b.(subjects(i,:)).a.vx-sen.(subjects(i,:)).a.vx_trim).';
RESIDUAL_CAT3B_VZ.(subjects(i,:))=(cat3b.(subjects(i,:)).a.vz-sen.(subjects(i,:)).a.vz_trim).';

r= 0.027; % [m] distance COG to skull base 
cat3b.(subjects(i,:)).a.cx = (cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)-(sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)+ r*calc.(subjects(i,:)).alpha_trim;
cat3b.(subjects(i,:)).a.cz = (sin(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tx_trim)+(cos(theta_trim.(subjects(i,:))).'.*sen.(subjects(i,:)).a.tz_trim)- calc.(subjects(i,:)).w.gy_trim.^2*r;
RMSE_CAT3B_CX(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cx_trim - cat3b.(subjects(i,:)).a.cx).^2));
RMSE_CAT3B_CZ(i,:) = sqrt(mean((sen.(subjects(i,:)).a.cz_trim - cat3b.(subjects(i,:)).a.cz).^2));

if i==1
    figure() %SUBJ01
    title(strcat('Model comparison VERTEX X-direction', num2str(subjects(i,:))))
    subplot(2,1,1)
    plot(t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.vx_trim,'k','LineWidth',1.5)
    hold on
    plot(t_trim.(subjects(i,:)),cat1.(subjects(i,:)).a.vx,'g',t_trim.(subjects(i,:)),cat2.(subjects(i,:)).a.vx,'m',t_trim.(subjects(i,:)),cat3a.(subjects(i,:)).a.vx,'b',t_trim.(subjects(i,:)),cat3b.(subjects(i,:)).a.vx,'--b')
    hold on
    plot(t_trim.(subjects(i,:))(1:end-1),calc.(subjects(i,:)).a.vx,':r','LineWidth',1.5)
    hold off
    legend ('Measured in experiment','Cat I','Cat II','Cat III-A','Cat III-B','New model')
    grid on
    xlabel('Time (s)')
    ylabel('Acceleration (m/s^2)')
    title('a)')
    
    subplot(2,1,2)
    plot(t_trim.(subjects(i,:)),RESIDUAL_CAT1_VX.(subjects(i,:)),'g',t_trim.(subjects(i,:)),RESIDUAL_CAT2_VX.(subjects(i,:)),'m',t_trim.(subjects(i,:)),RESIDUAL_CAT3A_VX.(subjects(i,:)),'b',t_trim.(subjects(i,:)),RESIDUAL_CAT3B_VX.(subjects(i,:)),'--b')
    hold on
    plot(t_trim.(subjects(i,:))(1:end-1),RESIDUAL_VX.(subjects(i,:)),':r','LineWidth',1.5)
    hold off
    xlabel('Time (s)')
    ylabel('Residual (m/s^2)')
    legend ('Cat I','Cat II','Cat III-A','Cat III-B','New model')
    grid on
    title('b)')
    
    
    figure()%SUBJ01
    subplot(2,1,1)
    title(strcat('Model comparison VERTEX Z-direction', num2str(subjects(i,:))))
    plot(t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.vz_trim,'k','LineWidth',1.5)
    hold on
    plot(t_trim.(subjects(i,:)),cat1.(subjects(i,:)).a.vz,'g',t_trim.(subjects(i,:)),cat3a.(subjects(i,:)).a.vz,'b',t_trim.(subjects(i,:)),cat3b.(subjects(i,:)).a.vz,'--b')
    hold on
    plot(t_trim.(subjects(i,:))(1:end-1),calc.(subjects(i,:)).a.vz,':r','LineWidth',1.5)
    hold off
    legend ('Measured in experiment','Cat I','Cat III-A','Cat III-B','New model')
    grid on
    xlabel('Time (s)')
    ylabel('Acceleration (m/s^2)')
    title('a)')
    
    
    subplot(2,1,2)
    plot(t_trim.(subjects(i,:)),RESIDUAL_CAT1_VZ.(subjects(i,:)),'g',t_trim.(subjects(i,:)),RESIDUAL_CAT3A_VZ.(subjects(i,:)),'b',t_trim.(subjects(i,:)),RESIDUAL_CAT3B_VZ.(subjects(i,:)),'--b')
    hold on
    plot(t_trim.(subjects(i,:))(1:end-1),RESIDUAL_VZ.(subjects(i,:)),':r','LineWidth',1.5)
    hold off
    xlabel('Time (s)')
    ylabel('Residual (m/s^2)')
    legend ('Cat I','Cat III-A','Cat III-B','New model')
    grid on
    title('b)')
end


% %2D plot of tangent line WITH color shade(540)
% figure()
% plot(mot.(subjects(i,:)).s.cx_trim,mot.(subjects(i,:)).s.cz_trim,'b')
% hold on
% plot(x_axis_cog_tang,z_coordinate_cog_tang,'g',x_axis_cog_tang,z_coordinate_cog_perp,'k')
% hold on
% plot(mot.(subjects(i,:)).s.vx_trim,mot.(subjects(i,:)).s.vz_trim,'r')
% hold on
% plot(x_axis_ver_tang,z_coordinate_ver_tang,'g',x_axis_ver_tang,z_coordinate_ver_perp,'k')
% hold on
% w=1:10:length(IAOR.(subjects(i,:)).x);
% 
% % create a default color map ranging from red to light pink
% length_color_vector = length(w);
% color1 = [1, 0.8, 0];
% color2 = [0, 1, 0.9];
% colors_p = [linspace(color1(1),color2(1),length_color_vector)', linspace(color1(2),color2(2),length_color_vector)', linspace(color1(3),color2(3),length_color_vector)'];
% 
% 
% for q=1:length(w)
%     plot([mot.(subjects(i,:)).s.cx_trim(w(q)),mot.(subjects(i,:)).s.vx_trim(w(q))],[mot.(subjects(i,:)).s.cz_trim(w(q)),mot.(subjects(i,:)).s.vz_trim(w(q))],'color',colors_p(q,:))
%     hold on
% end
% hold off
% 
% xlim([min(mot.(subjects(i,:)).s.vx_trim)-0.1 max(mot.(subjects(i,:)).s.vx_trim)+0.1])
% ylim([min(mot.(subjects(i,:)).s.cz_trim)-0.1 max(mot.(subjects(i,:)).s.vz_trim)+0.1])
% axis equal
% title(strcat('Tangent and perpendicular lines-', num2str(subjects(i,:))));

% %2D plot of tangent line WITHOUT color shade (540)
% figure()
% plot(mot.(subjects(i,:)).s.cx_trim,mot.(subjects(i,:)).s.cz_trim,'b')
% hold on
% plot(x_axis_cog_tang,z_coordinate_cog_tang,'g',x_axis_cog_tang,z_coordinate_cog_perp,'k')
% hold on
% plot(mot.(subjects(i,:)).s.vx_trim,mot.(subjects(i,:)).s.vz_trim,'r')
% hold on
% plot(x_axis_ver_tang,z_coordinate_ver_tang,'g',x_axis_ver_tang,z_coordinate_ver_perp,'k')
% hold on
% w=1:10:length(IAOR.(subjects(i,:)).x);
% for q=w
%     plot([mot.(subjects(i,:)).s.cx_trim(q),mot.(subjects(i,:)).s.vx_trim(q)],[mot.(subjects(i,:)).s.cz_trim(q),mot.(subjects(i,:)).s.vz_trim(q)],'k')
%     hold on
% end
% hold off
% 
% xlim([0.55 0.9])
% ylim([1.4 1.6])
% axis equal
% % title(strcat('Tangent and perpendicular lines-', num2str(subjects(i,:))));

clear slope_cog_perp slope_cog_tang slope_ver_perp slope_ver_tang x_axis_cog_tang x_axis_ver_tang b_cog_tang b_ver_tang b_cog_perp b_ver_perp z_coordinate_cog_tang z_coordinate_cog_perp z_coordinate_ver_tang z_coordinate_ver_perp L1 L2 P
   
   
% 
% 
% figure()
% plot(mot.(subjects(i,:)).s.vx_trim*1000,mot.(subjects(i,:)).s.vz_trim*1000,'b')
% hold on
% plot(mot.(subjects(i,:)).s.cx_trim*1000,mot.(subjects(i,:)).s.cz_trim*1000,'r')
% hold on
% plot(mot.(subjects(i,:)).s.cx_trim(1)*1000,mot.(subjects(i,:)).s.cz_trim(1)*1000,'ok',mot.(subjects(i,:)).s.cx_trim(end)*1000,mot.(subjects(i,:)).s.cz_trim(end)*1000,'*k')
% hold on
% plot(mot.(subjects(i,:)).s.vx_trim(1)*1000,mot.(subjects(i,:)).s.vz_trim(1)*1000,'ok',mot.(subjects(i,:)).s.vx_trim(end)*1000,mot.(subjects(i,:)).s.vz_trim(end)*1000,'*k')
% hold off
% legend('Vertex','C.O.G','Begin of shake cycle','End of shake cycle')
% grid on
% xlabel('X coordinate (mm)')
% ylabel('Z coordinate (mm)')
% title('Head trajectory');
% axis('equal')

%SUBJ16
if i==16
    figure()
    plot(t_trim.(subjects(i,:))(1:end-1),r_ocx.(subjects(i,:))*1000,'b',t_trim.(subjects(i,:))(1:end-1),r_ocz.(subjects(i,:))*1000,'m')
    ylim([-1000 1000])
    xlabel('Time (s)')
    ylabel('Distance between ICOR and COG (mm)')
    xlim([14.48 14.68])
    grid on
    legend('z direction')
    title(strcat(num2str(subjects(i,:))));
end

med=median(ROC.(subjects(i,:))*1000);
% 
% figure()
% subplot(2,1,1)
% semilogy(t_trim.(subjects(i,:))(1:end-1),ROC.(subjects(i,:))*1000,'k')
% ylim([10^-1 10^6])
% xlim([t_trim.(subjects(i,:))(1) t_trim.(subjects(i,:))(end-1)])
% xlimit=xlim;
% y = ylim; % current y-axis limits
% line([xlim],[med,med],'Color','red','LineStyle',':');
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% hold off
% grid on
% legend('ROC','ROC-median','t_{\omega-peak}','t_{\alpha-peak}')
% title('a) Radius of curvature');
% xlabel('Time (s)')
% ylabel('Absolute distance from ICOR to COG (mm)')



% subplot(3,1,2)
% plot(t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.vx_trim,'b',t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.vz_trim,'--b')
% hold on
% plot(t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.cx_trim,'r',t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.cz_trim,'--r')
% ylim([1.1*min(sen.(subjects(i,:)).a.vx_trim) 1.1*max(sen.(subjects(i,:)).a.vx_trim)])
% hold on
% y = ylim; % current y-axis limits
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% hold on
% 
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% hold on
% plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% hold off
% xlabel('Time (s)')
% ylabel('Acceleration (m/s^2)')
% legend('Vertex-x','Vertex-z','COG-x','COG-z')
% grid on
% title('c) Linear accelerations w.r.t. earth reference frame');

% subplot(2,1,2)
% plot(t_trim.(subjects(i,:)),calc.(subjects(i,:)).w.gy_trim,'c')
% ylim([1.1*min(calc.(subjects(i,:)).w.gy_trim) 1.1*max(calc.(subjects(i,:)).w.gy_trim)])
% xlabel('Time (s)')
% ylabel('Angular velocity (rad/s)')
% yyaxis right
% plot(t_trim.(subjects(i,:)),calc.(subjects(i,:)).alpha_trim,'m')
% ylim([1.1*min(calc.(subjects(i,:)).alpha_trim) 1.1*max(calc.(subjects(i,:)).alpha_trim)])
% xlim([xlimit])
% % hold on
% % y = ylim; % current y-axis limits
% % plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==max(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% % hold on
% % plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==max(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% % hold on
% % plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).w.gy_trim==min(calc.(subjects(i,:)).w.gy_trim)))],[y(1) y(2)],'--c','LineWidth',2)
% % hold on
% % plot([t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim))) t_trim.(subjects(i,:))(find(calc.(subjects(i,:)).alpha_trim==min(calc.(subjects(i,:)).alpha_trim)))],[y(1) y(2)],'--m','LineWidth',2)
% % hold off
% ylabel('Angular acceleration (rad/s^2)')
% legend('Angular velocity \omega','Angular acceleration \alpha')%,'Peak omega','Peak alpha')
% grid on
% title('b) Angular velocity and angular acceleration');

%STATS rotation axis
median_rot(i)=median(ROC.(subjects(i,:)));
min_rot(i)=min(ROC.(subjects(i,:)));
max_rot(i)=max(ROC.(subjects(i,:)));


% figure()
% subplot(2,1,1)
% plot(t_trim.(subjects(i,:)),sen.(subjects(i,:)).a.vx_trim,'k')
% hold on
% plot(t_trim.(subjects(i,:))(1:end-1),calc.(subjects(i,:)).a.vx,'--b')
% hold on
% plot(t_trim.(subjects(i,:)),cat1.(subjects(i,:)).a.vx)
% hold off
% legend('a_{xv} measured','a_{xv} model','Morison')
% xlabel('Time (s)')
% ylabel('Acceleration (m/s^2)')
% title(strcat('Calculated avx-', num2str(subjects(i,:))));
% grid on
% subplot(2,1,2)
% plot(t_trim.(subjects(i,:))(1:end-1),RESIDUAL.(subjects(i,:)),'g')
% hold on
% plot(t_trim.(subjects(i,:)),sqrt((sen.(subjects(i,:)).a.vx_trim-cat1.(subjects(i,:)).a.vx).^2),'m')
% legend('Model 1','Morison')
% hold off
% % hold on
% % plot(t_trim.(subjects(i,:))(1:end-1),r_ocz.(subjects(i,:)),'c')
% xlabel('Time (s)')
% ylabel('Residual (m/s^2)')
% grid on





% figure()
% plot(t_trim.(subjects(i,:)),rad2deg(theta_trim.(subjects(i,:))))
% ylabel('Time [s]')
% ylabel('Angle [deg]')
% title(strcat('Theta-', num2str(subjects(i,:))));
end

display('min_rot:')
display(min(min_rot))
display(max(min_rot))
display(mean(min_rot))
display(std(min_rot))

display('max_rot:')
display(min(max_rot))
display(max(max_rot))
display(mean(max_rot))
display(std(max_rot))

display('median_rot:')
display(min(median_rot))
display(max(median_rot))
display(mean(median_rot))
display(std(median_rot))

display('RMSE values:')
display([mean(RMSE_CAT1_VX) mean(RMSE_CAT1_VZ) mean(RMSE_CAT1_CX) mean(RMSE_CAT1_CZ)], 'CAT I')
display([mean(RMSE_CAT2_VX) mean(RMSE_CAT2_CX)], 'CAT II')
display([mean(RMSE_CAT3A_VX) mean(RMSE_CAT3A_VZ) mean(RMSE_CAT3A_CX) mean(RMSE_CAT3A_CZ)], 'CAT III-A')
display([mean(RMSE_CAT3B_VX) mean(RMSE_CAT3B_VZ) mean(RMSE_CAT3B_CX) mean(RMSE_CAT3B_CZ)], 'CAT III-B')
display([mean(RMSE_VX) mean(RMSE_VZ) mean(RMSE_CX) mean(RMSE_CZ)], 'New model')
