% function [] = residuals(subjectID)
% Purpose:  Reads the set of motion files output by Darryl's executables
%           corresponding to subjectID and generates plots of the
%           gencoords vs time for comparing the output of the IK, REA, and 
%           CMC steps to the subject's experimental data
%
% Input:    subjectID is the prefix ('string') of the following input files, 
%           generated from the preprocessing step and Darryl's executables:
%               subjectID.mot
%               subjectIDik.mot
%               subjectIDrea.mot
%               subjectIDcmc.mot
%
% Called Functions:  read_motionfile.m
%                    
% ASA, 3-05


% Set flag for printing (1 = print to printer; 0 = print to screen only)
print_flag = 0;

% Set flag for pausing between figures (1 = pause, 0 = do not pause)
pause_flag = 1;

ic = 1.36;
ic2 = 2.55;
dur = 1.19;

% Read file subjectID.mot
% fname = strcat(subjectID, '_raw_all.mot');%changed file name from '.mot' to 'raw.mot'
% fname = strcat(subjectID, '_raw_presw2pk.mot');
% mot = read_motionfile('hpl_overground_slow_walk.trc');

% Read files subjectIDik.mot
% fname = strcat(subjectID, '_invkin.mot');
% fname = strcat(subjectID, '_invkin_presw2pk.mot');
% fname = strcat(subjectID, 'cmc_presw2pk_original.mot');
ik = read_motionfile2('MDeMers_IK_SlowWalk.mot');

% Read files subjectIDrea.mot
% fname = strcat(subjectID, '_rea.mot');
% fname = strcat(subjectID, '_rea_presw2pk.mot');
rea = read_motionfile2('MDeMers_slow_walk_RRA_Kinematics_q.mot');
rra_act = read_motionfile3('MDeMers_slow_walk_RRA_Actuation_force.sto');
% Read files subjectIDcmc.mot
% fname = strcat(subjectID, '_cmc.mot');
% fname = strcat(subjectID, '_cmc_presw2pk.mot');
cmc_act = read_motionfile3('MDeMers_slow_walk_cmc_SLS_Actuation_force.sto');
cmc = read_motionfile2('MDeMers_slow_walk_cmc_SLS_Kinematics_q.mot');
cmc_cont = read_motionfile3('MDeMers_slow_walk_cmc_SLS_controls.sto');
cmc_state = read_motionfile3('MDeMers_slow_walk_cmc_SLS_states.sto');
% Generate plots
set(0, 'Units', 'pixels');
scnsize = get(0, 'ScreenSize');
scnwidth = scnsize(3);
scnheight = scnsize(4);
figpos_whole = [0.1*scnwidth, 0.1*scnheight, 0.8*scnwidth, 0.5*scnheight];
    

