%  Mouse-driven motion planning
%      for  water tank system
%  march 99
%  Ecole des Mines de Paris, 
%  Centre Automatique et Systemes
%  60 Bd St Michel 
%  75272 Paris
%    Nicolas Petit: petit@cas.ensmp.fr
%    Pierre Rouchon: rouchon@cas.ensmp.fr
% 
% Linear approximation of the Saint-Venant equation 
% see the paper:
%  Motion planning and nonlinear simulations for 
%  a tank containing a fluid. 
%  F. Dubois, N. Petit and P. Rouchon
%  in  European control conference, ECC99
%  july 1999.
%   
% 
%     

clear all;  
L=1.; 
D_max=2*L;
H=.2;


g=9.81;V=sqrt(g*H); delta=L/V;
dz=L/40;

z=[0:dz:L];
nz=length(z);
h=H*ones(1,nz);
Ht=H + 2*min(H,L);
S=min(19/(L+D_max),14/Ht); % scaling

% time step 
DT=delta/2.; 
dt0=DT/40;
dt=dt0; 
dtmin=DT/1000;
dtmax=DT/3.;

YY=0*[-DT:dt0:DT];
YYdot=0*[-DT:dt0:DT];
mt=length(YY);
m0=(mt+1)/2.;
mm=[1:mt-1];

ip=round((L/2-z)/V/dt0) + (mt+1)/2;
im=round((z-L/2)/V/dt0) + (mt+1)/2;




Y=0*[-5*DT:dt:0]; D=Y;t=Y;
nt=length(Y);
nn=[1:nt-1];





figure(1),clf;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add');
          

   
Xi=9.5-L*S/2.;
Yi=7.5-Ht/2 *S;

Xtank=Xi + L*S + L*S*[0   0  -1  -1 ];
Ytank=Yi+[Ht*S 0 0 Ht*S];
Xwater=[ Xtank Xi+z*S    ];
Ywater=[ Ytank Yi+h*S    ];
tank=fill(Xwater,Ywater,'c','EraseMode','background','FaceColor', [0 0 1 ]);

%  ref
Xr=Xi+ L*S/2 +[0   1  0.5  0  ] -.5;
Yr=Yi-1+[0   0  1/2  0  ];
ref=fill(Xr,Yr,'y','EraseMode','background');


set(gcf,'WindowButtonMotionFcn','1;');


Hplay=uicontrol('style','push',...
                'units','normalized','position',[15/19 12.7/14 2/19 .5/14], ....
                'string','Plot','callback', ...
                ['figure(2);clf;' ...
                 'subplot(211);' ...
                 'plot(t,Y);'...
                 'title(''flat output  (m) versus time'');'...
                 'subplot(212);'...
                 'plot(t,D);'...
                 'title(''tank position (m) versus time''); pause,figure(1);'...
                ]);
             
set(Hplay,'visible','off');
             
Hrun=uicontrol('style','push',...
                'units','normalized','position',[1/19 12.7/14 2/19 .5/14], ....
                'string','stop','callback', ...
                ' i=2; ','visible','off' );
             
             
             
             
  % title
text(9.5,13.5,'THE WATER TANK: MOUSE-DRIVEN MOTION PLANNING','color',[0 0 0],...
   'EraseMode','background','HorizontalAlignment','center');
text(9.5,13,'display speed slider','color',[0 0 0],'EraseMode','background','HorizontalAlignment','center');
p1=[7.5/19 12.6/14 4/19 0.2/14];
Hslider=uicontrol(figure(1),'units','normalized','style','slider','position',p1,...
			'min',log(dtmin),'max',log(dtmax),'value',log(dt0),...
         'callback',['buf=get(Hslider,''value'');dt=exp(buf);']); 
           
             
             
             

figure(1);
pause(1)
set(Hrun,'visible','on');
set(Hplay,'visible','on')


seuil=L/2;
t1=2*H/V;
t2=t1/1.5;
t3=t2/1.5;

y1=0;
y2=0;
y3=0;

i=0;

tt=0.;
y1_old=0;
y1dot_old=0;


while (i < 1)
    buf=get(gca,'CurrentPoint');
    A=(buf(1,1)-9.5)/S;
    A=max(-D_max/2.1,A);
    A=min(D_max/2.1,A); 
    
   y1=(y1+dt*y2/t1)/(1+dt/t1);
   y2=(y2+dt*y3/t2)/(1+dt/t2);
   t3b=t3*max(1,abs(A-y3)/seuil);
   y3=(y3+dt*A/t3b)/(1+dt/t3b);
   
   y1dot=(y2-y1)/t1;
   
   tt=tt+dt;
   if (tt >= dt0);
      ii=floor(tt/dt0);
      dy1=(y1-y1_old)/tt;
      dy1dot=(y1dot-y1dot_old)/tt;
      for iii=1:ii;
         YY(mm)=YY(mm+1);YY(mt)=y1_old+iii*dt0*dy1;
         YYdot(mm)=YYdot(mm+1);YYdot(mt)=y1dot_old+iii*dt0*dy1dot;
      end;
      y1_old=y1-dy1*(tt-ii*dt0);
      y1dot_old=y1dot-dy1dot*(tt-ii*dt0);
      tt=0.;
   end;
    
   
   h=( ( V+(YYdot(ip)-YYdot(im) )/4 ).^2)/g;
   dd=(YY(1)+YY(mt))/2;

    
    
   Y(nn)=Y(nn+1); Y(nt)=YY((mt+1)/2);
   D(nn)=D(nn+1); D(nt)=dd;
   t(nn)=t(nn+1); t(nt)=t(nt)+dt;
   
 	Ywater=[ Ytank Yi+h*S    ];
   set(tank,'XData',Xwater+S*dd,'YData',Ywater); 
   set(ref,'XData',Xr+S*A); 
   drawnow;
   
end;


close(figure(1));
close(figure(2));

return