function [dsol,lamsol,objvalsol] = qcqp2(Q,Q1,Q2,q,q1,q2,gam1,gam2)
% solves QCQP with two constraints 
% minimize x'*Q*x/2 + q'*x 
% subject to 
% x'*Q1*x/2 + q1'*x + gam1 <= 0, 
% x'*Q2*x/2 + q2'*x + gam2 <= 0. 
%
% S. Sakaue and Y. Nakatsukasa, 2015



%% construct matricees 
n = size(Q,1);
O = zeros(n); o = zeros(n,1); 

C1 = [Q1 -Q q1;-Q O -q;q1' -q' 2*gam1];
C2 = [Q2 -Q q2;-Q O -q;q2' -q' 2*gam2];
D1 = [O -Q1 o;-Q1 O -q1;o' -q1' 0];
D2 = [O -Q2 o;-Q2 O -q2;o' -q2' 0];

% compute projection vectors
%v1 = [(Q2\q2);o;-1];
if cond(Q1)<1e14
v2 = [(Q1\q1);o;-1];
else
    %{
[U,S,V] = svd(Q1);
S = diag(S);
[ix] = find(S/max(S)<1e-14);
v2 = [V(:,ix);zeros(n+1,length(ix))];    
    %}
% or simply work with D1 to handle more singularity
[U,S,V] = svd(D1);
[ix] = find(S/max(S)<1e-14);
v2 = V(:,ix);
end

Muc = kron(C1,D1) - kron(D1,C2);
Mulin = kron(D2,D1) - kron(D1,D2);

%svd(Muc+randn*Mulin) % check singular
%spy(Muc+randn*Mulin),shg


vv1 = kron(v2,v2);
[norm(Mulin*vv1) norm(Muc*vv1)] % check 0
[Qproj,~] = qr(vv1);Qproj = Qproj(:,size(vv1,2)+1:end);
%Qproj2  = Qproj;

[QD,~] = qr(v2); kk = size(v2,2); mm = size(v2,1);
%xx = 1:kk; 
xx = kk+1:mm;
ix =[];
for ii = 1:kk
ix = [ix mm*(ii-1)+xx];
end
ix = [ix ix(end)+1:mm^2];
Qproj2 = kron(QD,QD); 
Qproj2 = Qproj2(:,ix); 

prjMuc = Qproj2'*Muc*Qproj2;
prjMulin = Qproj2'*Mulin*Qproj2;

mu= eig(prjMuc,-prjMulin);


%% lam*mu != 0 case

[sum(svd(prjMulin)/norm(prjMulin)<1e-12) sum(abs(eig(prjMuc,-prjMulin))>1e7)]
%keyboard
%muini = mu; 
mu = mu(abs(imag(mu))<1e-2);
mucandidates = mu(abs(mu)<1e15); %we need tol(=100) evaluation



kktcandidates = [];
mindis = 1e10*ones(1,length(mucandidates)); %set mindis sufficiently large at first
for ii = 1:length(mucandidates)
mu = mucandidates(ii);

M1c = C1 + mu*D2;
lams1 = eig(M1c,-D1);
lams1 = lams1(abs(imag(lams1))<1e-2);
lams1 = real(lams1);
if isempty(lams1)
    lams1=nan;
end


M2c = C2 + mu*D2;
lams2 = eig(M2c,-D1);
lams2 = lams2(abs(imag(lams2))<1e-2);
lams2 = real(lams2);
if isempty(lams2)
    lams2=nan;
end
for ij = 1:length(lams1)
%    mindis(ii) = min(mindis(ii),min(abs(lams1(ij)-lams2)));
    mindis(ii) = min(min(abs(lams1(ij)-lams2))); % changed on 12/14
    
if mindis(ii)<1e-4
    [~,ix] = min(abs(lams1(ij)-lams2));
    kktcandidates = [kktcandidates;lams1(ij) mu];
    %break %removed on 12/14
end    
end



end
mindis

kktcandidates
kktcandidatesini = kktcandidates;
% bezout done
%% lambda*mu=0 cases

%lam=0
Lam0c = C2;
Lam0lin = D2;
mu_lam0 = eig(Lam0c,-Lam0lin);
mu_lam0 = mu_lam0(abs(mu_lam0)<1e14);

for i = 1:length(mu_lam0)
    kktcandidates = [kktcandidates;0 mu_lam0(i)];
end

%mu=0
Mu0c = C1; 
Mu0lin = D1;
%Mu0 = Mu0c+randn*Mu0lin;
lam_mu0 = eig(Mu0c,-Mu0lin);
lam_mu0 = lam_mu0(abs(lam_mu0)<1e14);

for i = 1:length(lam_mu0)
    kktcandidates = [kktcandidates;lam_mu0(i) 0];
end

%lam=mu=0
kktcandidates = [kktcandidates;0 0];
%% lam0=0 in Fritz John condition
%
fjcandidates = [];
fjcandidates = [fjcandidates ; 0 1; 1 0];

E1 = [Q1 -Q1 q1;-Q1 O -q1;q1' -q1' 2*gam1];
E2 = [Q2 -Q1 q2;-Q1 O -q1;q2' -q1' 2*gam2];

%rho=mu/lam
rho1 = eig(E1,-D2);
rho1 = rho1(abs(imag(rho1))<1e-2);
rho1 = real(rho1);
rho2 = eig(E2,-D2);
rho2 = rho2(abs(imag(rho2))<1e-2);
rho2 = real(rho2);

