function [S, iter2,plotrE] = satlib_relaxSprings(varargin)
% function [S, iter2,plotrE] = satlib_relaxSprings(varargin)
%
% Dr. A. I. Hanna, CMP, 2008.

[lambda, damp, dt, tol, S, stage_springs, new_spring_lengths, ...
    maxiter, K, animate, image_axis, error_axis] = parseargs(varargin);

% init the plotting data
plothandle.image_axis = image_axis;
plothandle.error_axis = error_axis;
plothandle = initPlot(plothandle);

% extract the 
xyEdg=S.tri_info.pts;
spring_len = new_spring_lengths;
spring_edges = stage_springs;
T = S.tri_info.T;
plothandle.image_axis = image_axis;

% calculate the deviations
[xyEdg, plothandle] = calcDeviations(lambda, tol, maxiter, animate, xyEdg, ...
    T, spring_len, spring_edges, K, dt, damp, plothandle);
    
% reset the new points
S.tri_info.pts = xyEdg;
iter2 = maxiter;
plotrE = 0;
plotErrorCurve(plothandle);

%%
function [xyEdg, plothandle] = calcDeviations(lambda, tol, maxiter, animate, xyEdg, T, spring_len, spring_edges, K, dt, damp, plothandle)
acc = zeros(size(xyEdg));
vel = zeros(size(xyEdg));
Eold = realmax;
iter = 1;
Etol = tol+1;

dE = sqrt(sum(xyEdg.^2,2));
N = xyEdg./repmat(dE, [1 2]);

%Dorig = distPts2OppEdge(xyEdg, T);

[Dorig] = satlib_triDist2OppEdge('pts', xyEdg, 'T', T);

while (Etol>tol) && (iter<=maxiter)

    if animate
        [plothandle] = animate_edges(plothandle, xyEdg, spring_edges);
        plotErrorCurve(plothandle);
    end
    forces = calcForces(xyEdg, spring_len, spring_edges, K);
    acc = forces - vel*damp;
    vel = vel + acc*dt;

    deltaXY = vel*dt;
    xyEdg = xyEdg + deltaXY;

    % introduce a penalty term that stops the triangles from overlapping
    % good values of lambda seem to be between 0.005-0.01
    if lambda>0
        [Fpenalty] = satlib_calcPenalty('DORIG', Dorig, 'XYedg', xyEdg, 'T', T);
        xyEdg = xyEdg + lambda*Fpenalty;
    end

    [E,erreurs] = cost_function_edge(xyEdg, spring_edges, spring_len);
    Etol = abs(Eold-E);
    
    fprintf('Error = %f, tol=%f\n', E, Etol);
    Eold = E;
    plothandle.ploterr = [plothandle.ploterr; [iter E]];
    iter = iter + 1;
end
fprintf('Iterations %d, Tolerence: %6.6f\n', iter, Etol);
fprintf('Error: %f\n', E);
return;
%%
function forces = calcForces(xy, len, edge,K)
T = size(xy,1);
forces = zeros(T, 2);
num_springs = size(edge,1);
for i=1:num_springs
    ci = xy(edge(i,1),:);
    cj = xy(edge(i,2),:);
    s = ci - cj;
    s_bar = norm(s);
    dL = s_bar - len(i);
    forces(edge(i,1),:) = forces(edge(i,1),:) - (s/s_bar)*K*dL;
    forces(edge(i,2),:) = forces(edge(i,2),:) + (s/s_bar)*K*dL;
end
return;
%%
function plothandle = animate_edges(plothandle, xy, edges)
cla(plothandle.image_axis);
hold(plothandle.image_axis, 'on');
for i=1:size(edges, 1)
    e = edges(i,:);
    plot(plothandle.image_axis, [xy(e(1),1) xy(e(2),1)],[xy(e(1),2) xy(e(2),2)], 'b');
end
drawnow;
return
%%
function plotErrorCurve(plothandle)
if ~isempty(plothandle.ploterr)
    plot(plothandle.error_axis, plothandle.ploterr(:,1), plothandle.ploterr(:,2), 'Color', plothandle.error_col);
    axis(plothandle.error_axis,  'tight'); grid(plothandle.error_axis, 'on');
    xlabel(plothandle.error_axis, 'number of iterations (k)');
    ylabel(plothandle.error_axis, '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);
hold(plothandle.image_axis, 'on');
axis(plothandle.image_axis,  'image', 'ij');
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);
%%
function [lambda, damp, dt, tol, S, stage_springs, new_spring_lengths, ...
    maxiter, K, animate, image_axis, error_axis] = parseargs(varargin)
varargin = varargin{1};%
error(nargchk(0,inf,nargin));
if mod(length(varargin),2) ~= 0
    % input args have not com in pairs, woe is me
    error(['Arguments to ', mfilename, ' must come param/value in pairs.'])
end
lambda = 0; 
damp = 1;
dt = 0.01;
tol = 1e-5;
S = [];
stage_springs = [];
new_spring_lengths = [];
maxiter = 200;
K = 1;
animate = 0;
image_axis = [];
error_axis = [];
for i=1:2:length(varargin)
    switch lower(varargin{i})
        case 'lambda'
            lambda = varargin{i+1};
        case 'damp'
            damp = varargin{i+1};
         case 'dt'
            dt = varargin{i+1};
          case 'tol'
            tol = varargin{i+1};
          case 's'
            S = varargin{i+1};
          case 'stage_springs'
            stage_springs = varargin{i+1};
          case 'new_spring_lengths'
            new_spring_lengths = varargin{i+1};
          case 'maxiter'
            maxiter = varargin{i+1};
          case 'k'
            K = varargin{i+1};
          case 'animate'
            animate = varargin{i+1};
          case 'image_axis'
            image_axis = varargin{i+1};
          case 'error_axis'
            error_axis = varargin{i+1};
        otherwise
            error(['Unknown parameter name passed to ', mfilename, '.  Name was ' varargin{i}])
    end
end
if isempty(S)
    error('Parameter S sets must be supplied');
end
if isempty(stage_springs)
    error('Parameter stage_springs sets must be supplied');
end
if isempty(new_spring_lengths)
    error('Parameter new_spring_lengths sets must be supplied');
end
if isempty(image_axis)
    image_axis = gca;
end
if isempty(error_axis)
    error_axis = gca;
end
return
