% function  [m,t] = wtnk1D
%  Mouse-driven motion planning
%      for  Quantumbox problem
%  sept 2002
%  Ecole des Mines de Paris, 
%  Centre Automatique et Systemes
%  60 Bd St Michel 
%  75272 Paris
%    Pierre Rouchon: pierre.rouchon@.ensmp.fr
% 
% Linear approximation of the 1D Schrodinger equation 
% around the ground state cos(pi*x/2) of energy pi^2/4  for a potential well of length 2: 
%  
%  i \dot phi = - phi''  for  x between u-1 and u+1
%  phi(u-1,t)=phi(u+1,t)=0
%  
% the control is the box position u
% 
%     

L=2.; 
Ht=4.;
D_max=1.5*Ht;

S=min(19/(L+D_max),14/Ht); % scaling

w0=pi^2/4;
%T_max=2*sqrt(D_max/g);  % interval of motion (s)
%eps=1.e-8;
%sig=1.7*sqrt(L/g);

rr=1/w0;
sig=.3/w0;
DT=4*sig ;


% space mesh 
dx=L/20.;
x=[0:dx:L];
nx=length(x);
phi0=cos(pi*(2*x/L-1)/2);
dphi0=-pi/2*sin(pi*(2*x/L-1)/2);
% time step 
dt0=DT/100/4; 




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

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



figure(1),clf;
go=1;
mouse=0;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add')
set(gcf,'CloseRequestFcn','go=0; closereq;',...
	'WindowButtonUpFcn',['mouse=0;set(ref,''facecolor'',''y'') ;'...
		'set(gcf,''WindowButtonMotionFcn'','''');']);
set(gcf,'renderer','openGL');


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

Xbox=Xi + L*S + L*S*[0   0  -1  -1 ];
Ybox=Yi+[Ht*S 0 0 Ht*S];
Xp=[ Xbox Xi+x*S    ];
Yp=[ Ybox Yi+S*abs(phi0).^2    ];
Qp=fill(Xp,Yp,'c','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',...
        'ButtonDownFcn',['mouse=1; set(ref,''facecolor'',''r'');' ...
           '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;' ...
                 'plot(t,Y,t,D,''linewidth'',3);'...
                 'legend(''flat output'',''box position'');pause,figure(1);'...
                 ]);
             
             
Hrun=uicontrol('style','push',...
                'units','normalized','position',[1/19 12.7/14 2/19 .5/14], ....
                'string','stop','callback', ...
                ' go=0; ','visible','off' );
             
             
             
             
  % title
text(9.5,13.5,'THE QUANTUM BOX: MOUSE-DRIVEN MOTION PLANNING','color',[0 0 0],...
   'HorizontalAlignment','center');
           
             
             
             

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




dth=2*pi/100;
th=[0:dth:2*pi-dth];




% the matrix (nx,mt)
Mphi=zeros(nx,mt);
Md=zeros(1,mt);
My=zeros(1,mt);
i=sqrt(-1);   
z=rr*exp(i*th); invz=1./z;

dq=0.01; q=[dq:dq:1]; nq=length(q);

for ix=1:nx;
     xx=2*x(ix)/L-1;
     buf0=0*z;
     for iq=1:nq  
         ss=-log(q(iq))*invz;
         buf0=buf0+(sin(xx*sqrt(i*ss+w0))./sqrt(i*ss+w0)) .* (sin( sqrt(-i*ss+w0))./sqrt(-i*ss+w0)) ;

     end;
     buf0=dq*buf0;
     for it=1:mt;
       buf1=buf0.*exp(-(z-TT(it)).^2/2/(sig^2));
       Mphi(ix,it)=dt0*(dth)*sum(buf1)/(2*pi)/sqrt(2*pi)/sig;
    end    
end  
Mphi=(pi/2)^3*Mphi;


buf0=0*z;
for iq=1:nq  
    ss=-log(q(iq))*invz;
    buf0=buf0+ (2/pi)*(sin(sqrt(i*ss+w0))./ sqrt(i*ss+w0)) .* (sin(sqrt(-i*ss+w0))./sqrt(-i*ss+w0)) ;
end;
buf0=dq*buf0;
for it=1:mt;   
    buf1=buf0.*exp(-(z-TT(it)).^2/2/(sig^2));
    Md(it)=dt0*(dth)*sum(buf1)/(2*pi)/sqrt(2*pi)/sig;
end    
Md=(pi/2)^3*Md;


for it=1:mt;   
    buf1=exp(-(z-TT(it)).^2/2/(sig^2));
    My(it)=dt0*(dth)*sum(buf1)/(2*pi)/sqrt(2*pi)/sig;
end    







t1=sig/5;
t2=t1;
seuil=D_max/S;
y1=0;
y2=0;


tt=0.;
y1_old=y1;
buf(1,1)=9.5;
go=1;
while (go >0)
    if mouse > 0 
       buf=get(gca,'CurrentPoint');
    end

    A=(buf(1,1)-9.5)/S;
    A=max(-D_max/2.1,A);
    A=min(D_max/2.1,A); 
     
   y1=(y1+dt0*y2/t1)/(1+dt0/t1);
   t2b=t2*max(1,abs(A-y2)/seuil);
   y2=(y2+dt0*A/t2b)/(1+dt0/t2b);
   
   YY(mm)=YY(mm+1);YY(mt)=y1;
    
   
   dphi=Mphi*YY;
   dd=Md*YY;
   yy=My*YY;
   dd=real(dd);
   yy=real(yy);
 
    dphi=dphi + dphi0'*dd;
   
  
    phi=phi0'+dphi;   
   
    phi=sqrt(sum(phi0.*conj(phi0))/sum(phi.*conj(phi))) * phi;
   


    
    
    Y(nn)=Y(nn+1); Y(nt)=yy;
    D(nn)=D(nn+1); D(nt)=dd;
    t(nn)=t(nn+1); t(nt)=t(nt)+dt0;
   
    
 

   Yp=[ Ybox Yi+S*(abs(phi').^2 )];
   
   delete(Qp);Qp=fill(Xp+S*dd,Yp,'c','FaceColor', [0 0 1 ]);
   set(ref,'XData',Xr+S*A); 
   drawnow;
   
end;


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

return
