%% create a function
function [] = Noise_Results()

%% start with a clean environment
clc;
clear all;
clear;
close all;

global data_RMS_error1;
global RMS_error_headers1;
global data_RMS_error2;
global RMS_error_headers2;
global count1;
global count2;
global str;

vari1 = 1;
vari2 = 1;

data_RMS_error1 = [];
RMS_error_headers1 = cell(vari1,1);
data_RMS_error2 = [];
RMS_error_headers2 = cell(vari2,1);
count1 = 0;
count2 = 0;

%% calculate RMS error due to noise
% do this for lower extremity only - joint angles, torques, translations

str = 's5_03725b07';

% pelvis
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_tx',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_ty',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_tz',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_tilt',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_list',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_rot',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_tilt_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_list_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('pelvis_rot_torque',str);

% hip
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_tx_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_ty_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_tz_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_flex_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_add_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_rot_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_flex_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_add_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('hip_rot_r_torque',str);

% knee
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_tx_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_ty_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_tz_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_flex_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_add_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_rot_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_flex_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_add_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('knee_rot_r_torque',str);

% ankle
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_tx_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_ty_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_tz_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_flex_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_add_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_rot_r',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_flex_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_add_r_torque',str);
[gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc('ankle_rot_r_torque',str);

RMS_error_headers = [RMS_error_headers1];
data_RMS_error = [data_RMS_error1]';

save S5_CGM_rand.mat RMS_error_headers data_RMS_error

% SAVE FILENAME X Y Z  saves X, Y, and Z. The wildcard '*' can be used to
% save only those variables that match a pattern.

%% noise_calc.m
function [gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc(gen_coord,file,row)

global data_RMS_error1;
global RMS_error_headers1;
global count1;
global str;

% Load motion trials with noisy markers
[noisy0,n0_headers]=load_motion2([file '_n00id.mot']);
[noisy3,n3_headers]=load_motion2([file '_n03id.mot']);
[noisy6,n6_headers]=load_motion2([file '_n06id.mot']);
[noisy9,n9_headers]=load_motion2([file '_n09id.mot']);
% Load motion trial with no noise added to markers
[noisy_nn,nn_headers]=load_motion2([file '_rea6dofikid.mot']);

% x = strmatch(str, strarray) looks through the rows of the character array
% or cell array of strings strarray to find strings that begin with the
% text contained in str, and returns the matching row indices. Any trailing
% space characters in str or strarray are ignored when matching. strmatch
% is fastest when strarray is a character array.

% extract time from data
t0 = strmatch('time',n0_headers);
t3 = strmatch('time',n3_headers);
t6 = strmatch('time',n6_headers);
t9 = strmatch('time',n9_headers);
tnn = strmatch('time',nn_headers);
time0 = noisy0(:,t0);
time3 = noisy3(:,t3);
time6 = noisy6(:,t6);
time9 = noisy9(:,t9);
timenn = noisy_nn(:,tnn);

% extract general coordinate data
gc0 = noisy0(:,strmatch(gen_coord,n0_headers,'exact'));
gc3 = noisy3(:,strmatch(gen_coord,n3_headers,'exact'));
gc6 = noisy6(:,strmatch(gen_coord,n6_headers,'exact'));
gc9 = noisy9(:,strmatch(gen_coord,n9_headers,'exact'));
gcnn = noisy_nn(:,strmatch(gen_coord,nn_headers,'exact'));

% calculate the RMS error due to noise
for i = 1:length(time0)  
    gen_c(i,1)=(gcnn(i,1) - gc0(i,1))^2;
    gen_c(i,2)=(gcnn(i,1) - gc3(i,1))^2;
    gen_c(i,3)=(gcnn(i,1) - gc6(i,1))^2;
    gen_c(i,4)=(gcnn(i,1) - gc9(i,1))^2;
end

% S = SUM(X) is the sum of the elements of the vector X. If X is a matrix,
% S is a row vector with the sum over each column. 
gc_t = sum(gen_c,1);

for i = 1:4;
    gc_error(1,i) = (gc_t(1,i)/length(time0))^0.5;
end

% figure;
% % plot(time0,gc0,time3,gc3,time6,gc6,time9,gc9,timenn,gcnn);
% plot(time0,gc0,time3,gc3,time6,gc6,time9,gc9);
% title(gen_coord);
% xlabel('Time (s)');
% legend('0 mm noise','3 mm noise','6 mm noise', '9 mm noise');
% % legend('0 mm noise','3 mm noise','6 mm noise', '9 mm noise','no noise added');

figure;
x = [0;3;6;9];
bar(x,gc_error')
xlabel('mm standard deviation of noise added');
ylabel('RMS error');
title(gen_coord);

count1 = count1 + 1;

data_RMS_error1 = [data_RMS_error1, gc_error'];
RMS_error_headers1(count1,1) = cellstr(gen_coord);

%% noise_calc2.m
function [gc0,gc3,gc6,gc9,gcnn,time0,time3,time6,time9,timenn,gen_coord,gc_error] = noise_calc2(gen_coord,file,gc_value)

global data_RMS_error2;
global RMS_error_headers2;
global count2;
global str;

% Load motion trials with noisy markers
[noisy0,n0_headers]=load_motion2([file '_n00id.mot']);
[noisy3,n3_headers]=load_motion2([file '_n03id.mot']);
[noisy6,n6_headers]=load_motion2([file '_n06id.mot']);
[noisy9,n9_headers]=load_motion2([file '_n09id.mot']);
% Load motion trial with no noise added to markers
[noisy_nn,nn_headers]=load_motion2([file '_reaid.mot']);

% x = strmatch(str, strarray) looks through the rows of the character array
% or cell array of strings strarray to find strings that begin with the
% text contained in str, and returns the matching row indices. Any trailing
% space characters in str or strarray are ignored when matching. strmatch
% is fastest when strarray is a character array.

% extract time from data
t0 = strmatch('time',n0_headers);
t3 = strmatch('time',n3_headers);
t6 = strmatch('time',n6_headers);
t9 = strmatch('time',n9_headers);
tnn = strmatch('time',nn_headers);
time0 = noisy0(:,t0);
time3 = noisy3(:,t3);
time6 = noisy6(:,t6);
time9 = noisy9(:,t9);
timenn = noisy_nn(:,tnn);

% extract general coordinate data
gc0 = noisy0(:,strmatch(gen_coord,n0_headers,'exact'));
gc3 = noisy3(:,strmatch(gen_coord,n3_headers,'exact'));
gc6 = noisy6(:,strmatch(gen_coord,n6_headers,'exact'));
gc9 = noisy9(:,strmatch(gen_coord,n9_headers,'exact'));
gcnn = noisy_nn(:,strmatch(gen_coord,nn_headers,'exact'));

% calculate the RMS error due to noise
for i = 1:length(time0)  
    gen_c(i,1)=(gc_value - gc0(i,1))^2;
    gen_c(i,2)=(gc_value - gc3(i,1))^2;
    gen_c(i,3)=(gc_value - gc6(i,1))^2;
    gen_c(i,4)=(gc_value - gc9(i,1))^2;
end

% S = SUM(X) is the sum of the elements of the vector X. If X is a matrix,
% S is a row vector with the sum over each column. 
gc_t = sum(gen_c,1);

for i = 1:4;
    gc_error(1,i) = (gc_t(1,i)/length(time0))^0.5;
end

% figure;
% % plot(time0,gc0,time3,gc3,time6,gc6,time9,gc9,timenn,gcnn);
% plot(time0,gc0,time3,gc3,time6,gc6,time9,gc9);
% title(gen_coord);
% xlabel('Time (s)');
% legend('0 mm noise','3 mm noise','6 mm noise', '9 mm noise');
% % legend('0 mm noise','3 mm noise','6 mm noise', '9 mm noise','no noise added');

figure;
x = [0;3;6;9];
bar(x,gc_error')
xlabel('mm standard deviation of noise added');
ylabel('RMS error');
title(gen_coord);

count2 = count2 + 1;

data_RMS_error2 = [data_RMS_error2, gc_error'];
RMS_error_headers2(count2,1) = cellstr(gen_coord);

%% load_motion2.m
function [data,headers]=load_motion2(file);
% [data,headers]=load_motion(file)
% Input:
% file - the input file name
% Output:
% data - matrix of data
% headers - column headers
%
fid=fopen(file,'r');
disp(['Loading file...' file] );
%read the file name line
hdr=fgetl(fid);
% Read the number of data columns and rows
line=fgetl(fid);	nc=sscanf(line,'%*s %f');
line=fgetl(fid);	nr=sscanf(line,'%*s %f');
line=fgetl(fid);	[x0,x1]=sscanf(line,'%*s %f %f');
% Skip through the next nine lines of header info
for i=1:9
     line=fgetl(fid);
end
% Load the headers
headers=cell(nc,1);
line=fgetl(fid);
j=1;
jl=length(line);
for i=1:nc
	name=sscanf(line(j:jl),'%s',1);
    ii = findstr(line(j:jl), name);
    j=j+ii(1)+length(name);
	headers(i,1)=cellstr(name);
end
% Now load the data	
data=zeros(nr,nc);
i=0;
while ((feof(fid)==0)&(i<nr))
     i=i+1;
     line=fgetl(fid);
     data(i,:)=sscanf(line,'%f');
end
if (i<nr)
	disp(['Number of rows (',num2str(i),') is less than that specified in header (',num2str(nr),')']);
	data=data(1:nr,:);
end
fclose(fid);


function [t,q]=load_exp(file);
global NQ NM;
rawdata=load_motionfile(file);
[t,data]=extractcolumns(rawdata,1);
[q,data]=extractcolumns(data,NQ);


function [x,outdata]=extractcolumns(data,nc);
x=data(:,1:nc);
[m,n]=size(data);
outdata=data(:,(nc+1):n);
