function J = obj_func_DC_ud(P,Prob)
% returns the objective function to SNOPT
% J = Sum of the mean of the muscle activations to the power of n_p PLUS 
%     deviations from kinematics (hip, knee, and ankle angles) and of grfs
%     of data in Winter (1991) 
% Last modification: 01/24/2009 (by Marko Ackermann)

n_nod = Prob.user.n_nod;
model = Prob.user.model; % spedifies model used
n_f =  Prob.user.n_f; % number of DoFs
n_m = Prob.user.n_m; % number of muscles
bsymm = Prob.user.bsymm; % bilateral symmetry activated or not
n_p = Prob.user.n_p; % power in the cost function
F_isomax = Prob.user.F_isomax; % vector of CE maximal isometric forces
L_ceopt = Prob.user.L_ceopt; % vector of optimal muscle CE lengths
c1_J = Prob.user.c1_J; % weighting factor for term 1 of cost function
c2_J = Prob.user.c2_J; % weighting factor for term 2 of cost function

v_walk = Prob.user.v_walk;
% For optimization of speed
if isempty(v_walk)
    v_walk = P(end-1);
end

% Extract the matrix of states and the matrix of neural excitations from 
% the vector of optimization variables P
X = zeros((2*n_f+2*n_m),n_nod-1); % initialization
for k = 1:n_nod-1
    X(:,k) = P((k-1)*(2*n_f+3*n_m)+1:(k-1)*(2*n_f+3*n_m)+(2*n_f+2*n_m)); % states
    %U(:,k) = P((k-1)*(2*n_f+3*n_m)+(2*n_f+2*n_m)+1:k*(2*n_f+3*n_m)); % 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);

% % 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);
%     %Ui(Ui<=0.01) = 0.005 + 0.005*exp(Ui(Ui<=0.01)/0.005)/exp(2);
%     U(i,:) = Ui;
% end

% Using activations instead of neural excitations for "effort" term
U = X(2*n_f+n_m+1:end,:); % muscle activations

% Compute the objective function: sum of the mean squared muscle
% activations multiplied by muscle volume and divided by total muscle 
% volume, where muscle volume is estimated with Lceopti*Fisomaxi
VP_v = [L_ceopt L_ceopt].*[F_isomax F_isomax];
J1 =  sum(VP_v*U.^n_p)/((n_nod-1)*sum(VP_v));

% Tracking of kinematics and grfs using data of Winter (1991)
if model == 0 % legs2d
    [XD, CM, GRF] = legs2d(T, X, U);
    
    if bsymm == 1 % bilateral symmetry activated
        hip_ang = [X(4,:) X(7,:) X(4,1)]; 
        knee_ang = [X(5,:) X(8,:) X(5,1)];
        ankle_ang = [X(6,:) X(9,:) X(6,1)];
        GRF_X = [GRF(1,:) GRF(4,:) GRF(1,1)];
        GRF_Y = [GRF(2,:) GRF(5,:) GRF(2,1)];
        
        J2 = c1_J*(1/(5*(2*n_nod-1)))*sum(sum((([hip_ang;                    knee_ang;                    ankle_ang;                    GRF_X;                    GRF_Y] - ...
                                          [Prob.user.kin_W_hip_int;    Prob.user.kin_W_knee_int;    Prob.user.kin_W_ankle_int;    Prob.user.grf_W_X_int;    Prob.user.grf_W_Y_int])./ ...
                                          [Prob.user.kin_W_hip_sd_int; Prob.user.kin_W_knee_sd_int; Prob.user.kin_W_ankle_sd_int; Prob.user.grf_W_X_sd_int; Prob.user.grf_W_Y_sd_int]).^2));    
        
    else % bsymm = 0 - bilateral symmetry DEactivated
        hip_ang_l = [X(4,:) X(4,1)]; 
        knee_ang_l = [X(5,:) X(5,1)];
        ankle_ang_l = [X(6,:) X(6,1)];
        GRF_X_l = [GRF(1,:) GRF(1,1)];
        GRF_Y_l = [GRF(2,:) GRF(2,1)];
        
        hip_ang_r = [X(7,:) X(7,1)]; 
        knee_ang_r = [X(8,:) X(8,1)];
        ankle_ang_r = [X(9,:) X(9,1)];
        GRF_X_r = [GRF(4,:) GRF(4,1)];
        GRF_Y_r = [GRF(5,:) GRF(5,1)];       
        
        J2 = c1_J*(1/(5*(2*n_nod-1)))*sum(sum((([hip_ang_l;                  knee_ang_l;                  ankle_ang_l;                  GRF_X_l;                  GRF_Y_l;                   hip_ang_r;                    knee_ang_r;                    ankle_ang_r;                    GRF_X_r;                    GRF_Y_r] - ...
                                          [Prob.user.kin_W_hip_int;    Prob.user.kin_W_knee_int;    Prob.user.kin_W_ankle_int;    Prob.user.grf_W_X_int;    Prob.user.grf_W_Y_int;     Prob.user.kin_W_hip_int_c;    Prob.user.kin_W_knee_int_c;    Prob.user.kin_W_ankle_int_c;    Prob.user.grf_W_X_int_c;    Prob.user.grf_W_Y_int_c])./ ...
                                          [Prob.user.kin_W_hip_sd_int; Prob.user.kin_W_knee_sd_int; Prob.user.kin_W_ankle_sd_int; Prob.user.grf_W_X_sd_int; Prob.user.grf_W_Y_sd_int;  Prob.user.kin_W_hip_sd_int_c; Prob.user.kin_W_knee_sd_int_c; Prob.user.kin_W_ankle_sd_int_c; Prob.user.grf_W_X_sd_int_c; Prob.user.grf_W_Y_sd_int_c]).^2));    
        
    end 
    
