function demo_geometric_hermite_interpolation
% plots the interpolating Hermite curve to given
% points, tangents and osculating circles

% Initialise data
global AMB_Data;
AMB_Data=[];
AMB_Data.plotpoints=300;
AMB_Data.movepoints=100;
AMB_Data.c=zeros(0,2);
AMB_Data.handles=[0 0 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'), ...
        'FontWeight','bold',...
        'FontSize',20,...
	'Position',[.05,.91,.9,.07],...
	'String','Geometric Hermite Interpolation',...
	'Style','text');

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

% Text for click
AMB_Data.uicontrols(1)=uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'HorizontalAlignment','right', ...
        'FontWeight','bold',...
	'Position',[.25,.01,.7,.05],...
	'String','Select first point ', ...
	'Style','text');

% Text for Approximation
AMB_Data.uicontrols(2)=uicontrol('Units','normalized', ...
	'BackgroundColor',get(gcf,'Color'), ...
	'ForegroundColor',[.8,0,0], ...
	'Visible','off', ...
	'Position',[.12,.065,.8,.04],...
	'String','Interpolation failed, approximation displayed', ...
        'FontWeight','bold',...
	'Style','text');

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

% frame 
fill([-.1,1.6,1.6,-.1],[-.1,-.1,1.1,1.1],[1,1,1],'edgecolor','none');
plot([0,1.5,1.5,0,0],[0,0,1,1,0],'-k');

%%% Callback Functions %%%

% Reset button pressed
function restart(varargin) 
global AMB_Data;
AMB_Data.c=zeros(0,2);
AMB_Data.movetype=0;
AMB_Data.moveind=0;
for k=AMB_Data.handles
  if k,delete(k);end
end
AMB_Data.handles=[0,0,0,0,0,0,0];
set(AMB_Data.uicontrols(1),'String','Select first point');    
set(AMB_Data.uicontrols(2),'visible','off');

% Mouse button pressed
function myDownfct(varargin)
global AMB_Data;
set(AMB_Data.uicontrols(1),'String','');    
if size(AMB_Data.c,1) < 6 % Input data
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);  
  if min(p) >= 0 & p(2) <=1 & p(1) <=1.5
    AMB_Data.c=[AMB_Data.c;p];
    n=size(AMB_Data.c,1);
    AMB_Data.movetype=1;
    AMB_Data.moveind=n;
  end
  % plot
  myplot;
else % move point
  p=get(AMB_Data.axis(1),'CurrentPoint');
  p=p(1,1:2);  
  if min(p) >= 0 & p(2) <=1 & p(1) <=1.5 
    % find nearest point
    [m,ind]=min((p(1)-AMB_Data.c(:,1)).^2+(p(2)-AMB_Data.c(:,2)).^2);
    if m < .01 % react only on "near" click
      AMB_Data.movetype=1; 
      AMB_Data.moveind=ind;
      AMB_Data.c(ind,:)=p;
      myplot;      
    end
  end
end

% Mouse button released
function myUpfct(varargin)
global AMB_Data;
AMB_Data.movetype=0;
AMB_Data.moveind=0;
Strings = { ...
    'Select first point ',...
    'Select point on first tangent ',...
    'Select point on first osculating circle ',...
    'Select second point ',...
    'Select point on second tangent ',...
    'Select point on second osculating circle ',...
    'Mouse: left = move point ',...
    };
set(AMB_Data.uicontrols(1),'String',Strings{size(AMB_Data.c,1)+1});    
    
% 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 & p(2) <=1 & p(1) <=1.5 
    AMB_Data.c(AMB_Data.moveind,:)=p;
    myplot;
  end 
end     
  
%%% Internal Funcions %%%

function myplot
global AMB_Data;

if AMB_Data.movetype
  panz = AMB_Data.movepoints;
else
  panz = AMB_Data.plotpoints;
end  

t=linspace(0,2*pi,100);
c=AMB_Data.c;
n=size(c,1);
if (n==1) % plot first point   
  if AMB_Data.handles(1)
    set(AMB_Data.handles(1),'Xdata',c(1,1));
    set(AMB_Data.handles(1),'Ydata',c(1,2));  
  else
    AMB_Data.handles(1)=plot(AMB_Data.c(1,1),c(1,2),'-ok');  
  end
end 
if (n>1) % plot first tangent   
  set(AMB_Data.handles(1),'Xdata',c(1:2,1));
  set(AMB_Data.handles(1),'Ydata',c(1:2,2));
  d0=c(2,:)-c(1,:);d0=d0/norm(d0);
end
if (n>2) % plot first circle
  A=[d0(2),c(3,2)-c(1,2);-d0(1),c(1,1)-c(3,1)];
  s=A\[c(1,1)-c(3,1);c(1,2)-c(3,2)];
  m=c(1,:)-s(1)/2*[d0(2),-d0(1)]; 
  r=norm(c(1,:)-m,2);
  x=m(1)+r*cos(t);x(x<0)=NaN;x(x>1.5)=NaN;
  y=m(2)+r*sin(t);y(y<0)=NaN;y(y>1.5)=NaN;
  k0=1/r*sign(det([d0;m-c(1,:)]));
  if AMB_Data.handles(2)
    set(AMB_Data.handles(2),'Xdata',x);
    set(AMB_Data.handles(2),'Ydata',y);  
  else
    AMB_Data.handles(2)=plot(x,y,'-k');  
  end
  if AMB_Data.handles(3)
    set(AMB_Data.handles(3),'Xdata',c(3,1));
    set(AMB_Data.handles(3),'Ydata',c(3,2));  
  else
    AMB_Data.handles(3)=plot(c(3,1),c(3,2),'-ok');  
  end
