% invpend_manifold.m - inverted pendulum on a manifold
% KJA

aminit;
figure(1); clf; xsubplot(221);

%Initialization
dth=pi/100;th=-pi:dth:pi;
x=cos(th);
y=sin(th);
h=3;
z1=-h*ones(size(th));
z2=h*ones(size(th));
plot3(x,y,z1);hold on
plot3(x,y,z2);

%Cylindrical surface added
x1=-1:0.1:1;
x2=-1:0.1:1;
x3=-h:0.1:h;
[X1,X2,X3]=meshgrid(x1,x2,x3);
h2=X1.^2+X2.^2-1;
p=patch(isosurface(x1,x2,x3,h2,0),'FaceColor',[0.9 0.9 0.9],...
    'EdgeColor','none','FaceAlpha',0.8);
isonormals(x1,x2,x3,h2,p)
daspect([1 1 2])

%Arrows, text, and environment
tharrow=-pi:dth:-3/4*pi;
xarrow=cos(tharrow);
yarrow=sin(tharrow);
zarrow=h*ones(size(tharrow));
plot3(xarrow,yarrow,zarrow,'k','LineWidth',1.5)
plot3([0 -1],[0 0],[3 3],'k')
arrow3([0 0 3],[0 0 3.8],...
    '1.5',0.175,0.35,[],0.8)
arrow3([-0.707 -0.707 3],[-0.507 -0.907 3],...
    '1.5',0.175,0.35,[],0.8)
view(-45,20)
camlight
lighting phong
axis off

%Manifold for pendulum
for a=[1 1.5]
  zp=sqrt(2*(a-cos(th)));
  zn =-sqrt(2*(a-cos(th)));
  plot3(x,y,zp, 'b', x,y,zn, 'b','LineWidth', AM_data_linewidth);
end

for th0=[pi/10 pi/5 pi/2.5]
th=pi-th0:dth:pi+th0;;
x=cos(th);
y=sin(th);
zp=sqrt(2*(cos(pi+th0)-cos(th)));
zn=-sqrt(2*(cos(pi+th0)-cos(th)));
plot3(x,y,zp,'b',x,y,zn,'b','LineWidth', AM_data_linewidth);
end

% Add text directly to the plot
% annotation('textbox', [0.235 0.89 0.05 0.05], 'String', 'W', ...
%   'EdgeColor', 'none');
% annotation('textbox', [0.245 0.79 0.05 0.05], 'String', 'T', ...
%   'EdgeColor', 'none');

amprint('invpend-manifold.eps');
