% Dec 2008
% This matlab code reads in an interior solution of the rho ESDP
% relaxation of the sensor localization problem. For details of formulation
% see the fortran code sensor_esdp_routine.f
% It displays the solution and tests which sensors are 
% accurately localized.
% It takes Prob1.mat and sensorsoln.dat.
clear
% read in  n m dim  x nd d head tail
load('Prob1.mat');
fprintf(' *********************************************\n')
fprintf(' %g pts,  %g anchors \n',n,m);
gamma = input(' noisefactor (%)= ');
degbd = input(' degree bound = ');
% read in interior solution
load sensorsoln.dat;
sol = sensorsoln';
draw = input(' 1 for drawing fixed points, 2 for drawing all points, 3 for no drawing ');
fprintf(' *********************************************\n')
% calculate square error of solution
err_soln = 0;
err_soln3 = 0;  
err_soln2 = 0;  
err_soln4 = 0;
m2 = n - m;
mu = 1e-2 + 30*gamma;
nfix = 0;
deg = sparse(m2,1);
dis = sparse(m2,1);
fin = sparse(m2,1);
fou = sparse(m2,1);

% drop edges as in the code sensor_esdp.f
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;
    end
  else
    nxtou(k) = fou(i);
    fou(i) = k;
  end
end

% 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

for i = 1:m2
  err_soln = err_soln + norm(sol(1:2,i)-x(:,i))^2;
  err_soln3 = max(err_soln3,norm(sol(1:2,i)-x(:,i)));
  % test accuracy
  if sol(3,i) <= mu*dis(i)^2
    nfix = nfix + 1;
    err_soln2 = max(err_soln2,norm(sol(1:2,i)-x(:,i)));
    err_soln4 = err_soln4 + norm(sol(1:2,i)-x(:,i))^2;
    if draw == 1
      plot(x(1,i),x(2,i),'g*',sol(1,i),sol(2,i),'b.',x(1,m2+1:n),x(2,m2+1:n),'o');
      hold on
      line([x(1,i);sol(1,i)],[x(2,i);sol(2,i)]);
      hold on
    end
  end
end
tr = sol(3,:);
fprintf(' RMSD error & max-norm error over all pts: %g, %g \n',sqrt(err_soln/m2),err_soln3); 
fprintf(' nfix, rms error & max-norm error over fixed points: %g, %g ,%g\n', nfix, sqrt(err_soln4/nfix), err_soln2);
fprintf(' nfix/err_ap/err/rmsd = %g/%2.1e/%3.2f/%2.1e\n',nfix, err_soln2, err_soln3, sqrt(err_soln/m2));
if draw == 2
  for i = 1:m2
    plot(x(1,i),x(2,i),'g*',sol(1,i),sol(2,i),'b.',x(1,m2+1:n),x(2,m2+1:n),'o');
    hold on
    line([x(1,i);sol(1,i)],[x(2,i);sol(2,i)]);
    hold on
  end
end
 
fprintf(' *********************************************\n')
s0 = input(' type 1 to see improvement from range constraints, otherwise to skip this part ');
if s0 == 1
  % read in interior solution
  load sensorsoln3.dat;
  sol3 = sensorsoln3';
  draw = input(' 1 for drawing fixed points, 2 for drawing all points, 3 for no drawing ');
  % calculate square error of solution
  err_soln = 0;
  err_soln3 = 0;  
  err_soln2 = 0;  
  err_soln4 = 0;
  m2 = n - m;
  nfix = 0;

  for i = 1:m2
    err_soln = err_soln + norm(sol3(1:2,i)-x(:,i))^2;
    err_soln3 = max(err_soln3,norm(sol3(1:2,i)-x(:,i)));
    % test accuracy
    if sol3(3,i) <= mu*dis(i)^2
      nfix = nfix + 1;
      err_soln2 = max(err_soln2,norm(sol3(1:2,i)-x(:,i)));
      err_soln4 = err_soln4 + norm(sol3(1:2,i)-x(:,i))^2;
      if draw == 1
        plot(x(1,i),x(2,i),'g*',sol3(1,i),sol3(2,i),'m.',x(1,m2+1:n),x(2,m2+1:n),'o');
        hold on
        line([x(1,i);sol3(1,i)],[x(2,i);sol3(2,i)]);
        hold on
      end
    end
  end
  tr = sol3(3,:);
  fprintf(' RMSD error & max-norm error over unfixed pts (range): %g, %g \n',sqrt(err_soln/m2),err_soln3); 
  fprintf(' nfix, rms error & max-norm error over newly fixed points (range): %g, %g ,%g\n', nfix, sqrt(err_soln4/nfix), err_soln2);
  fprintf(' nfix/err_ap/err/rmsd=%g/%2.1e/%3.2f/%2.1e\n',nfix, err_soln2, err_soln3, sqrt(err_soln/m2));
  if draw == 2
    for i = 1:m2
      plot(x(1,i),x(2,i),'g*',sol3(1,i),sol3(2,i),'b.',x(1,m2+1:n),x(2,m2+1:n),'o');
      hold on
      line([x(1,i);sol3(1,i)],[x(2,i);sol3(2,i)]);
      hold on
    end
  end  
end

fprintf(' *********************************************\n')
s2 = input(' type 1 to see improvement after gradient descent, otherwise to quit ');
% overall check shows max error and rms error over all points.
% mfix strategy looks only at improvements on those originally with "small trace".
 
if s2 == 1
  % read in interior solution
  load sensorsoln2.dat;
  sol = sensorsoln2';
  % calculate square error of solution  
  err_soln4 = 0;
  for i = 1:m2
    err_soln4 = err_soln4 + norm(sol(1:2,i)-x(:,i))^2; 
  end
  fprintf(' rmsd = %2.1e\n', sqrt(err_soln4/m2));
end