% Mar 2009
%
% This matlab code reads in reads in Prob.mat, sensorsoln.dat, and 
% (optionally) sensorsoln2.dat.  Prob.mat is generated by
% sensor_esdp_gen.m,
% while the last two files are the output of the Fortran program
% sensor_esdp.
% It computes the rmsd error, max error and produces a drawing.

  clear
% read in  n m dim  x nd d head tail 
 
  load Prob
  fprintf(' %g pts,  %g anchors \n',n,m);
  gamma = input(' noisefactor (%)= '); % the noisefactor estimate used in solving the rho-ESDP

% read in solution

  load sensorsoln.dat;
  sol = sensorsoln';
  choice = input(' Type 1 for a plot ');
  
% 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; % The choice as in the paper
  nfix = 0;

  deg = zeros(m2,1);
  dis = zeros(m2,1); 

  for k = 1:nd   
    i = head(k);
    j = tail(k);
    
    if i <= m2      
      deg(i) = deg(i) + 1;      
      dis(i) = dis(i) + d(k);
    end
    
    if j <= m2      
      deg(j) = deg(j) + 1;        
      dis(j) = dis(j) + d(k);
    end

  end

  dis = dis./max(deg,1);

  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)));

    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;
    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, RMSD error & max-norm error over fixed points: %g, %g ,%g\n', nfix, sqrt(err_soln4/nfix), err_soln2);  

  if choice == 1
    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

  s2 = input(' Type 1 to check error after refinement ');

  if s2 == 1
    % read in interior solution
    load sensorsoln2.dat;
    sol = sensorsoln2';
    % calculate square error of solution
    err_soln2 = 0;
    err_soln4 = 0;
    tol = 0;
    
    for i = 1:m2
      err_soln2 = max(err_soln2,norm(sol(1:2,i)-x(:,i)));
      err_soln4 = err_soln4 + norm(sol(1:2,i)-x(:,i))^2;
      hold on
      plot(x(1,i),x(2,i),'g*',sol(1,i),sol(2,i),'r.',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
    
    fprintf(' rms error & max-norm error over all points: %g, %g\n', sqrt(err_soln4/m2), err_soln2); 
  end
  