function drawChain(p)

% unpack the points into x and y
Np = length(p)/2;
x = p(1:Np);
y = p(Np+1:end);
x0 = 0; % first anchor x
y0 = 0; % first anchor y
xn = 1; % second anchor x
yn = 0; % second anchor y
xx = [x0; x(:); xn];
yy = [y0; y(:); yn];
N = Np + 2;
ms = 10; % markersize
plot(xx(1),yy(1),'bs','markersize',ms)
hold on
plot(xx(end),yy(end),'bs','markersize',ms)
for i=2:(N-1)
    plot(xx(i),yy(i),'rs','markersize',ms)
    plot([xx(i) xx(i-1)],[yy(i) yy(i-1)],'r-')
end
plot([xx(N-1) xx(N)],[yy(N-1) yy(N)],'r-')
plot([xx(1) xx(end)], [yy(1) yy(end)], 'k--')
hold off
ymin = -Np/2/10; ymax = Np/2/10;
axis([0 1 ymin ymax])
drawnow

end

function E = chainEnergy(p)

m = 0.1; % mass of each segment
g = 9.8; % gravitational constant
k = 50;  % stiffness of each segment (inverse of stretchiness)
L = 0.1; % resting length of each segment

N = (length(p)/2) + 2; % number of inner points + two anchors

% unpack the points into x and y
Np = length(p)/2;
x = p(1:Np);
y = p(Np+1:end);
x0 = 0; % first anchor x
y0 = 0; % first anchor y
xn = 1; % second anchor x
yn = 0; % second anchor y
xx = [x0; x(:); xn];
yy = [y0; y(:); yn];

E1 = 0;
for i=2:(N-1)
    E1 = E1 + (m*g*(yy(i)));
end

E2 = 0;
for i=2:N
    Li = sqrt((xx(i)-xx(i-1))^2 + (yy(i)-yy(i-1))^2);
    E2 = E2 + (k*((Li-L)^2));
end

E = E1 + E2;

drawChain([xx' yy']); 
title(sprintf('cost=%.8f',E));
drawnow

end

nn = 7; % number of inner points
x0 = linspace(0.1, 0.9, nn) + randn(1,nn)*0.01;
y0 = zeros(1,nn) - rand(1,nn)*.25;

p0 = [x0 y0];
drawChain(p0)

%% optimize using gradient descent (fminsearch)
p_opt = fminsearch(@chainEnergy,p0);

%% optimize using gradient descent (fminunc)
p_opt = fminunc('chainEnergy',p0);