%FMINCON Finds a constrained minimum of a function of several variables.
%    FMINCON solves problems of the form:
%        min F(X)  subject to:  A*X  <= B, Aeq*X  = Beq (linear constraints)
%         X                       C(X) <= 0, Ceq(X) = 0   (nonlinear constraints)

%X=FMINCON(FUN,X0,A,B,Aeq,Beq,LB,UB) defines a set of lower and upper
%    bounds on the design variables, X, so that a solution is found in 
%    the range LB <= X <= UB. Use empty matrices for LB and UB
%    if no bounds exist. Set LB(i) = -Inf if X(i) is unbounded below; 
%    set UB(i) = Inf if X(i) is unbounded above.
%FUN can be specified using @:


function [A,C,fval] = mylp(h)
global A0
LB = zeros(6,1);
UB = ones(6,1);
Aeq = zeros(4,6);
Aeq(1,1) = 1; Aeq(1,2) = 1;
Aeq(2,3) = 1; Aeq(2,4) = 1;
Aeq(3,5) = 1; Aeq(3,6) = 1;
Aeq(4,1) = 1;
Beq = ones(4,1);
Beq(4) = A0(1,1); % removing a degree of freedom
%Aeq,Beq
%h = strict(A0);
Ain = zeros(2,6);
Ain(1,1) =  1; Ain(1,3) = -1;
Ain(2,1) = -1; Ain(2,3) =  1;
Bin = zeros(2,1)+h;
%Ain,Bin

%A1 = collapse(A0);
A1 = pnormdim(rand(2),1);
x0 = zeros(6,1);
x0(5:6) = ones(2,1)/2;
x0(1:4) = A1(:);
[xp,fvalp] = fmincon(@myobj   ,x0,Ain,Bin,Aeq,Beq,LB,UB)
[xn,fvaln] = fmincon(@negmyobj,x0,Ain,Bin,Aeq,Beq,LB,UB)
if abs(fvalp)>abs(fvaln)
  x = xp;
  fval = fvalp;
else
  x = xn;
  fval = fvaln;  
end
A = reshape(x(1:4),[2 2]);
C = x(5:6);
return

function y=negmyobj(x)
y = -myobj(x);
return

function A1 = collapse(A)
[n,n] = size(A);
p1 = A(:,1);
P1 = repmat(p1,[1 n]);
dd = sum(abs(A-P1),1);
[a,b] = max(dd);
A1(1,1) = A(1,1);
A1(2,1) = 1-A1(1,1);
A1(1,2) = A(b,1);
A1(2,2) = 1-A1(1,2);
return

function [h,i1,i2] = strict(A)
[n,n,T] = size(A);
h = 0;
i1 = 0;
i2 = 0;
for t=1:T
  %for i=1:n
  for i=1
    for j=i+1:n
      x = A(:,i);
      y = A(:,j);
      d = .5*sum(abs(x-y));
      if d>h
        i1=i;
        i2=j;
        h=d;
      end
    end
  end
end
return