figure(1);   % Residuals from RRA
    clf
    set(gcf, 'Position', figpos_whole, 'Color', 'w');
    set(gcf, 'PaperOrientation', 'landscape');
    set(gcf, 'PaperPosition', [1 1 9 6.5]);
    
    subplot(1, 2, 1);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(rra_act.labels, 'time'));
    cid_FX = find(strcmpi(rra_act.labels, 'FX'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FX));
    set(p, 'LineWidth', 1, 'Color', [0.8 0.0 0.5]);
    hold on;
    cid_FY = find(strcmpi(rra_act.labels, 'FY'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FY));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.8 0.3]);
    hold on;
    cid_FZ = find(strcmpi(rra_act.labels, 'FZ'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FZ));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.0 0.8]);
    hold on;
    title_string = ['Residual Forces'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Force (N)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itFX}', '{\itFY}', '{\itFZ}', 0);
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
    subplot(1, 2, 2);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(rra_act.labels, 'time'));
    cid_MX = find(strcmpi(rra_act.labels, 'MX'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MX));
    set(p, 'LineWidth', 1, 'Color', [0.8 0.0 0.5]);
    hold on;
    cid_MY = find(strcmpi(rra_act.labels, 'MY'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MY));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.8 0.3]);
    hold on;
    cid_MZ = find(strcmpi(rra_act.labels, 'MZ'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MZ));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.0 0.8]);
    hold on;
    title_string = ['Residual Moments'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itMX}', '{\itMY}', '{\itMZ}', 0);
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
%  2nd orientation
figure(2);   % Residuals from RRA
    clf
    set(gcf, 'Position', figpos_whole, 'Color', 'w');
    set(gcf, 'PaperOrientation', 'landscape');
    set(gcf, 'PaperPosition', [1 1 9 6.5]);
    
    subplot(2, 1, 1);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(rra_act.labels, 'time'));
    cid_FX = find(strcmpi(rra_act.labels, 'FX'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FX));
    set(p, 'LineWidth', 1, 'Color', [0.8 0.0 0.5]);
    hold on;
    cid_FY = find(strcmpi(rra_act.labels, 'FY'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FY));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.8 0.3]);
    hold on;
    cid_FZ = find(strcmpi(rra_act.labels, 'FZ'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_FZ));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.0 0.8]);
    hold on;
    title_string = ['Residual Forces'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Force (N)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itFX}', '{\itFY}', '{\itFZ}', 'Location', 'Best');
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
    subplot(2, 1, 2);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(rra_act.labels, 'time'));
    cid_MX = find(strcmpi(rra_act.labels, 'MX'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MX));
    set(p, 'LineWidth', 1, 'Color', [0.8 0.0 0.5]);
    hold on;
    cid_MY = find(strcmpi(rra_act.labels, 'MY'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MY));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.8 0.3]);
    hold on;
    cid_MZ = find(strcmpi(rra_act.labels, 'MZ'));
    p = plot(rra_act.data(:, cid_time), rra_act.data(:, cid_MZ));
    set(p, 'LineWidth', 1, 'Color', [0.0 0.0 0.8]);
    hold on;
    title_string = ['Residual Moments'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itMX}', '{\itMY}', '{\itMZ}', 'Location', 'Best');
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
%-----------------------------------------------------------------
% cmc reserves

