% Matthew McNaughton for KDC, 16-711. Feb 13 2007
% DOESN'T WORK. Won't converge.
%
function [x,fval,exitflag,output] = mincontest2(x0)

% Test fmincon for direct collocation point mass example.
% This one treats the cost as a state variable with nonlinear constraints.
%

global N ix0 ixN iddx0 iddxN ifrc0 ifrcN1 icost0 icostN h

% 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.
% There is one less force variable.
%
ix0 = 1;
ixN = N;
idx0 = N+1;
idxN = 2*N;
ifrc0 = 2*N+1;
ifrcN1 = 3*N-1;
icost0 = 3*N;
icostN = 3*N + N-1;

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

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


% 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+5 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
% - 1 for the starting cost
%
Aeq = zeros(2*N+5,4*N-1);
beq = zeros(2*N+5,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;

% Cost starts at 0.
Aeq(cnum,icost0) = 1;
beq(cnum) = 0;
cnum = cnum +1;

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

  % Set up linear p<>v and v<>a constraints for the interior knot points
  %

  % position to velocity: 2 p_{i+1} - 2 p_i - h v_{i+1} - h v_i = 0
  Aeq(cnum,ix+1) = 2/h;
  Aeq(cnum,ix) = -2/h;
  Aeq(cnum,idx+1) = -1;
  Aeq(cnum,idx) = -1;
  beq(cnum) = 0;
  cnum = cnum+1;
  
  % velocity to force: v_{i+1} - v_i - h f_i = 0
  Aeq(cnum,idx+1) = 1/h;
  Aeq(cnum,idx) = -1/h;
  Aeq(cnum,ifrc) = -1;
  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
% [0,100] for the cost
lb = [ zeros(N,1); zeros(N,1); -10 * ones(N-1,1); zeros(N,1) ];
ub = [ ones(N,1); 10*ones(N,1); 10 * ones(N-1,1); 100*ones(N,1) ];

%keyboard;

options = optimset('MaxFunEvals', 10000);

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

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



function [f,g] = objective(vec)

global N ix0 ixN ifrc0 ifrcN1 icost0 icostN


  % The value of the minimization is the cost at the end.
  %
  f = 10*vec(icostN);
  
  % Need gradient?
  %
  if nargout > 1
    g = zeros(4*N-1,1);

    % Only the cost variable at final time directly affects the
    % objective value.
    %
    g(icostN) = 10;
  end
  
% c is the nonlinear inequality constraints
% ceq is the nonlinear equality constraints
%
function [c,ceq] = nonlcon(vec)

global N ix0 ixN ifrc0 ifrcN1 icost0 icostN h


  % No inequality constraints
  c = [];
  
  % There is one constraint for each cost after the first.
  %
  ceq = zeros(N-1,1);
  
  for i = 1:(N-1)
    icost = icost0+i-1;
    ifrc = ifrc0+i-1;
    value = vec(icost+1) - vec(icost) - vec(ifrc)^2;
    ceq(i) = value;
  end

  if nargout > 2
    GC = [];

    GCeq = zeros(N,N-1);
    
    for i = 1:(N-1)
      icost = icost0+i-1;
      ifrc = ifrc0+i-1;
      %value = vec(icost+1) - vec(icost) - vec(ifrc)^2;
      GCeq(icost+1,i) = 1;
      GCeq(icost,i) = -1;
      GCeq(ifrc,i) = -2 * vec(ifrc);
    end
  end