% Water tank motion planning
%
% by Nicolas Petit and Pierre Rouchon
% Centre Automatique et Systemes
% Ecole Nationale Superieure des Mines de Paris
% 60 boulevard Saint-Michel
% 75272 Paris Cedex 06
% petit@cas.ensmp.fr, rouchon@cas.ensmp.fr
%
% http://cas.ensmp.fr/~petit/, http://cas.ensmp.fr/~rouchon/
%
% February 2002
%
% Details can be found in the following publication (to appear)
% "Dynamics and solutions to some control problems for water-tank systems.
% N. Petit, P. Rouchon, IEEE Transaction on Automatic Control 2002"
%
% Translation of the circular tank
% modification for OpenGL compatibility (Feb 2002)
%
%

clear all

uicontrol('style','push',...
                'units','normalized','position',[15/19 11.6/14 2/19*2 .7/14], ....
                'string','Faceting On/Off','callback', ...
                ['flif=-flif;' 'if flif==-1;' 'set(tapis,''Visible'',''off'');' 'axis on;' 'shading flat;'  'else;'  'axis off;' 'shading faceted;' 'tapis=patch([-3 3 3 -3]*1.2,[3 3 -3 -3]*1.2,[0 0 0 0],''g'');' 'end'        ]);
            
uicontrol('style','text',...
                'units','normalized','position',[1/19 .6/14 2/19*6 .7/14], ....
                'string','Water tank dynamics. N. Petit, P. Rouchon');
         %       'callback', ...
         %       ['grup'       ]
                
arret=0;
nice3d=0;

figure(1)
st=.1*3;
lr=[0 .1 2:10]*1.005;

tt=0:st:(2*pi); tt2=(0-pi/6):st/2:(2*pi-pi/6);
cx=[cos(tt) 1]; cy=[sin(tt) 0]; cx2=cos(tt2); cy2=sin(tt2);

mx=[]; my=[];
for r=lr;
   mx=[mx; cx*(r)/10]; my=[my; cy*(r)/10];
end 
th=ones(size(mx));
f=surf(mx,my,th,th*0+.2);
axis([-3,3, -3, 3, 0, 4/1.2]*1.2);
light('position',[-3,-3,1.1])
light('position',[-3,-3,1.1])


hold on
ggfond=surf(mx,my,th*0+.00,th*0-.5*0);
lfond=size(cx2);
mx2=[cx2 fliplr(cx2)]; my2=[cy2 fliplr(cy2)]; fond=zeros(lfond);
cx3=[cx2; cx2]; cy3=[cy2; cy2];
ggg=zeros(size(cx2));
bord=surface(cx3,cy3,[ggg; 1.7*ones(size(cy2))],[ggg; 2*ones(size(cy2))]*10);

hold off

t=0;
nn=20; tabY1=zeros(1,nn); tabY1d=zeros(1,nn); tabY2=zeros(1,nn); tabY2d=zeros(1,nn);
ctt=cos(tt); ctt2=cos(tt).^2;

flif=1;
set(gcf,'WindowButtonMotionFcn','1;');
dt=.1;f1=[0;0]; f2=[0;0];
C=1/mean(ctt2);
ax2=line([0 0],[0 0],[-.03 -.0]);set(ax2,'color','r','linewidth',5);
ax3=line([0 0],[0 0],[1 2]);set(ax3,'color','r','linewidth',2);
set(GCF,'Renderer','OpenGL');
lighting gouraud
alpha(.5)
material shiny

if nice3d==1
    shading flat
else
    axis off; 
    tapis=patch([-3 3 3 -3]*1.2,[3 3 -3 -3]*1.2,[0 0 0 0],'g');
end
set(gcf,'CloseRequestFcn','arret=1;');

while arret==0
   buf=get(gca,'CurrentPoint');
   ref=[ min(3,max(-3,buf(2,1))) ; min(2.5,max(-2.5,buf(2,2))) ] ;
   f1=f1+dt*(ref-f1);
   der=f1-f2;
   f2=f2+dt*der;
   Y1=f2(1); Y2=f2(2);
   Y1d=der(1)/2; Y2d=der(2)/2;
   tabY1=[tabY1(2:nn) Y1];
   tabY1d=[tabY1d(2:nn) Y1d];
   tabY2=[tabY2(2:nn) Y2];
   tabY2d=[tabY2d(2:nn) Y2d];
   access=round(min(nn,max(1,1+nn/2*ctt+nn/2)));
   D1=C*mean(tabY1(access).*ctt2);
   D2=C*mean(tabY2(access).*ctt2);
   
   M=[];
   for r=lr;
      access=round(min(nn,max(1,1+nn/2*ctt*r/10+nn/2)));
      M=[M; cx.* mean(tabY1d(access).*ctt) + cy* mean(tabY2d(access).*ctt)];
   end
   
   access=round(min(nn,max(1,1+nn/2*ctt+nn/2)));
   tmp=fliplr([1+ fliplr( cx2*mean(tabY1d(access).*ctt )+ cy2* mean(tabY2d(access).*ctt) )]);
   set(f,'Xdata',mx+D1,'Ydata',my+D2,'Zdata',1+M)
   set(ggfond,'Xdata',mx+D1,'Ydata',my+D2)
   set(ax3,'Xdata',[ref(1) ref(1)],'Ydata',[ref(2) ref(2)])  
   set(ax2,'Xdata',[ref(1) ref(1)],'Ydata',[ref(2) ref(2)])  
   set(bord,'Xdata', cx3+D1, 'Ydata',cy3+D2)
  drawnow
end

delete(gcf)

break