%  Mouse-driven motion planning
%      for  the 1D Heat Rod
%  march 99
%  Ecole des Mines de Paris, 
%  Centre Automatique et Systemes
%  60 Bd St Michel 
%  75272 Paris
%    Pierre Rouchon: rouchon@cas.ensmp.fr
% 
% Linear  model:  H_t = H_zz 
%                 H_z(0,t)=0, H(1,t)=u(t)
% 
% flat output: y(t)=H(0,t) 
% motion planing formulae 
% 
%  H(x,t) =  sum_0^\infty y^(n)/(2n)!  x^(2n)
%  with y a Gevrey function of order less or equal to 2 
%
%  see Ph. Martin, R Murray and P. Rouchon 
%   flat systems, European control conference, Mini-cources, 1997, Brussel
%  see also B. Laroche, Ph Martin, P. Rouchon
%   Motion planning for a class of partial differential equations 
%   with boundary control, CDC 98, Tampa. 
%     
clear all;  


Igevrey=1.9;
expo=1/(Igevrey-1); % Gevrey exponent 1+1/expo 



T_max= 2.  ;   
DT=1;
scale=15; 

% space mesh 
dz=1/20.;
z=[0:dz:1];
nz=length(z);

nS=10;


HH=zeros(nS+1,nz);
HH(1,:)=z.^0;
z2=z.^2;
for i=2:nS+1
   HH(i,:)=HH(i-1,:).*z2;
end;



% time step and convolution length
eps= 0.5;
dt0=eps/50;

dt=dt0; 
dtmin=eps/1000;
dtmax=eps/3.;


tt0= [dt0:dt0:eps-dt0]';
YY=0*tt0';
GG=heatdrv(tt0,nS,expo,eps);
GG=GG/sum(GG(:,1));
% comment next  line for antidiffusion 
GG=GG*diag((-ones(1,nS+1)).^[0:nS]);


H=YY*GG*HH;


mt=length(YY);
m0=(mt+1)/2.;
mm=[1:mt-1];

t=0*[-5:dt:0];
nt=length(t);
Nrec=[1 floor(nz/4)  floor(nz/2) floor(3*nz/4) nz];
Yrec=zeros(nt,length(Nrec)); 
nn=[1:nt-1];

y1=0;





figure(1),clf;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add');
             
% Color table  and map
% Color table 
Cmap=jet(100);
colormap(Cmap);
caxis([0 1 ]);  

Rmin=2; 
Rmax=12;
Xx=[0  0  .45 .45] + 0;
Yy=[Rmin  Rmax Rmax  Rmin] + 0.; 

Cc=[4/12  10/12  10/12   4/12]; 
fill(Xx,Yy,Cc,'EraseMode','background'); 

%  ref
Xr=[0.5   .95  .95   0.5  ] ;
Yr=[0    .5  -.5  0  ];
Cr=[1  1    1  1  ]; 
YHr=(7 + 5*y1/T_max);
CHr=(7 + 3*y1/T_max)/12; 
ref=fill(Xr,YHr+Yr,'y','EraseMode','background');

% 
% adiabatic edge
E=.3;
Xx=[17 15 15   0   0   15    15   17   ] + 1.5;   
Yy=[4*E 4*E E    E   -E  -E   -4*E -4*E ] + 7.;
line(Xx,Yy,'EraseMode','background','LineWidth', 6, 'Color', [0 0 0  ])
%  control source
Xx=[17  15   15    17   ] + 1.5;   
Yy=[4*E 4*E -4*E  -4*E ] + 7.;
Cu=[1  1    1     1 ];
CHu=(7+3*y1/2)/12;
UU=fill(Xx,Yy,CHu*Cu,'EraseMode','background');
text(18.7,7+4*E+.3,'heat source','color',[0 0 0],'EraseMode','background','HorizontalAlignment','right');

%heated beam 
Xt=scale*[ z   fliplr(z)  ] + 1.5 ;
Yt=E*[ -ones(1,nz) ones(1,nz)] + 7. ;
Ct=(7+3*H/2)/12;
Ct=[Ct fliplr(Ct)];
tube=fill(Xt,Yt,Ct,'EraseMode','background');




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


Hplay=uicontrol('style','push',...
                'units','normalized','position',[15/19 12.6/14 2/19 .5/14], ....
                'string','Plot','callback', ...
                ['figure(2);clf;' ...
                 'plot(t,Yrec);zoom on;'...
                 'title(''temperatures''); 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', ...
                ' ; ' ,'visible','off');
             
             
             
             
  % title
text(9.5,13.5,'HEATED ROD: 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,'string','Stop','callback','i=2;');
set(Hplay,'visible','on')
set(Hrun,'visible','on')



t1=1/10.;
seuil=T_max/3;
y1=0;

i=0;


tt=0.;
y1_old=y1;
while (i < 1)
    buf=get(gca,'CurrentPoint');
    A=2*(buf(1,2)-7)/5;
    A=max(-T_max,A);
    A=min(T_max,A); 
    
   t1b=t1*max(1,abs(A-y1)/seuil);
   y1=(y1+dt*A/t1b)/(1+dt/t1b);
   
   tt=tt+dt;
   if (tt >= dt0);
      ii=floor(tt/dt0);
      dy1=(y1-y1_old)/tt;
      for iii=1:ii;
         YY(mm)=YY(mm+1);YY(mt)=y1_old+iii*dt0*dy1;
      end;
      y1_old=y1-dy1*(tt-ii*dt0);
      tt=0.;
   end;
    
   
   
   H=YY*GG*HH;

    
    
    Yrec(nn,:)=Yrec(nn+1,:); Yrec(nt,:)=H(Nrec);
    t(nn)=t(nn+1); t(nt)=t(nt)+dt;
 
    CHu=(7+3*H(nz)/2)/12; CHu=max(0,CHu);CHu=min(1,CHu);
    Ct=(7+3*H/2)/12;
    Ct=[Ct fliplr(Ct)];Ct=max(0,Ct);Ct=min(1,Ct);
    
    YHr=(7 + 5*A/T_max);
    CHr=(7 + 3*A/T_max)/12; 

    
    set(tube,'FaceVertexCData',Ct'); 
    set(UU,'FaceVertexCData',CHu*Cu'); 
    set(ref,'Ydata',YHr+Yr); 
    drawnow;
   
end;


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

return