function demo_smoothing_spline
% smoothing spline to interactve given
% data points

% Initialize data
global AMB_Data;
AMB_Data=[];
AMB_Data.c=zeros(0,2);
AMB_Data.sigma=.5;                 
AMB_Data.handles=[0 0 0 0 0];
AMB_Data.movetype=0;
AMB_Data.moveind=0;
AMB_Data.uicontrols=[];
AMB_Data.axis=0;

% Initialize GUI
clf reset;
set(gcf,'WindowButtonDownFcn',@myDownfct);
set(gcf,'WindowButtonMotionFcn',@myMovefct);
set(gcf,'WindowButtonUpFcn',@myUpfct);

% Text for title
uicontrol('Units','normalized', ...
  	'BackgroundColor',get(gcf,'Color'), ...
	'HorizontalAlignment','center', ...
        'FontWeight','bold',...
        'FontSize',20,...
	'Position',[.05,.91,.9,.07],...
	'String','Smoothing Spline',...
	'Style','text');

% Text for sigma
AMB_Data.uicontrols(1)=uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'HorizontalAlignment','left', ...
	'ListboxTop',0, ...
	'Position',[.5,.84,.3,.05],...
	'String','sigma=0.50', ...
	'Style','text');

% Slider for sigma
AMB_Data.uicontrols(2)=uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'Position',[.05,.85,.4,.05], ...
	'Min',0,'Max',1,'Value',AMB_Data.sigma,...
	'Sliderstep',[.01,.01],...
	'Callback',@slider_update,...
	'Style','slider');

% Text for click
uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'ForegroundColor',[0,0,0], ...
	'Fontweight','bold', ...
	'HorizontalAlignment','right', ...
	'ListboxTop',0, ...
	'Position',[.1,.01,.85,.05],...
	'String','Mouse: left = insert, right = move, middle = delete point ',...
	'Style','text');

% Button for new curve
uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'String','Restart', ...
	'Position',[.83,.85,.12,.05], ...
	'Callback',@restart,...
	'Style','pushbutton');

% Axis for curve
AMB_Data.axis(1)=subplot('position',[.05,.07,.9,.75]);
axis([-.01,1.01,-.01,1.01]);
axis manual;
axis off
hold on;

% frame 
fill([-.1,1.1,1.1,-.1],[-.1,-.1,1.1,1.1],[1,1,1],'edgecolor','none');
plot([0,1,1,0,0],[0,0,1,1,0],'-k');
plot([0,1],1/3*[1,1],'-k');


%%% Callback Functions %%%

% Retart button pressed
function restart(varargin) 
global AMB_Data;
AMB_Data.c=zeros(0,2);
AMB_Data.step=0;
AMB_Data.movetype=0;
AMB_Data.moveind=0;
AMB_Data.sigma=.5;
set(AMB_Data.uicontrols(1),'String','sigma=0.50');
set(AMB_Data.uicontrols(2),'Value',.5);
myplot;

function slider_update(varargin) 
global AMB_Data;
% Read t value
AMB_Data.sigma = get(AMB_Data.uicontrols(2),'Value');

% display w and type
set(AMB_Data.uicontrols(1),'String',sprintf('sigma=%04.2f',AMB_Data.sigma));
myplot;

% Mouse button pressed
function myDownfct(varargin)
global AMB_Data;

if strcmp(get(gcf,'SelectionType'),'normal') % Left mouse: new point 
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);    
  if min(p) >= 0 & max(p) <=1
    AMB_Data.c=[AMB_Data.c;p];
    n=size(AMB_Data.c,1);
    AMB_Data.movetype=1; 
    AMB_Data.moveind=n;	  
  end
  myplot;
end

if strcmp(get(gcf,'SelectionType'),'alt') % 
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);  
  if min(p) >= 0 & max(p) <=1 % Right mouse: move 
    [m,ind]=min((p(1)-AMB_Data.c(:,1)).^2+(p(2)-AMB_Data.c(:,2)).^2);
    if m < .01 % 
      AMB_Data.movetype=2; 
      AMB_Data.moveind=ind;
      AMB_Data.c(ind,:)=p(1:2);
      myplot;      
    end
  end
end

if strcmp(get(gcf,'SelectionType'),'extend') % middle mouse 
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);  
  if min(p) >= 0 & max(p) <=1 % Middle mouse: delete
    [m,ind]=min((p(1)-AMB_Data.c(:,1)).^2+(p(2)-AMB_Data.c(:,2)).^2);
    if m < .01 
      AMB_Data.c(ind,:)=[];
      myplot;      
    end
  end
end

% Mouse button released
function myUpfct(varargin)
global AMB_Data;
AMB_Data.movetype=0;
AMB_Data.moveind=0;
myplot;

% Mouse movement
function myMovefct(varargin)
global AMB_Data;
if AMB_Data.movetype
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);
  if min(p) >= 0 & max(p) <=1 
    AMB_Data.c(AMB_Data.moveind,:)=p;
    myplot;
  end 
end     
  
%%% Internal Functions %%%
function myplot
global AMB_Data;
panz=200;

