function [Edg2, S2, iter2,plotrE] = relax_edges(S,Edg, order, maxiter, K, animate, image_axis, error_axis, shape_springs)


if nargin<2,
    error('Insufficient arguments');
end
plothandle.image_axis = image_axis;
plothandle.error_axis = error_axis;


plothandle = initPlot(plothandle);
T=size(S,1);
Q = size(Edg,1);
sizeEdg = size(Edg);
xyEdg = Edg(:, end-1:end);
xyS = S(:, end-1:end);
N = zeros(size(xyEdg,1) + size(xyS,1));
L = zeros(size(xyEdg,1) + size(xyS,1));

% remember that the adjacency matrix for Edg corresponds to the xy values
% in S. So row 1 of N from Egd say has 1 1 0 0 1, then this means that xy
% value (1) from Edg is linked to xy values (1, 2, 5) from S.
N(1:size(xyEdg,1), end-T+1:end) = Edg(:,1:T);
N(end-T+1:end, 1:size(xyEdg,1)) = Edg(:,1:T)';
N(end-T+1:end, end-T+1:end) =  S(:,1:T);

L(1:size(xyEdg,1), end-T+1:end) = Edg(:,T+(1:T));
L(end-T+1:end, 1:size(xyEdg,1)) = Edg(:,T+(1:T))';
L(end-T+1:end, end-T+1:end) =  S(:,T+(1:T)) + S(:,T+(1:T))';


for i=1:length(shape_springs)
    spr = shape_springs(i);
    indx = spr.indx;
    N(indx(1), indx(2)) = 1;
    N(indx(2), indx(1)) = 1;
    L(indx(1), indx(2)) = spr.dist;
    L(indx(2), indx(1)) = spr.dist;
end
xyEdg = [xyEdg; xyS];
nr = 1:size(N,1);
nsc = cell(size(N,1),1);
for i=1:size(N,1)
    nsc{i} = find(N(i,nr));
end
plothandle.image_axis = image_axis;

NEdg = Edg(:,1:T);
DEdg = Edg(:,T+(1:T));

%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%
damp = 1;
dt = 0.5;
[xyEdg, plothandle] = calcDeviations(maxiter, animate, xyEdg, nsc, L, K, dt, damp, plothandle, Q);
Edg2 = [NEdg DEdg xyEdg(1:Q,:)];
S2 = S;
S2(:, end-1:end) = xyEdg(Q+1:end,:);
iter2 = maxiter;
plotrE = 0;
plotErrorCurve(plothandle);

%%%%%%
%
%
%%%%%%
function [xyEdg, plothandle] = calcDeviations(maxiter, animate, xyEdg, nsc, dist, K, dt, damp, plothandle, Q)
acc = zeros(size(xyEdg));
vel = zeros(size(xyEdg));
for iter = 1:maxiter
    if animate
        [plothandle] = animate_edges(plothandle, xyEdg, nsc);
    end
    forces = calcForces(xyEdg, nsc, dist, K, Q);
    acc = forces - vel*damp;
    vel = vel + acc*dt;
    xyEdg = xyEdg + vel*dt;
    [sumerr,erreurs] = cost_function_edge(xyEdg, nsc, dist);
    plothandle.ploterr = [plothandle.ploterr; [iter sumerr]];
end
return;
%%%%%%%
%
%
%%%%%%%
function forces = calcForces(xy, nsc, dist, K, Q)
T = size(xy,1);
forces = zeros(T, 2);
for i=1:T
    ci = xy(i,:);
    ns = nsc{i};
    ns = ns(ns>i);
    for j=ns
        cj = xy(j,:);
        s = ci - cj;
        s_bar = norm(s);
        dL = s_bar - dist(i, j);
        forces(i,:) = forces(i,:) - (s/s_bar)*K*dL;
        forces(j,:) = forces(j,:) + (s/s_bar)*K*dL;
    end
end
return;
%%%%%%%
%
%
%%%%%%%
function plothandle = animate_edges(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;
return
%%%
%
%%%
function plotErrorCurve(plothandle)
if ~isempty(plothandle.ploterr)
    plot(plothandle.error_axis, plothandle.ploterr(:,1), 10*log10(plothandle.ploterr(:,2)), 'Color', plothandle.error_col);
    axis(plothandle.error_axis,  'tight'); grid(plothandle.error_axis, 'on');
    xlabel('number of iterations (k)');
    ylabel('10log_{10}(error(k))');
end
drawnow;
return
%%%%%%%
%
%
%%%%%%%
function plothandle = initPlot(plothandle)
plothandle.ploterr=[];
plothandle.error_plot_handle = [];
plothandle.error_col = rand(1,3);
cla(plothandle.image_axis);
cla(plothandle.error_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);
