function [S2,bestest,ploterr]=relax_springs(S, max_iteration, K, animate, image_axis, error_axis, scale_plot)
% modif 15/11/00 : stiffness changes
% modif 28/11/05 : changed a lot of the code to work with the toolbox.
%[S2,iter]=relax_springs(S,n,opts) for now agrelax_springs2 not different from version 1.
%          Relax 2D springs in S indexed by n.
%          Finishes when no changes made, or 1000 iterations.
%           S    : 2D spring description
%           n    : numbers of springs to relax   (default: all)
%           opts : Options                       (default: [0 1000])
%                  [verbose max_iterations]
%
%          Returns:
%           S2   : relaxed version of S
%           iter : number of iterations used
%
%          See also: make_springs
%
%          (c) Stephen Impey, 1996-7
%              rolland 1999-2000
%              Dr. A. I. Hanna (2005)


if nargin<1,
    error('Insufficient arguments');
end

T = size(S,1);

n=1:T;

% Default options
if nargin<2
    animate = 0;
end
if (animate) & (nargin<5)
    image_axis = gca;
    error_axis = axes;
end
%max_iteration = 200;

nr=n;
Neig = S(:,1:T);
dist = S(:,T+(1:T));
xy = S(:,2*T+(1:2));
xy_orig = xy;

for i=1:T
    nsc{i} = find(Neig(i,nr));
end

detailerr=[];
S2 = [Neig dist xy];

plothandle.image_axis = image_axis;
plothandle.error_axis = error_axis;
plothandle.scale_plot = scale_plot;
plothandle = initPlot(plothandle);
fraction = 100;
damp = 0.5;
dt = 0.5;
mass = 1;
[xy, plothandle] = calcDeviations(max_iteration, animate, xy, nsc, dist, K, dt, damp, plothandle, S2);
S2 = [Neig dist xy];
bestest = max_iteration;
ploterr = [];
%bestest=find(plothandle.ploterr(:,2)==min(plothandle.ploterr(:,2)));
%bestest=min(bestest);
disp([num2str(max_iteration) ' iterations for centroid']);
% Can take the best one if we like but we are pretty smooth and convergent
% using these values.
%[xy_orig, plothandle] = calcDeviations(bestest, animate, xy_orig, nsc, dist, K, dt, damp, plothandle, S2);
%S2 = [Neig dist xy_orig];
%%%%%%%
%
%
%%%%%%%
function [xy, plothandle] = calcDeviations(max_iteration, animate, xy, nsc, dist, K, dt, damp, plothandle, S2)
acc = zeros(size(xy));
vel = zeros(size(xy));
for iter = 1:max_iteration
    if animate
        [plothandle] = animate_springs(plothandle, xy, nsc);
    end
    forces = calcForces(xy, nsc, dist, K);
    acc = forces - vel*damp;
    vel = vel + acc*dt;
    xy = xy + vel*dt;
    %[sumerr,erreurs] = cost_function_spring(xy, S2, plothandle.image_axis, 0);
    %plothandle.ploterr = [plothandle.ploterr; [iter sumerr]];
end
return;
%%%%%%%
%
%
%%%%%%%

function forces = calcForces(xy, nsc, dist, K)
T = size(xy,1);
forces = zeros(T, 2);
for i=1:T
    ci = xy(i,:);
    ns = nsc{i};
    ns = ns(find(ns>i));
    for j=ns
        cj = xy(j,:);
        s = ci - cj;
        s_bar = norm(s);
        dL = s_bar - (dist(i,j)+dist(j,i));
        forces(i,:) = forces(i,:) - (s/s_bar)*K*dL;
        forces(j,:) = forces(j,:) + (s/s_bar)*K*dL;
    end
end



%%%%%%%
%
%
%%%%%%%
function [plothandle] = animate_springs(plothandle, xy, nsc);
cla(plothandle.image_axis);
hold(plothandle.image_axis, 'on');
for i=1:length(nsc)
   n = nsc{i};
   for ii=1:length(n)
        plot(plothandle.image_axis, [xy(i,1) xy(n(ii),1)],[xy(i,2) xy(n(ii),2)], 'b');
   end
end
drawnow;

%%%%%%%
%
%
%%%%%%%
function plothandle = initPlot(plothandle)
plothandle.marker_handle = [];
plothandle.line_handle = [];
plothandle.ploterr=[];

plothandle.text_handle = [];
plothandle.error_plot_handle = [];
cla(plothandle.image_axis);
hold(plothandle.image_axis, 'on');
axis(plothandle.image_axis,  'image', 'xy');
grid(plothandle.image_axis, 'on');
hold(plothandle.error_axis, 'on');
axis(plothandle.error_axis,  'tight'); grid(plothandle.error_axis, 'on');
xlabel(plothandle.error_axis, 'number of iterations (k)', 'FontSize', 8);
ylabel(plothandle.error_axis, '10log_{10}(error^2(k))', 'FontSize', 8);