%
% [toterr,erri] = cost_function(S)
%
%
% Dr. A. I. Hanna (2005)

function [toterr,erri] = cost_function_spring(xy, S, image_axis, animate)

T = size(S,1);
nr = 1:T;
Neig = S(:,1:T);
dist = S(:,T+(1:T));
k=size(nr,2);
for i=nr, 
   nsc{i}=intersect(nr,find(Neig(i,nr))+nr(1)-1);
end
erri=[];
for i=nr,
  ns=nsc{i};
  erri = [erri; sum((sqrt(sum((ones(length(ns),1)*xy(i,:) - xy(ns,:)).^2,2)) - (dist(i,ns)' + dist(ns, i))).^2)];
end
S(:,end-1:end) =xy;
if animate
    cla(image_axis);
    [marker_handle, line_handle, text_handle] = display_springs(S, image_axis);
    drawnow;
end
toterr=sum(erri);