% This code first solves ESDP to identify the top 20% most violated
% constraints, and then add to the ESDP_lb

clear

gamma = input(' Noise = ');
degbd = input(' Degree bound = ');
rgebd = input(' lb bound = ');

load Prob
load Prob1
load Prob2

m2 = n - m;
mu = 1e-2 + 30*gamma;
deg = sparse(m2,1);
dis = sparse(m2,1);
fin = sparse(m2,1);
fou = sparse(m2,1);
dd_select = zeros(n,n);

% Drop edges
for k = 1:nd
  i = head(k);
  j = tail(k);
  if i > m2
    head(k) = j;
    tail(k) = i;
    i = j;
    j = tail(k);
  end
  if j <= m2
    if ((deg(i) < degbd) && (deg(j) < degbd))
      deg(i) = deg(i) + 1;
      deg(j) = deg(j) + 1;
      nxtou(k) = fou(i);
      fou(i) = k;
      nxtin(k) = fin(j);
      fin(j) = k;
      dd_select(min(i,j),max(i,j)) = 1;
    end
  else
    nxtou(k) = fou(i);
    fou(i) = k;
    dd_select(i,j) = 1;
  end
end
dd = dd.*dd_select;
fprintf(' done dropping edges\n')

[Xiter,Z] = ESDPD(PP,m,dd);

% compute average distances of edges connected to each sensor
for i = 1:m2
  s = 0;
  dist = 0;
  k = fou(i);
  while k > 0
    s = s+1;
    dist = dist + d(k);
    k = nxtou(k);
  end
  k = fin(i);
  while k > 0
    s = s + 1;
    dist = dist + d(k);
    k = nxtin(k);
  end
  if s > 0
    dis(i) = (dist/s);
  else
    dis(i) = 0;
  end
end
trace_vio = Z - mu*dis.^2;
rmsd = sqrt(norm(PP(:,1:m2) - Xiter,'fro')^2/m2);
err = 0;
err_ap = 0;
for i = 1:m2
  error = norm(PP(:,i) - Xiter(:,i));
  err = max(err,error);
  if trace_vio(i) <= 0
    err_ap = max(err_ap,error);
  end
end
m_minus_m_ap = length(find(trace_vio > 0));
m_ap = m2 - m_minus_m_ap;
fprintf(' m_ap/err_ap/err/rmsd = %d/%2.1e/%3.2f/%2.1e\n\n', m_ap, err_ap, err, rmsd)

Sol = [Xiter PP(:,n-m+1:end)];

index = zeros(1,2);
k = 0;
count = 0;
for i = 1:n-m
  for j = i+1:n
    if rr(i,j) > 0
      k = k + 1;
      vio(k) = norm(Sol(:,i) - Sol(:,j))^2 - rr(i,j)^2; % compute excess
      index(k,:) = [i j]; % store indices
      if vio(k) < 0
        count = count + 1; % save violations
      end
    end
  end
end
fprintf(' done computing violated constraints\n')

if count == 0
  fprintf(' No lb constraints violated\n')
  return
end

number = min(ceil(count*0.2),m_minus_m_ap); % 20% of violations
[c,I] = sort(vio,'ascend');
J = I(1:number);
Final_index = index(J,:); % indices of most violated constraints

Select = sparse(n,n);
deg = sparse(n,1);
tmp = 0;
for ii = 1:length(J)
  i = Final_index(ii,1);
  j = Final_index(ii,2);
  if max(deg(i),deg(j)) < rgebd
    tmp = tmp + 1;
    deg(i) = deg(i) + 1;
    deg(j) = deg(j) + 1;
    Select(i,j) = 1; % select those 20%
  end
end
fprintf(' vio/vio_used = %d/%d\n\n', count, tmp)
rr = Select.*rr;

[Xiter,Z] = ESDPDlb(PP,m,dd,rr);
trace_vio = Z - mu*dis.^2;
rmsd = sqrt(norm(PP(:,1:m2) - Xiter,'fro')^2/m2);
err = 0;
err_ap = 0;
for i = 1:m2
  error = norm(PP(:,i) - Xiter(:,i));
  err = max(err,error);
  if trace_vio(i) <= 0
    err_ap = max(err_ap,error);
  end
end
m_minus_m_ap = length(find(trace_vio > 0));
m_ap = m2 - m_minus_m_ap;
fprintf(' m_ap/err_ap/err/rmsd = %d/%2.1e/%3.2f/%2.1e\n', m_ap, err_ap, err, rmsd)
fprintf(' done computing m - m_ap, %g\n', m_minus_m_ap)
