gpopsObjandCons(x)

function C = gpopsObjandCons(x)
%------------------------------------------------------------------%
% Compute the nonlinear constraints and objective function         %
%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David   %
% Benson, Michael Patterson, Christopher Darby, & Camila Francolin %
%------------------------------------------------------------------%

global mysetup;

Cons = cell(mysetup.numphases,1);
endpoint_info = cell(mysetup.numphases,5);
Cost = 0;
for i=1:mysetup.numphases
    xcurr = x(mysetup.variable_indices{i});
    nstates = mysetup.sizes(i,1);
    ncontrols = mysetup.sizes(i,2);
    % nparameters = mysetup.sizes(i,3);
    % npaths = mysetup.sizes(i,4);
    nevents = mysetup.sizes(i,5);
    t0 = xcurr(mysetup.indices(i).time(1));
    tf = xcurr(mysetup.indices(i).time(2));
    state_vector = xcurr(mysetup.indices(i).states);
    control_vector = xcurr(mysetup.indices(i).controls);
    tau_all = [-1; mysetup.ps{i,2}; 1];
    t_all = (tf-t0)*(tau_all+1)/2+t0;
    t_gauss = t_all(2:end-1);
    state_matrix = reshape(state_vector,mysetup.nodes(i)+2,nstates);
    state_gauss = state_matrix(2:end-1,:);
    x0 = state_matrix(1,:);
    xf = state_matrix(end,:);
    control_gauss = reshape(control_vector,mysetup.nodes(i),ncontrols);
    parameters = xcurr(mysetup.indices(i).parameters);
    dae_use = {t_gauss,state_gauss,control_gauss,parameters};
    endpoint_use = {t0,x0.',tf,xf.',parameters};
    dae_out = feval(mysetup.funcs{2},dae_use,i);
    odelhs  = mysetup.ps{i,4}*state_matrix(1:mysetup.nodes(i)+1,:);
    oderhs  = (tf-t0)*dae_out(:,1:nstates)/2;
    paths   = dae_out(:,nstates+1:end);
    ode_defects = odelhs-oderhs;
    quad_defects = zeros(size(xf));
    defects = [ode_defects; quad_defects];
    if nevents>0,
        events = feval(mysetup.funcs{3},endpoint_use,i);
    else
        events = [];
    end;
    endpoint_info(i,:) = endpoint_use;
    Cons{i,1} = [defects(:); paths(:); events];
    cost_use(1,:) = {t0,x0.',tf,xf.'};
    cost_use(2,:) = {t_gauss,state_gauss,control_gauss,parameters};
    [Mayer,Lagrange] = feval(mysetup.funcs{1},cost_use,i);
    integrand = (tf-t0)*mysetup.ps{i,3}*Lagrange/2;
    Cost = Cost + Mayer + integrand;
end;
Constraints = vertcat(Cons{:,1});
if ~isempty(mysetup.connections),
    link_out = cell(mysetup.numconnections,1);
    for i=1:mysetup.numconnections;
    	left_phase  = mysetup.connections{i,3}(1);
        right_phase = mysetup.connections{i,3}(2);
        xf_left = endpoint_info{left_phase,4};
        p_left  = endpoint_info{left_phase,5};
        x0_right = endpoint_info{right_phase,2};
        p_right  = endpoint_info{right_phase,5};
        link_info = {xf_left,p_left,x0_right,p_right};
        link_out{i,1} = feval(mysetup.funcs{4},link_info,left_phase,right_phase);
    end;
    Clink = vertcat(link_out{:,1});
    Constraints = [Constraints; Clink];
end;
% Combine the Constraints with the Cost Function
C = [Cost; Constraints; mysetup.initlincons];