function out=robot_movie(X0,Y0,TH0,KP,KD,DSEP)

%%  William Dunbar and Jorge Cortes

%%  Computer Engineering
%%  University of California at Santa Cruz
%%  dunbar@soe.ucsc.edu

%%  Applied Mathematics and Statistics
%%  University of California at Santa Cruz
%%  jcortes@soe.ucsc.edu
	
%%  Copyright (c) July 2005, Jorge Cortes, William Dunbar
%%  Permission is granted to copy, modify and redistribute this file, 
%%  provided that this header message is retained.
 
% Function robot_movie(x0,y0,th0,kp,kd,dsep) has input arguments:
%   x0   - initial x
%   y0   - initial y
%   th0  - initial theta
%   kp   - proportional control gain
%   kd   - derivative control gain
%   dsep - desired separation distance (constant) between robot and wall
%
% Function calls ROBOBRAINCONTROL.M and
%   outputs visualization of feedback control.


%simulation parameters
delta = 0.01;
N = 1000;

[x,y,theta,tout] = robobrainControl(X0,Y0,TH0,delta,N,KP,KD,DSEP);

figure(1)
grid on,hold 
xlabel('X motion (m)')
ylabel('Y motion (m)')
title('Position Space (Tracking)')

daspect([1.0 1.0 1.0]) %shows even scaling on axes
plot(x,y,'k'),hold on,plot(x(1),y(1),'k x'),plot(x(end),y(end),'k o')
plot([1 1],[-5 5],'r')


%%%%% Movie %%%%% 

% vehicle dims speced at theta = 0
wallX = [-6 0 0 -6]';
wallY = [-30 -30 30 30]';

veh1X = [-0.2 0.2 0.2 -0.2]';
veh1Y = [-0.2 -0.2 0.2 0.2]';
LwheelX = [-0.1 0.1 0.1 -0.1]';
LwheelY = 0.5+[-0.3 -0.3 -0.2 -0.2]';
RwheelX = [-0.1 0.1 0.1 -0.1]';
RwheelY = [-0.3 -0.3 -0.2 -0.2]';


hold on
h = fill(wallX,wallY,'k');

h1 = fill(veh1X,veh1Y,'b');
h2 = fill(LwheelX,LwheelY,'y'); 
h3 = fill(RwheelX,RwheelY,'y'); 


set(h1,'Erasemode','Background');
set(h2,'Erasemode','Background');
set(h3,'Erasemode','Background');

hold off


i=1;
%axis([x(i)-2 x(i)+2 y(i)-2 y(i)+2])     
axis([-5 15 -5 15])

    set(h1,'Xdata',veh1X*cos(theta(i))-veh1Y*sin(theta(i))+x(i),...
        'Ydata',veh1Y*cos(theta(i))+veh1X*sin(theta(i))+y(i));
    set(h2,'Xdata',LwheelX*cos(theta(i))-LwheelY*sin(theta(i))+x(i),...
        'Ydata',LwheelY*cos(theta(i))+LwheelX*sin(theta(i))+y(i));
    set(h3,'Xdata',RwheelX*cos(theta(i))-RwheelY*sin(theta(i))+x(i),...
        'Ydata',RwheelY*cos(theta(i))+RwheelX*sin(theta(i))+y(i));
     
    drawnow
    %pause
    

for i=2:length(tout),
%axis([x(i)-2 x(i)+2 y(i)-2 y(i)+2])
    
    set(h1,'Xdata',veh1X*cos(theta(i))-veh1Y*sin(theta(i))+x(i),...
        'Ydata',veh1Y*cos(theta(i))+veh1X*sin(theta(i))+y(i));
    set(h2,'Xdata',LwheelX*cos(theta(i))-LwheelY*sin(theta(i))+x(i),...
        'Ydata',LwheelY*cos(theta(i))+LwheelX*sin(theta(i))+y(i));
    set(h3,'Xdata',RwheelX*cos(theta(i))-RwheelY*sin(theta(i))+x(i),...
        'Ydata',RwheelY*cos(theta(i))+RwheelX*sin(theta(i))+y(i));
    
drawnow

end
