function J = obj_func_DC_m(P,Prob)
% returns the objective function to SNOPT
% J = metabolic cost of transport using expressions of Umberger et al.
%     (2003)
% NOT IMPLEMENTED YET

n_nod = Prob.user.n_nod;

% CE maximal isometric muscle force
F_isomax = [821 1705 1770 663 7403 1639 3883 1528]; % [N] from Gerritsen et al. (1998)

% Optimal muscle CE length
L_ceopt = [0.102 0.2 0.104 0.081 0.093 0.055 0.055 0.082]; % [m] from Gerritsen et al. (1998)

% Extract the matrix of states and the matrix of neural excitations from 
% the vector of opt. variables P
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

% Computing the time step size
dt = P(end)/(n_nod-1);

% Vector of time instants
T = 0:dt:dt*(n_nod-2);

% Computation of the state derivatives, the center of mass position and velocities,
% and the ground reaction forces
[XD, CM, GRF, mforces, stick, mforcesce] = legs2d(T, X, U);


% % Correct the neural excitations that are less than 0.01 (lower bound)
% % to be between 0.005 and 0.010 using an exponetial function as
% % Note: this correction takes half of the time for the function evaluation!!
% for i = 1:16
%     Ui = U(i,:);
%     Ui(Ui<=0.01) = 0.005 + 0.005*exp(Ui(Ui<=0.01))/exp(0.01);
%     U(i,:) = Ui;
% end


%%%%%%%%%%
%% Metabolic Cost of transport using expressions in Umberger et al.(2003)
%%%%%%%%%%
% Parameters
PAR_MUSC.Lceopt = [L_ceopt L_ceopt];
PAR_MUSC.Fisomax = [F_isomax F_isomax];
PAR_MUSC.width = [1.298 0.625 1.197 1.443 0.627 0.888 1.039 0.442 1.298 0.625 1.197 1.443 0.627 0.888 1.039 0.442]; % [] relative muscle width
PAR_MUSC.ftg = [50 45 35 65 50 50 20 25 50 45 35 65 50 50 20 25]; % [%] percent of muscle fast twitch fibers from Umberger

number = 16; % number of muscles
T_comp = 0:dt:dt*(n_nod-1); % time vector including the last node that coincides with the first
for i = 1:8
    M_Lce(i,:) = [X(18+i,:) X(26+i,1)]*L_ceopt(i); % CE length vector
    M_Lce(8+i,:) = [X(26+i,:) X(18+i,1)]*L_ceopt(i); % CE length vector
    M_Vce(i,:) = [XD(18+i,:) XD(26+i,1)]*L_ceopt(i); % CE velocities vector
    M_Vce(8+i,:) = [XD(26+i,:) XD(18+i,1)]*L_ceopt(i); % CE velocities vector
end
M_Fce = [mforcesce(:,:) [mforcesce(9:16,1); mforcesce(1:8,1)]]; % CE forces vector
M_a = [X(35:50,:) [X(43:50,1); X(35:42,1)]]; % muscle activations
M_u = [U(:,:) [U(9:16,1); U(1:8,1)]]; % neural excitations
[TE,WT,EP,W,AM,SP] = metabolical_cost_ger_mod(M_Vce, M_Lce, M_Fce, M_u, M_a, T_comp, number, PAR_MUSC);

% %%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%
% hold on
% plot(TE,'b');
% plot(WT,'r');
% hold off
% 
% figure
% hold on
% plot(EP(1,:)','b');
% plot(W(1,:)','r');
% plot(AM(1,:)','g');
% plot(SP(1,:)','k');
% hold off
% pause
% %%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%

DS = Prob.user.v_walk*P(end); % [m] step length = v_walk*(gait period)/2
J = 0.001*TE(end)/DS;
%%%%%%%%%%
%%%%%%%%%%

