function walkode_test
% This function checks if the tested version of walkode gives the same
% output as the version of walkode used by Marko Ackermann on
% 10.17.2007, using a standard input. The reference input is saved in
% input_walkode.mat, the reference output is saved in output_walkode.mat 
% Author: Marko Ackermann
% Date: 10.17.2007
% Last modification:
% copying the updated .mexw32 in the same directory by Xuemei


% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Computationf of reference input and reference input
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% %% Using result of direct collocation as reference input
% load('result_back_59.mat'); n_nod = 50;
% P = Result.x_k;
% for k = 1:n_nod-1
%     X(:,k) = P((k-1)*66+1:(k-1)*66+50); % states
%     U(:,k) = P((k-1)*66+51:k*66); % neural excitations
% end
% t_f = P(end);
% 
% % Time vector
% T = 0:t_f/(n_nod-1):(t_f-t_f/(n_nod-1));
% 
% % Save as standard input
% save('input_walkode.mat','X','U','T');
% 
% 
% %% Using reference input to generate reference output, using walkode
% %% version used by Marko Ackermann at 10.17.2007
% % Load reference input
% load('input_walkode.mat');
% 
% % Generate reference output from reference input
% [statedots, cm, grf, mforces, stick, mforcesce] = walkode(T, X, U);
% 
% % Save the reference output
% save('output_walkode.mat','statedots','cm','grf','mforces','stick','mforcesce');
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Load reference input
load('input_walkode.mat','T','X','U');

copyfile ('../../mdl/legs2dmex/legs2d.mexw32', 'legs2d.mexw32');

%% Compute output of tested walkode using reference input
[statedots_t, cm_t, grf_t, mforces_t, stick_t, mforcesce_t, Premind] = legs2d(T, X, U);

% %% For testing differences
% statedots_t(20,1) = NaN;
% cm_t(2,20) = NaN;
% grf_t(3,13) = NaN;
% mforces_t(10,3) = NaN;
% mforces_t(12,3) = NaN;
% stick_t(16,37) = NaN;
% mforcesce_t(3,1) = NaN;

%% Load reference output
load('output_walkode.mat','statedots','cm','grf','mforces','stick','mforcesce');

%% Compare output of tested walkode with reference output

disp(' ')
disp('1) Derivatives of States ')

% Derivatives of states
if isequal(statedots_t,statedots)
    disp('OK! Derivatives of states are equal to reference output.');
else
    disp('DIFFERENCES FOUND! Derivatives of states are unequal reference output in the following rows:');
    for i=1:length(statedots(:,1))
        if isequal(statedots_t(i,:),statedots(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')
disp('2) CM trajectories')

% Trajectories of CM
if isequal(cm_t,cm)
    disp('OK! CM trajectories are equal to reference output.');
else
    disp('DIFFERENCES FOUND! CM trajectories are unequal reference output in the following rows:');
    for i=1:length(cm(:,1))
        if isequal(cm_t(i,:),cm(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')
disp('3) GRFs')

% Ground reaction forces
if isequal(grf_t,grf)
    disp('OK! GRFs are equal to reference output.');
else
    disp('DIFFERENCES FOUND! GRFs are unequal reference output in the following rows:');
    for i=1:length(grf(:,1))
        if isequal(grf_t(i,:),grf(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')
disp('4) Total muscle forces')

% Total muscle forces
if isequal(mforces_t,mforces)
    disp('OK! Muscle forces are equal to reference output.');
else
    disp('DIFFERENCES FOUND! Muscle forces are unequal reference output in the following rows:');
    for i=1:length(mforces(:,1))
        if isequal(mforces_t(i,:),mforces(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')
disp('5) Stick coordiantes')

% Joint coordinates for stick figure
if isequal(stick_t,stick)
    disp('OK! Stick coordinates are equal to reference output.');
else
    disp('DIFFERENCES FOUND! Stick coordinates are unequal reference output in the following rows:');
    for i=1:length(stick(:,1))
        if isequal(stick_t(i,:),stick(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')
disp('6) CE forces')

% CE muscle forces
if isequal(mforcesce_t,mforcesce)
    disp('OK! CE forces are equal to reference output.');
else
    disp('DIFFERENCES FOUND! CE forces are unequal reference output in the following rows:');
    for i=1:length(mforcesce(:,1))
        if isequal(mforcesce_t(i,:),mforcesce(i,:)) == 0
            disp(['row ' int2str(i)]);
        end
    end
end

disp(' ')