elseif model == 1 % legs3d
    [XD, CM, GRF] = legs3d(T, X, U);
    
    if bsymm == 1 % bilateral symmetry activated
        
%         % Using Winter's data
%         hip_ang = [X(7,:) X(13,:) X(7,1)];  % hip flexion
%         knee_ang = [X(10,:) X(16,:) X(10,1)]; % knee extension
%         ankle_ang = [X(11,:) X(17,:) X(11,1)]; % ankle plantar flexion
%         GRF_Y = [GRF(2,:) GRF(8,:) GRF(2,1)]; % posterior-anterior ground reaction force
%         GRF_Z = [GRF(3,:) GRF(9,:) GRF(3,1)]; % vertical ground reaction force
%         
%         J2 = c1_J*(1/(5*(2*n_nod-1)))*sum(sum((([hip_ang;                    knee_ang;                    ankle_ang;                    GRF_Y;                    GRF_Z] - ...
%                                           [Prob.user.kin_W_hip_int;    Prob.user.kin_W_knee_int;    Prob.user.kin_W_ankle_int;    Prob.user.grf_W_Y_int;    Prob.user.grf_W_Z_int])./ ...
%                                           [Prob.user.kin_W_hip_sd_int; Prob.user.kin_W_knee_sd_int; Prob.user.kin_W_ankle_sd_int; Prob.user.grf_W_Y_sd_int; Prob.user.grf_W_Z_sd_int]).^2));    
        
        % Using general reference data
        Q_ref_int = Prob.user.Q_ref_int;
        Q_sd_ref_int = Prob.user.Q_sd_ref_int;
        GRF_ref_int = Prob.user.GRF_ref_int;
        GRF_sd_ref_int = Prob.user.GRF_sd_ref_int;
        
        % Simulated gen. coords. for whole gait cycle
        Xmirr = mirr3D(X,bsymm);
        Xmirr(2,:) = Xmirr(2,:)+v_walk*P(end);
        Xend = X(:,1);
        Xend(2) = Xend(2)+v_walk*2*P(end);
        Xsim = [X Xmirr Xend];
        Qsim = Xsim(1:21,:);
        
        % Simulated ground reaction forces
        GRFmirr = [-GRF(7,:); GRF(8,:); GRF(9,:); GRF(10,:); GRF(11,:); -GRF(12,:); -GRF(1,:); GRF(2,:); GRF(3,:); GRF(4,:); GRF(5,:); -GRF(6,:)];
        GRFmirr(5,:) = GRF(11,:)+v_walk*P(end);
        GRFmirr(11,:) = GRF(5,:)+v_walk*P(end);
        GRFmirr(4,:) = 2*X(1,:) - GRF(10,:);
        GRFmirr(10,:) = 2*X(1,:) - GRF(4,:);
        GRFend = GRF(:,1);
        GRFend(5) = GRF(5,1)+v_walk*2*P(end);
        GRFend(11) = GRF(11,1)+v_walk*2*P(end);
        GRFend(4) = GRF(4,1) + (Xsim(1,end)-Xsim(1,1));
        GRFend(10) = GRF(10,1) + (Xsim(1,end)-Xsim(1,1));        
        GRFsim = [GRF GRFmirr GRFend];  
        
        % Reference gen coords.
        Qref = Q_ref_int;
        Qsdref = Q_sd_ref_int;
        
        % Reference ground reaction forces
        GRFref = GRF_ref_int([1:3 4:5 9 10:12 13:14 18],:);
        GRFsdref = GRF_sd_ref_int([1:3 4:5 9 10:12 13:14 18],:);
        
        % Select indices of tracked gen coords.
        ind_gc = [1 2 3 7 8 9 10 11 12]; 
        
        % Select indices of tracked grfs
        ind_grf = [1 2 3 6];
        
