function J = obj_func_DC_d(P,Prob)
% returns the objective function to SNOPT
% J = deviations from kinematics (hip, knee, and ankle angles) and of grfs
%     of data in Winter (1991)
% Note: the implementation is speecific for legs2d or legs3d
% Last modification: 10/08/2008 by Marko Ackermann

n_nod = Prob.user.n_nod;
model = Prob.user.model; % specifies 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
c1_J = Prob.user.c1_J; % scaling of cost function

% Extract the matrix of states and the matrix of neural excitations from 
% the vector of opt. 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
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
U = zeros(n_m,n_nod-1); % set to zero because stimulations have no direct influence on THIS cost function

% 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)];
        
        J = 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)];       
        
        J = 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
        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
        
        J = 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));    
        
    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)];       
        
        J = 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                      