function [dObj, dObjGrad] = obj_costOfTransport(state,control,p)
% [dObj, dObjGrad] = obj_torqueSquared(state,control)
%
% This function computes the integrand of the cost of transport integral
%

empty = zeros(size(state(1,:)));

u = state(11:15,:);
du = control(1:5,:);
sn = control(6:10,:);
sp = control(11:15,:);

if nargout == 1 % numerical gradients
    
    dObj = autoGen_obj_costOfTransport(...
        p.m1,p.m2,p.m3,p.m4,p.m5,...
        u(1,:),u(2,:),u(3,:),u(4,:),u(5,:),...
        du(1,:),du(2,:),du(3,:),du(4,:),du(5,:),...
        sn(1,:),sn(2,:),sn(3,:),sn(4,:),sn(5,:),...
        sp(1,:),sp(2,:),sp(3,:),sp(4,:),sp(5,:),...
        p.g,p.stepLength,p.gammaNeg,p.gammaPos, p.alpha,p.beta,empty);
    
else  %Analytic gradients
    
    [dObj,fz,fzi] = autoGen_obj_costOfTransport(...
        p.m1,p.m2,p.m3,p.m4,p.m5,...
        u(1,:),u(2,:),u(3,:),u(4,:),u(5,:),...
        du(1,:),du(2,:),du(3,:),du(4,:),du(5,:),...
        sn(1,:),sn(2,:),sn(3,:),sn(4,:),sn(5,:),...
        sp(1,:),sp(2,:),sp(3,:),sp(4,:),sp(5,:),...
        p.g,p.stepLength,p.gammaNeg,p.gammaPos,p.alpha,p.beta,empty);
    
    dObjGrad = zeros(31,length(dObj));  % 26 = 1 + (5 + 5 + 5) + (5 + 10) = time + angle + rate + torque + control + slack
    dObjGrad(fzi,:) = fz;
    
end

end