%%%%generate random ... generically NOT hard case???
%%% TRS min x'Ax+2b'x  s.t. x'x <= s2
%%% SDP: min   trace Abar*Xbar   s.t. trace Xbar <= 1+s2, Xbar_11 = 1
%%
clear all
n=2;
d=blkdiag(10*diag(rand(n,1)),-10*diag(rand(n,1)));
n=2*n;
[q,~]=qr(randn(n));
A=q'*d*q;
D=sort(diag(d),'ascend');
%muu=max(-(D(1)+D(2))/2,(D(1)+D(2))/2);   % must be > 0 for n=2
muu=-(D(1)+D(2))/2;   % must be > 0
Hess=A+muu*eye(n);   % should have exactly one neg eig.
[v,d]=eig(Hess);
[~,ind]=sort(diag(d),'descend');
xl=10*(rand+.1)*v(:,ind(1));
%%%Xbar=[x_0 x'
%%%      x X];
b=-(A+muu*eye(n))*xl;
Abar=[0 b'
      b A];
s2=norm(xl)^2;
n1=n+1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%generate a loop for solving with increasing s2 to test Lemma 3.4 Martinez
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%for s2=s2:1:100*s2,
cvx_begin  quiet sdp
   cvx_precision best
   variable Xbar(n1,n1) symmetric;
   dual variable lambda;
   minimize trace(Abar*Xbar);
   subject to
            Xbar(1,1)-1==0;
            trace(Xbar)-1 - s2 <= 0: lambda;
	    Xbar>=0;
cvx_end
fprintf(' ')
xtrs=Xbar(2:end,1);
[xv,~]=eig(Xbar);
xtrse=xv(:,end);
xtrse=xtrse/xtrse(1);
xtrse=xtrse(2:end);
pfeas=norm(xtrs)^2-s2;
pfease=norm(xtrse)^2-s2;
if max(pfeas,pfease) > 1e-11,
	fprintf('error in pfeas value \n')
	keyboard
end
pvaltrs=xtrs'*A*xtrs+2*b'*xtrs;
pvale=xtrse'*A*xtrse+2*b'*xtrse;
dfeas=norm( (A+lambda*eye(n))*xtrs +b)/(norm(b)+1);  %%relative stationarity 
dfease=norm( (A+lambda*eye(n))*xtrse +b)/(norm(b)+1);  %%relative stationarity 
if max(dfeas,dfease) > 1e-6,
	fprintf('error in rel. dfeas value \n')
	keyboard
end
fprintf('\n \n starting with new trs radius s2   %g  \n',s2)
fprintf('primal feasibility ||xtrs||^2-s2 %g  \n',pfeas)
fprintf('rel. dual feasibility/stationarity %g \n',dfeas)
fprintf('pval(x),pval(Xbar)  %g %g  \n',pvaltrs,trace(Abar*Xbar))
if abs(pvaltrs-trace(Abar*Xbar))/(abs(pvaltrs)+1)>1e-8,
	fprintf('error in pvaltrs values \n')
	keyboard
end
mineig=min(eig(A+lambda*eye(n)));
fprintf('min eig Hessian Lagr %g \n',mineig)
if mineig<0,
	fprintf('error in mineig %g \n',mineig)
	keyboard
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for shift=.001:.001:.99,
%%%%now add a linear constraint to cut off global min. xtrs
%%%  let bb=(1-shift)*xtrs and alpha= norm(bb)^2
%%% equation is 0 <= alpha-bb'x  note that alpha =shift^2*delta 
%%%                 so xtrs is infeas.
%%% Add the second order cone constraint
%%%  norm(alpha*x-X*bb)<=alpha-bb'x
bb=(1-shift)*xl;
alpha=norm(bb)^2;
cvx_begin quiet sdp
   cvx_precision best
   %variable Xbar(n1,n1) symmetric;
   variable X(n,n) symmetric;
   variable x(n);
   dual variable muuu;
   minimize trace(A*X)+2*(b'*x);
   subject to
            %Xbar(1,1)-1==0;
            %trace(Xbar)-1 - s2 <= 0: lambda;
            trace(X) - s2 <= 0: muuu;
	    norm(alpha*x-X*bb)-(-alpha+bb'*x) <= 0;
	    %Xbar>=0;
	    [1 x'
	      x X ] >=0;
cvx_end
fprintf(' ')
pfeas=norm(x)^2-s2;
pfease=norm(x)^2-s2;
if max(pfeas) > 1e-11,
	fprintf('error in pfeas value \n')
	keyboard
end
pvalx=x'*A*x+2*b'*x;
fprintf('\n \n starting with new shift   %g  \n',shift)
fprintf('primal feasibility ||x||^2-s2 %g  \n',pfeas)
mineig=min(eig(A+lambda*eye(n)));
fprintf('min eig Hessian Lagr %g \n',mineig)
if mineig<0,
	fprintf('error in mineig %g \n',mineig)
	keyboard
end
lam=sort(eig(A+muuu*eye(n)),'ascend');
fprintf('lam1 muuu lam2 for Hessian Lagr %g  %g  %g \n',lam(1),muuu,lam(2))
linineq=-alpha+bb'*x;
fprintf('value of linear inequality - test *active*  %g  \n',linineq)
if linineq>1e-3,
	fprintf('lin constr NOT active \n')
	keyboard
end
keyboard
end   %   end for 'shift'
%end   %   end for s2 increases