subplot(AMB_Data.axis(1));
n=size(AMB_Data.c,1);
cla;
fill([-.1,1.1,1.1,-.1],[-.1,-.1,1.1,1.1],[1,1,1],'edgecolor','none');
plot([0,1,1,0,0],[0,0,1,1,0],'-k');
plot([0,1],1/3*[1,1],'-k');


% delete curve and control polygon and c-steps 
if n < 2 
  if AMB_Data.handles(2)~=0  
    delete(AMB_Data.handles(2));
    AMB_Data.handles(2)=0;
  end
end
if n < 1 
  if AMB_Data.handles(1)~=0  
    delete(AMB_Data.handles(1));
    AMB_Data.handles(1)=0;
  end
end

plot([0,1],1/3*[1,1],'-k');

if n>0 
  % draw points
  plot(AMB_Data.c(:,1),AMB_Data.c(:,2),'ok');
  
  % plot c_k
  for k=1:size(AMB_Data.c,1)   
    if k==AMB_Data.moveind
      plot(AMB_Data.c(k,1)*[1 1],1/3+[-.01,.01],'-r','linewidth',1);
      plot(AMB_Data.c(k,1)*[1 1],[1/3,AMB_Data.c(k,2)],':r','linewidth',1);
      plot(AMB_Data.c(k,1),AMB_Data.c(k,2),'or');
    else
      plot(AMB_Data.c(k,1)*[1 1],1/3+[-.01,.01],'-k','linewidth',1);
      plot(AMB_Data.c(k,1)*[1 1],[1/3,AMB_Data.c(k,2)],':k','linewidth',1);
      plot(AMB_Data.c(k,1),AMB_Data.c(k,2),'ok');
    end    
  end
end  

if n > 1
  % draw curve
  points=sortrows(AMB_Data.c);
  [c,xi] = smoothing_spline(points(:,1),points(:,2),1000*ones(size(points,1),1),AMB_Data.sigma);
  x=linspace(points(1,1),points(end,1),panz)';
  y=spline_evaluation(c,xi,x);
  plot(x,y,'-b','Linewidth',3);  
end

%%% Functions from AMB_Programs (included for standalone use) %%% 

function [c,xi] = smoothing_spline(t,f,w,sigma)
% SMOOTHING_SPLINE: cubic smoothing spline 
% [c,xi] = smoothing_spline(t,f,w,sigma)
% coefficients c and knots xi of a cubic spline p which 
% minimizes (1-sigma) sum w(k) (f(k)-p(t(k)))^2 + sigma int |p''|^2
% with 0 <= sigma <= 1 

% add 3 knots to left and right
M = [4 -3; 3 -2; 2 -1];
xi = [M*t(1:2); t; M(3:-1:1,:)*t([end, end-1])];

% form knot differences
h1 = diff(xi);
h2 = h1(1:end-1)+h1(2:end);
h3 = h2(1:end-1)+h1(3:end);

% matrix of B-spline values
dl = (h1(4:end-2).^2)./(h2(3:end-2).*h3(2:end-2));
dr = (h1(3:end-3).^2)./(h2(3:end-2).*h3(3:end-1));
d = 1-dl-dr;
A = diag([dl; 0],-1) + diag([0; d; 0]) + diag([0; dr],1);
A = A(2:end-1,:);

% matrix of weights 
W = diag(w);

% differentiation matrix 
dl = 6./(h3(2:end-2).*h2(3:end-2)); 
dr = 6./(h3(3:end-1).*h2(3:end-2));
d = -dl-dr;
D = diag([dl; 0],-1) + diag([0; d; 0]) +  diag([0; dr],1);
D = D(2:end-1,:);

% matrix of scalar products
dlr = h1(4:end-3)/6; 
d = [h1(4)/3; h2(4:end-3)/3 ; h1(end-3)/3];
G = diag(dlr,-1) + diag(d) + diag(dlr,1);

if sigma < .00001
  c = A\f;
elseif sigma >.99999
  p=polyfit(t,f,1);
  c=p(2)+p(1)*(xi(2:end-3)+xi(3:end-2)+xi(4:end-1))/3;
else
  c = ((1-sigma)*A'*W*A + sigma*D'*G*D )\((1-sigma)*A'*W*f);
end

function p = spline_evaluation(c,xi,x) 
% SPLINE_EVALUATION: evaluation of a spline function
% p = spline_evaluation(c,xi,x) 
% values p(j) at x(j) of a spline function  
% with coefficients c and knots xi

% change to column vectors (if necessary)
c = c(:); xi = xi(:); x = x(:);

% degree
n = length(xi)-length(c)-1;

% add points and knots to avoid exceptional cases
c = [zeros(n+1,1); c; zeros(n+1,1)];
xi_max = (2*norm([xi; x],inf)+1)*ones(n+1,1);
xi = [-xi_max; xi; xi_max];

for j = 1:length(x)
   % interval: xi(kj) <= x(j) < xi(kj+1)
   kj = min(find(xi>x(j)))-1;
   % relevant control points
   pxj = c(kj-n:kj);
   % triangular scheme
   for m = 1:n 
      ind = [kj-n+m:kj];
      gamma = ((x(j) - xi(ind)) ./ (xi(ind+n+1-m) - xi(ind)));
      pxj = gamma.*pxj(2:end) + (1-gamma).*pxj(1:end-1);
   end 
   p(j) = pxj;
end
p = p(:); 

