function [x,fval,exitflag,output] = mincontest()

% Test fmincon for direct collocation point mass example.
%

global N ix0 ixN idx0 idxN ifrc0 ifrcN

% Number of knot points.
N = 11;

% The indices of the first and last position knot point variables,
% velocity knot point variables, and force knot point variables.
%
ix0 = 1;
ixN = N;
idx0 = N+1;
idxN = 2*N;
ifrc0 = 2*N+1;
ifrcN = 3*N-1;

% Time increment between knot points.
%
h = 1 / (N-1);

% Starting guess:
% Guess that positions move linearly, velocities are 1, forces are 0.
x0 = [ linspace(0,1,N)'; ones(N,1); zeros(N-1,1) ];

% Linear inequalities. There are none. We'll use lower and upper
% bounds to keep the variables in a reasonable range.
A = [];
b = [];

% Linear equalities. Use these to set the start and end conditions
% for the point mass, and to enforce the constraints between the
% knot point variables
% There 2*N+4 constraints:
% - N for relating positions to velocities
% - N for relating velocities to forces
% - 2 for the starting and ending positions
% - 2 for the starting and ending velocities
%
Aeq = zeros(2*N+4,3*N-1);
beq = zeros(2*N+4,1);
cnum = 1;

% point starts at 0.
Aeq(cnum,ix0) = 1;
beq(cnum) = 0;
cnum = cnum+1;

% point ends at 1.
Aeq(cnum,ixN) = 1;
beq(cnum) = 1;
cnum = cnum+1;

% velocity starts at 0.
Aeq(cnum,idx0) = 1;
beq(cnum) = 0;
cnum = cnum +1;

% ...and ends at 0.
Aeq(cnum,idxN) = 1;
beq(cnum) = 0;
cnum = cnum +1;


for ix = (ix0):(ixN-1)
  idx = ix + N;
  ifrc = ix + 2*N;

  % Set up p<>v and v<>a constraints for the interior knot points
  % For a nonlinear system we wouldn't be able to do this.
  %

  % position to velocity: 2 p_{i+1} - 2 p_i - h v_{i+1} - h v_i = 0
  Aeq(cnum,ix+1) = 2;
  Aeq(cnum,ix) = -2;
  Aeq(cnum,idx+1) = -h;
  Aeq(cnum,idx) = -h;
  beq(cnum) = 0;
  cnum = cnum+1;
  
  % velocity to force: v_{i+1} - v_i - h f_i = 0
  Aeq(cnum,idx) = -1;
  Aeq(cnum,idx+1) = 1;
  Aeq(cnum,ifrc) = -h;
  beq(cnum) = 0;
  cnum = cnum+1;
end

% Reasonable bounds are
% [0,1] for the position variables,
% [0,10] for the velocities
% [-10,10] for the forces
lb = [ zeros(N,1); zeros(N,1); -10 * ones(N-1,1) ];
ub = [ ones(N,1); 10*ones(N,1); 10 * ones(N-1,1) ];

%keyboard;

[x,fval,exitflag,output] = fmincon(@objective,x0, A,b,Aeq,beq,lb,ub);

% Constraint satisfaction
%
%disp(Aeq * x - beq)

% objective(x)


function [f,g] = objective(vec)

global N ix0 ixN ifrc0 ifrcN

  % The value of the minimization is the sum of applied forces
  %
  f = sum(vec(ifrc0:ifrcN).^2);
  
  % Need gradient?
  %
  if nargout > 1
    g = zeros(3*N-1,1);

    % The position and velocity variables don't affect the value directly,
    % but the forces do.
    %
    g(ifrc0:ifrcN) = 2 * vec(ifrc0:ifrcN);
    
  end