%%%%generate a random ... (note generically NOT hard case)
%%% TRS min x'Ax+2b'x  s.t. x'x <= s2=1
%%% SDP: min   trace Abar*Xbar   s.t. trace Xbar <= 1+s2=2, Xbar_11 = 1
%%
clear all
n=2;
d=blkdiag(10*diag(rand(n,1)),-10*diag(rand(n,1)));
%d(end)=d(end)+2*min(d(n+1:end));  % min eig is made smaller
n=2*n;
xl=randn(n,1)+1;
xl=xl/norm(xl);
[q,~]=qr(randn(n));
A=q'*d*q;
D=sort(diag(d),'ascend');
[q,~]=qr(xl);
V=q(:,2:end);    % want V'*Hess*V psd
lamV=max(-D(2),-min(eig(V'*A*V)));   % -D(1)  > muu > lamV
muu=(lamV-D(1))/2;
if min([muu (muu-lamV) (-D(1)-muu)])<0,
	fprintf('error in muu calculation??? \n')
	keyboard
end
Hess=A+muu*eye(n);   % should have exactly one neg eig.
%%%Xbar=[x_0 x'
%%%      x X];
b=-(A+muu*eye(n))*xl;
Abar=[0 b'
      b A];
s2=1;   % xl is norm 1
%%%%%verify local min  sufficient conditions
if abs(norm(xl)-1)>1e-12,
	fprintf('error in xl feasibility - not active wrt unit ball \n')
	keyboard
end
[q,~]=qr(xl);
V=q(:,2:end);
if min(eig(V'*Hess*V))<=0,
	fprintf('error in 2nd order suff cond for xl \n')
	keyboard
end
if norm(b+(A+muu*eye(n))*xl)>1e-13,
	fprintf('error in stationarity for xl \n')
	keyboard
end
if muu<=0,
	fprintf('error in muu not > 0 \n')
	keyboard
end
fprintf('xl successfully passed sufficient local optimality conditions \n')
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);   % use eigenvalue approx. ?
xtrse=xtrse/xtrse(1);
xtrse=xtrse(2:end);
pfeas=norm(xtrs)^2-s2;
pfease=norm(xtrse)^2-s2;
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-6,
	fprintf('error in pvaltrs values \n')
	keyboard
end
mineig=min(eig(A+lambda*eye(n)));
fprintf('min eig Hessian Lagr %g \n',mineig) % check 2nd order cond.
if mineig<0,
	fprintf('error in mineig %g \n',mineig)
	keyboard
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for shift=.01:.01:.99,
%%%%now add a linear constraint to cut off local noglobal min. xl
%%%  let bb=(1-shift)*xl and alpha= norm(bb)^2
%%% equation is bb'x >= alpha  note that xl is feasible
%%% Add the second order cone constraint
%%%  norm(alpha*x-X*bb)<=bb'x-alpha
bb=(1-shift)*xl;
alpha=norm(bb)^2;
cvx_begin quiet sdp
   cvx_precision best
   cvx_solver sedumi
   %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)-(bb'*x-alpha) <= 0;
	    %Xbar>=0;
	    [1 x'
	      x X ] >=0;
cvx_end
fprintf(' ')
pfeas=norm(x)^2-s2;
if (pfeas) > 1e-11,
	fprintf('error in pfeas value %g \n',pfeas)
	%keyboard
end
pvalx=x'*A*x+2*b'*x;
pvalxtrs=xtrs'*A*xtrs+2*b'*xtrs;
pvalxl=xl'*A*xl+2*b'*xl;
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=bb'*x-alpha;
fprintf('value of linear inequality - test *active*  %g  \n',linineq)
fprintf('****optimal values for xl x xtrs are %g %g %g \n ',...
                          pvalxl,pvalx,pvalxtrs)
if abs(pvalxl-pvalx)> 1e-8,
    fprintf('local-nonglobal and SOCP pvals differ with shift size %g - continue shifting? \n ',shift)
    keyboard
else
    fprintf('local-nonglobal min found successfully with shift size %g \n',...
                    shift);
end
'before end'
keyboard
end   %   end for 'shift'
%end   %   end for s2 increases
