function [X, tx] = simulate3d(X0, tu, U, ns)
% Forward integrates the dynamics of the model in legs3d and animates the
% results. The control nodes in U are linearly interpolated.
% Inputs:
% X0 - (2*n_f+2*n_m) x 1 - vector of initial values of states
% tu - 1 x nu - vector of time instants for excitation nodes
% U - n_m x nu - matrix containing the time history of the controls (neural
%     excitations
% ns (optional) - number of nodal times for states in X (also used for animation)
% where:
% n_f - number of DoFs
% n_m - number of muscles
% nu - number of nodal values of the controls
%
% Output:
% X - n_m x ns - matrix containing time history of states
% Author: Marko Ackermann
% Last modification: 10.14.2008 by MA

% ns
if nargin < 3
    error('Insufficient number of input arguments!');
elseif nargin == 3
    ns = 101; % number of nodal times for states
elseif nargin == 4
    % use passed ns
else 
    error('Too many input arguments!');
end

X0 = X0(:); % ensure column vector

t_dur = tu(end)-tu(1); % duration of simulation

% Numerical integration
%options = odeset('RelTol',1e-5);
options = [];
display('Integrating diff. equations using initial states and neural excitations ...');
tic

%[t_for,x_for]=ode23(@odefun,[tu(1) tu(end)],X0,options);
%[t_for,x_for]=ode15s(@odefun,[tu(1) tu(end)],X0,options);

%% Euler explicit with constant step sizes
n_nod = 97; % number of nodes for a complete gait cycle
dt = (tu(end)-tu(1))/(n_nod-1); % delta t
t_for = tu(1):dt:tu(end);
u = interp1(tu,U',t_for)';

x_for = zeros(length(X0),n_nod); % initialization
xdot = zeros(length(X0),n_nod); % initialization

hold on
x_for(:,1) = X0;
plot(t_for(1)*ones(1,length(x_for(:,1))),x_for(:,1),'o');

% Initial guess forward integration
load('ini_OpenSim2392_1000forward_ode15s_pect.mat','Xs','Us','Ts');
x_for0 = interp1(Ts,Xs',t_for)';

global FX
options = optimset('MaxFunEvals',1000000);

xdot(:,1) = legs3d(t_for(1), x_for(:,1), u(:,1), 'legs3d');
for i = 2:n_nod
    i
%     % Explicit Euler
%     xdot(:,i-1) = legs3d(t_for(i-1), x_for(:,i-1), u(:,i-1), 'legs3d');
%     x_for(:,i) = x_for(:,i-1) + dt*xdot(:,i-1);
    
    % Implicit Euler
    %x_for0(:,i) = x_for(:,i-1);
    
    %[x_for(:,i), fval, exitflag] = fsolve(@eulerimpl,x_for0(:,i),options);
    [x_for(:,i), fval, exitflag] = fsolve(@eulerimpl,x_for(:,i-1),options);
    max(abs(FX))
    if (exitflag <-1) || (max(abs(FX)) > 1.0)
        FX_max = max(abs(FX));
        display('Using a second initial guess ...');
        %[x_for_new, fval, exitflag] = fsolve(@eulerimpl,x_for(:,i-1),options);
        [x_for_new, fval, exitflag] = fsolve(@eulerimpl,x_for0(:,i),options);
        max(abs(FX))
        if max(abs(FX)) < FX_max
            x_for(:,i) = x_for_new;
            FX_max = max(abs(FX));
        end
        if (exitflag <-1) || (max(abs(FX)) > 1.0)
            FX_max = max(abs(FX));
            display('Using a third initial guess ...');
            [x_for_new, fval, exitflag] = fsolve(@eulerimpl,x_for0(:,i-1),options);
            %[x_for_new, fval, exitflag] = fsolve(@eulerimpl,x_for(:,i-1),options);
            max(abs(FX))
            if max(abs(FX)) < FX_max
                x_for(:,i) = x_for_new;
            end
        end
    end
    
    plot(t_for(i)*ones(1,length(x_for(:,i))),x_for(:,i),'o');
    %pause
    
%     % Trapezoidal  scheme
%     x_for(:,i) = fsolve(@trapezoidal,x_for0(:,i),options);
%     max(FX)
%     plot(t_for(i)*ones(1,length(x_for(:,i))),x_for(:,i),'o');
%     pause
end
x_for = x_for';


save('forward.mat','t_for','x_for');

toc
size(t_for)

% Animation
tx = tu(1):(t_dur/(ns-1)):t_for(end); % vector of nodal times for states
X = interp1(t_for,x_for,tx)';
Ui = interp1(tu,U',tx); 
anim3D(tx,X',Ui);
    

    %%%%%%%%%%%%%%%%%%%%
    % Function that implements implicit Euler integration scheme
    %%%%%%%%%%%%%%%%%%%%
    function FX = eulerimpl(xfor)
        global FX
        xdot(:,i) = legs3d(t_for(i), xfor, u(:,i), 'legs3d');
        FX = dt*xdot(:,i) - xfor + x_for(:,i-1);
    end

    %%%%%%%%%%%%%%%%%%%%
    % Function that implements trapezoidal integration scheme
    %%%%%%%%%%%%%%%%%%%%
    function FX = trapezoidal(xfor)
        global FX
        xdot(:,i) = legs3d(t_for(i), xfor, u(:,i), 'legs3d');
        FX = xfor - x_for(:,i-1) - 0.5*dt*(xdot(:,i-1)+xdot(:,i));
    end

    
    %%%%%%%%%%%%%%%%%%%%
    % Function for interpolation of neural excitations and calling
    % legs3d.m
    %%%%%%%%%%%%%%%%%%%%
    function [xdot] = odefun(t,x) 
    % evaluates system equations xdot = f(t,x,u) for model in legs3d  
    
      if (t <= tu(1))
        u = U(:,1);
      elseif (t >= tu(end))
        u = U(:,end);
      else
        u = interp1(tu,U',t)';
      end
         
    % call the MEX DLL legs3d to get derivatives
%     t
%     x
%     u
%     size(t)
%     size(x)
%     size(u)
%       if any(isnan(u))
%           t
%           u
%           error('There is NaN in the neural excitations vector');
%       end
%       if any(isnan(x))
%           t
%           x
%           error('There is NaN in the states vector');
%       end
      xdot = legs3d(t, x, u, 'legs3d');     
%       if any(isnan(xdot))
%           t
%           u
%           x
%           xdot
%           error('There is NaN in the xdot vector');
%       end
    end
end