figure(3);   % cmc reserves
    clf
    set(gcf, 'Position', figpos_whole, 'Color', 'w');
    set(gcf, 'PaperOrientation', 'landscape');
    set(gcf, 'PaperPosition', [1 1 9 6.5]);
    
    subplot(3, 1, 1);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(rra_act.labels, 'time'));
    cid_M1 = find(strcmpi(cmc_act.labels, 'hip_flexion_r_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M1));
    set(p, 'LineWidth', 1, 'Color', 'b');
    hold on;
%     cid_M2 = find(strcmpi(cmc_act.labels, 'hip_adduction_r_reserve'));
%     p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M2));
%     set(p, 'LineWidth', 1, 'Color', 'g');
%     hold on;
%     cid_M3 = find(strcmpi(cmc_act.labels, 'hip_rotation_r_reserve'));
%     p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M3));
%     set(p, 'LineWidth', 1, 'Color', 'r');
%     hold on;
    cid_M4 = find(strcmpi(cmc_act.labels, 'knee_angle_r_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M4));
    set(p, 'LineWidth', 1, 'Color', 'c');
    hold on;
    cid_M5 = find(strcmpi(cmc_act.labels, 'ankle_angle_r_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M5));
    set(p, 'LineWidth', 1, 'Color', 'm');
    hold on;
    title_string = ['Right Leg Reserves'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itHip flex}', '{\itKnee}', '{\itAnkle}', 'Location', 'NorthEastOutside');
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
    subplot(3, 1, 2);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_M6 = find(strcmpi(cmc_act.labels, 'hip_flexion_l_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M6));
    set(p, 'LineWidth', 1, 'Color', 'b');
    hold on;
%     cid_M7 = find(strcmpi(cmc_act.labels, 'hip_adduction_l_reserve'));
%     p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M7));
%     set(p, 'LineWidth', 1, 'Color', 'g');
%     hold on;
%     cid_M8 = find(strcmpi(cmc_act.labels, 'hip_rotation_l_reserve'));
%     p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M8));
%     set(p, 'LineWidth', 1, 'Color', 'r');
%     hold on;
    cid_M9 = find(strcmpi(cmc_act.labels, 'knee_angle_l_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M9));
    set(p, 'LineWidth', 1, 'Color', 'c');
    hold on;
    cid_M10 = find(strcmpi(cmc_act.labels, 'ankle_angle_l_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M10));
    set(p, 'LineWidth', 1, 'Color', 'm');
    hold on;
    title_string = ['Left Leg Reserves'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itHip flex}', '{\itKnee}', '{\itAnkle}', 'Location','NorthEastOutside');
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
    subplot(3, 1, 3);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_M11 = find(strcmpi(cmc_act.labels, 'lumbar_extension_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M11));
    set(p, 'LineWidth', 1, 'Color', 'b');
    hold on;
    cid_M12 = find(strcmpi(cmc_act.labels, 'lumbar_bending_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M12));
    set(p, 'LineWidth', 1, 'Color', 'g');
    hold on;
    cid_M13 = find(strcmpi(cmc_act.labels, 'lumbar_rotation_reserve'));
    p = plot(cmc_act.data(:, cid_time), cmc_act.data(:, cid_M13));
    set(p, 'LineWidth', 1, 'Color', 'r');
    hold on;
    title_string = ['Lumbar Reserves'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    hlegend = legend('{\itext}', '{\itbend}', '{\itrot}', 'Location', 'NorthEastOutside');
    set(hlegend, 'Box', 'off', 'XColor', 'w', 'YColor', 'w');
    
    figure (4)
    
    clf
    set(gcf, 'Position', figpos_whole, 'Color', 'w');
    set(gcf, 'PaperOrientation', 'landscape');
    set(gcf, 'PaperPosition', [1 1 9 6.5]);
    
    subplot(2, 1, 1);
%     cid_motx = find(strcmpi(mot.labels, 'time'));
%     cid_moty = find(strcmpi(mot.labels, 'pelvis_tx'));
%     p = plot(mot.data(:, cid_motx), mot.data(:, cid_moty));
%     set(p, 'LineWidth', 1, 'Color', 'k');
%     hold on;
    cid_time = find(strcmpi(cmc_state.labels, 'time'));
    cid_gas_act = find(strcmpi(cmc_state.labels, 'med_gas_r.activation'));
    p = plot(cmc_state.data(:, cid_time), cmc_state.data(:, cid_gas_act));
    set(p, 'LineWidth', 1, 'Color', 'b');
    hold on;
    cid_time2 = find(strcmpi(cmc_cont.labels, 'time'));
    cid_gas_ex = find(strcmpi(cmc_cont.labels, 'med_gas_r.excitation'));
    p = plot(cmc_cont.data(:, cid_time2), cmc_cont.data(:,cid_gas_ex));
    set(p, 'LineWidth', 1, 'Color', 'r');
    title_string = ['Right Gas Excitation and Activation'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')
    
    subplot(2,1,2)
    cid_time = find(strcmpi(cmc_act.labels, 'time'));
    cid_gas_force = find(strcmpi(cmc_act.labels, 'med_gas_r'));
    p = plot(cmc_state.data(:, cid_time), cmc_state.data(:, cid_gas_act));
    set(p, 'LineWidth', 1, 'Color', 'g');
    hold on;
    title_string = ['Right Gas Force'];
    title(title_string, 'FontSize', 9, 'FontWeight', 'Bold');
    xlabel('time (s)', 'FontSize', 9, 'VerticalAlignment', 'top');
    ylabel('Torque (N-m)', 'FontSize', 9, 'VerticalAlignment', 'bottom');
%     set(gca, 'Xtick', [0 0.2 0.4 0.6 0.8 1.0]);
    set(gca, 'Box', 'off', 'FontSize', 8);
    ytickpositions = get(gca, 'Ytick');
    if length(ytickpositions) > 5
        new_ytickpositions = ytickpositions(1:2:length(ytickpositions));
        set(gca, 'Ytick', new_ytickpositions);
    end
    vline(2.025, 'k', 'RHS')
    vline(2.175, 'k', 'LTO')
    vline(2.65, 'k', 'LHS')
    vline(2.775, 'k', 'RTO')