end
if (n==4)
  if AMB_Data.handles(4)
    set(AMB_Data.handles(4),'Xdata',c(4,1));
    set(AMB_Data.handles(4),'Ydata',c(4,2));  
  else
    AMB_Data.handles(4)=plot(c(4,1),c(4,2),'-ok');  
  end
end 
if (n>4) % plot second tangent   
  set(AMB_Data.handles(4),'Xdata',c(4:5,1));
  set(AMB_Data.handles(4),'Ydata',c(4:5,2));  
  d1=c(5,:)-c(4,:);d1=d1/norm(d1);
  df=6*(c(4,:)-c(1,:));
end
if (n==6)% plot second cicle and curve
  A=[d1(2),c(6,2)-c(4,2);-d1(1),c(4,1)-c(6,1)];
  s=A\[c(4,1)-c(6,1);c(4,2)-c(6,2)];
  m=c(4,:)-s(1)/2*[d1(2),-d1(1)]; 
  r=norm(m-c(4,:),2);
  k1=1/r*sign(det([d1;m-c(4,:)]));
  x=m(1)+r*cos(t);x(x<0)=NaN;x(x>1.5)=NaN;
  y=m(2)+r*sin(t);y(y<0)=NaN;y(y>1.5)=NaN;
  if AMB_Data.handles(5)
    set(AMB_Data.handles(5),'Xdata',x);
    set(AMB_Data.handles(5),'Ydata',y);  
  else
    AMB_Data.handles(5)=plot(x,y,'-k');  
  end
  if AMB_Data.handles(6)
    set(AMB_Data.handles(6),'Xdata',c(6,1));
    set(AMB_Data.handles(6),'Ydata',c(6,2));  
  else
    AMB_Data.handles(6)=plot(c(6,1),c(6,2),'-ok');  
  end
  
  % compute Hermite spline
  [cp,fail]=hermite_geometric(c([1,4],:),[d0;d1],[k0,k1]); 
  if fail
    set(AMB_Data.uicontrols(2),'visible','on');
  else
    set(AMB_Data.uicontrols(2),'visible','off');
  end
  
  % draw curve 
  t=linspace(0,1,panz);    
  s=casteljau(cp,t');  
  if AMB_Data.handles(7)==0  
    AMB_Data.handles(7)=plot(s(:,1),s(:,2),'-b','Linewidth',2);  
  else
    set(AMB_Data.handles(7),'Xdata',s(:,1));
    set(AMB_Data.handles(7),'Ydata',s(:,2));
  end
end

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

function p = casteljau(c,t)
% CASTELJAU: de Casteljau's algorithm for Bezier curves 
% p = casteljau(c,t)
% points p(j,:) on a Bezier curve with control points c(k,:)
% for parameters t(j) 

% convert to column vector (if necessary)
t = t(:); 
 
[np1,dim] = size(c);
% de Casteljau's recursion 
if np1 == 1
   p = (1+0*t)*c;
else
   p0 = casteljau(c(1:end-1,:),t); 
   p1 = casteljau(c(2:end,:),t); 
   t = repmat(t,1,dim); 
   p = (1-t).*p0 + t.*p1;
end


function [c,fail] = hermite_geometric(p,d,kappa)
% HERMITE_GEOMETRIC: planar geometric Hermite interpolant 
% c = hermite_geometric(p,d,kappa)
% positions p(1:2,:), tangent directions d(1:2,:), 
% and curvatures kappa(1:2) at endpoints, 
% determining Bezier control points c(1:4,:)
% fail = 1 indicates interpolation failure

% normalize tangent directions 
for k = 1:2
   d(k,:) = d(k,:)/norm(d(k,:));
end

% parameters for nonlinear system
dp = diff(p); 
d12 = 2*det(d);
a1 = 6*det([d(1,:);dp])/d12; a2 = 6*det([d(2,:);dp])/d12;
b1 = kappa(1)/d12; b2 = kappa(2)/d12;

% solve: s1 = -a2 - b2*s2^2, s2 = a1 - b1*s1^2
s1 = roots([b2*b1^2, 0, -2*b2*a1*b1, 1, b2*a1^2+a2]);
s2 = a1 - b1*s1.^2; 

% select admissible (approximate) solution
ind = find(s1>0 & s2>0 & imag(s1)==0 & imag(s2) == 0); 
if isempty(ind) 
   fail = 1;
else 
   s1 = s1(ind); s2 = s2(ind);fail = 0;
end
   
% closest solution to a linear fit: p(2,:)-p(1,:) ~ s*d
[mins,k] = min(sum(abs([s1 s2]'-norm(dp))));
s1 = real(s1(k)); s2 = real(s2(k));

% control points
c = [p(1,:); p(1,:)+s1*d(1,:)/3; p(2,:)-s2*d(2,:)/3; p(2,:)];