%ij = 1;
for i=1:length(rho1)
    for j=1:length(rho2)
        %resrho(ij)=rho1(i)-rho2(j); ij=ij+1;
        if abs(rho1(i)-rho2(j))<1e-10 && abs(rho1(i))<1e14 && rho1(i) >= -1e-10 %need to discuss
            fjcandidates = [fjcandidates ; 1 rho1(i)];
        end
    end
end
%}


%% calculate KKT points

kktcandidates = real(kktcandidates)

jjj=1;
 for i=1:length(kktcandidates(:,1)) 
     if kktcandidates(i,1)>-1e-3 & kktcandidates(i,2)>-1e-3 
         lamcand(jjj,1)=kktcandidates(i,1); 
         lamcand(jjj,2)=kktcandidates(i,2);
         jjj=jjj+1;
     end
 end

toll = 1e-3;
toll1 = 1e-5;
toll2 = 1e-13;
objvalsol = inf; 
negeigvec=[];

'       lam       mu          res1        res2         cond(H)       objval'
for ii = 1:size(kktcandidates,1)
    lamhere = real(kktcandidates(ii,1));
    muhere = real(kktcandidates(ii,2));
    H = (Q+lamhere*Q1+muhere*Q2); 
    y = -q-lamhere*q1-muhere*q2;
    x = H\y;
    d = x;
    x=real(x); % added 11/13
    
    d = real(d); 
    %if cond(H)<1e7 ,

    if lamhere >= -toll & muhere >= -toll
         
    
    % pertub solution so that g1,g2<1e-8
    dx = zeros(n,1);        
    g1 = x'*Q1*x/2+q1'*x+gam1;
    g2 = x'*Q2*x/2+q2'*x+gam2;
    if g1>toll2 | g2>toll2
        a1 = max([0,g1]); a2 = max([0,g2]);
        dx = -pinv([x'*Q1+q1' ; x'*Q2+q2'])*[a1;a2];
        x=x+dx;
        d=x;
    end
            
    if x'*Q1*x/2+q1'*x+gam1<toll2 & x'*Q2*x/2+q2'*x+gam2<toll2 %& norm([abs(lamhere)*norm(x'*Q1*x/2+q1'*x+gam1) abs(muhere)*norm(x'*Q2*x/2+q2'*x+gam2)])< toll1   
    
    disp([lamhere muhere  x'*Q1*x/2+q1'*x+gam1 x'*Q2*x/2+q2'*x+gam2  cond(H) x'*Q*x/2+q'*x])
    objvalsol = min(objvalsol,x'*Q*x/2+q'*x);
    
    if norm([abs(lamhere)*norm(x'*Q1*x/2+q1'*x+gam1) abs(muhere)*norm(x'*Q2*x/2+q2'*x+gam2)])< toll
      eigH = eig(H);
      negeig = length(eigH(eigH<-1e-8));
      negeigvec=[negeigvec ; negeig];  
    end
    
    end
       
    else
        %'badinfo',disp([lamhere muhere norm([abs(lamhere)*norm(x'*Q1*x/2+q1'*x+gam1) abs(muhere)*norm(x'*Q2*x/2+q2'*x+gam2)])       x'*Q1*x/2+q1'*x+gam1 x'*Q2*x/2+q2'*x+gam2 cond(H)])
    end
if objvalsol == x'*Q*x/2+q'*x
    lamsol = [lamhere muhere];
    dsol = d;
end
end




%% calculate Fritz John point with lam0=0
%{
fjcandidates = real(fjcandidates)

'       lam       mu          res1        res2         cond(H)       objval'
for ii = 1:size(fjcandidates,1)
    lamhere = real(fjcandidates(ii,1));
    muhere = real(fjcandidates(ii,2));
    H = (lamhere*Q1+muhere*Q2); 
    y = -lamhere*q1-muhere*q2;
    x = H\y;
    x=real(x); % added 11/13
    d = x;
    toll = 1e-3;
    d = real(d); 
    %if cond(H)<1e7 ,
    if norm([abs(lamhere)*norm(x'*Q1*x/2+q1'*x+gam1) abs(muhere)*norm(x'*Q2*x/2+q2'*x+gam2)])< toll & ...
            x'*Q1*x/2+q1'*x+gam1<toll & x'*Q2*x/2+q2'*x+gam2<1e-3 & lamhere >= -toll & muhere >= -toll
disp([lamhere muhere  x'*Q1*x/2+q1'*x+gam1 x'*Q2*x/2+q2'*x+gam2  cond(H) x'*Q*x/2+q'*x+gam])        
    objvalsol = min(objvalsol,x'*Q*x/2+q'*x+gam);
        
%chebfunsettingyy = kktcandidates;
plot(lamhere,muhere,'k.','markersize',12),shg,hold on
    else
        %'badinfo',disp([lamhere muhere norm([abs(lamhere)*norm(x'*Q1*x/2+q1'*x+gam1) abs(muhere)*norm(x'*Q2*x/2+q2'*x+gam2)])       x'*Q1*x/2+q1'*x+gam1 x'*Q2*x/2+q2'*x+gam2 cond(H)])
        %keyboard
    end
if objvalsol == x'*Q*x/2+q'*x+gam 
    lamsol = [lamhere muhere];
    dsol = d;
end
end

if exist('lamsol','var')
    plot(lamsol(1),lamsol(2),'rx','markersize',12),shg,hold on    
end
grid on



%}