%         %%%%%%%%%
%         %%%%%%%%%
%         close all
%         for i=1:length(ind_gc)
%             i
%             hold on
%             plot(Qsim(ind_gc(i),:),'r');
%             plot(Qref(ind_gc(i),:));
%             plot(Qref(ind_gc(i),:)-Qsdref(ind_gc(i),:),'--');
%             plot(Qref(ind_gc(i),:)+Qsdref(ind_gc(i),:),'--');         
%             hold off
%             pause
%             close
%         end
%         
%         for i=1:length(ind_grf)
%             i
%             hold on
%             plot(GRFsim(ind_grf(i),:),'r');
%             plot(GRFref(ind_grf(i),:));
%             plot(GRFref(ind_grf(i),:)-GRFsdref(ind_grf(i),:),'--');
%             plot(GRFref(ind_grf(i),:)+GRFsdref(ind_grf(i),:),'--');         
%             hold off
%             pause
%             close
%         end        
%         %%%%%%%%%
%         %%%%%%%%%  
            
        J2 = c1_J*(1/((length(ind_gc)+length(ind_grf))*(2*n_nod-1)))*sum(sum((([Qsim(ind_gc,:)   ;  GRFsim(ind_grf,:)  ] - ...
                                                                               [Qref(ind_gc,:)   ;  GRFref(ind_grf,:)  ])./ ...
                                                                               [Qsdref(ind_gc,:) ;  GRFsdref(ind_grf,:)]).^2));   
                            
    else % bsymm = 0 - bilateral symmetry DEactivated
        hip_ang_r = [X(7,:) X(7,1)]; 
        knee_ang_r = [X(10,:) X(10,1)];
        ankle_ang_r = [X(11,:) X(11,1)];
        GRF_Y_r = [GRF(2,:) GRF(2,1)];
        GRF_Z_r = [GRF(3,:) GRF(3,1)];
        
        hip_ang_l = [X(13,:) X(13,1)]; 
        knee_ang_l = [X(16,:) X(16,1)];
        ankle_ang_l = [X(17,:) X(17,1)];
        GRF_Y_l = [GRF(8,:) GRF(8,1)];
        GRF_Z_l = [GRF(9,:) GRF(9,1)];       
        
        J2 = c1_J*(1/(5*(2*n_nod-1)))*sum(sum((([hip_ang_r;                  knee_ang_r;                  ankle_ang_r;                  GRF_Y_r;                  GRF_Z_r;                   hip_ang_l;                    knee_ang_l;                    ankle_ang_l;                    GRF_Y_l;                    GRF_Z_l] - ...
                                          [Prob.user.kin_W_hip_int;    Prob.user.kin_W_knee_int;    Prob.user.kin_W_ankle_int;    Prob.user.grf_W_Y_int;    Prob.user.grf_W_Z_int;     Prob.user.kin_W_hip_int_c;    Prob.user.kin_W_knee_int_c;    Prob.user.kin_W_ankle_int_c;    Prob.user.grf_W_Y_int_c;    Prob.user.grf_W_Z_int_c])./ ...
                                          [Prob.user.kin_W_hip_sd_int; Prob.user.kin_W_knee_sd_int; Prob.user.kin_W_ankle_sd_int; Prob.user.grf_W_Y_sd_int; Prob.user.grf_W_Z_sd_int;  Prob.user.kin_W_hip_sd_int_c; Prob.user.kin_W_knee_sd_int_c; Prob.user.kin_W_ankle_sd_int_c; Prob.user.grf_W_Y_sd_int_c; Prob.user.grf_W_Z_sd_int_c]).^2));    
        
    end 
    
else
    error('Tracking cost function not implemented to models other than legs2d or legs3d!')
end 

% Cost function
J = c1_J*J1 + c2_J*J2;