dt = 1;
n = 1;
v = .5;
theta = pi/4;
u = 0.00;
w = 3000;

agent1 = [];

x1= -2;
y1= 0; 
v1= v;
theta1= 0;
u1 = u;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;

figure;
n = 1;
coords = [];
coords(1, :) = [x1, y1, theta1];
targetx = 100;
targety = 0;
% barrier = [3, 0; 3, 1; 3, 2;3, 3; 3, -1; 3, -.5; 2, 3; 1, 3; 0,3; -1, 3; -2, 3;...
%     3, .5; 3, 1.5; 3, 2.5;3, 3; 3, -1; 3, -.5; 2.5, 3; 1.5, 3; 0.5,3; -.5, 3; -1.5, 3;...
%     -2, 2; -2, 1; -2, 0; 0, 3];
% barrier = [3, 0; 3, .1; 3, .2; 3, .3; 3, .4; 3, .5; 3, .6; 3, .7; 3, -.1;...
%     3, -.2; 3, -.3; 3, -.4; 3, -.5; 3, -.6; 3, -.7];
barrier = [30, 0];

plot(agent1(1,1),agent1(1,2), 'b+', targetx, targety, 'go', barrier(:, 1), barrier(:, 2), 'rs')

for t=2:dt:1600
    agent1 = SAControl(t, dt, agent1, 1, coords, n, barrier, targetx, targety, w);
     
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];

    plot(agent1(:,1),agent1(:,2), agent1(t,1), agent1(t,2), 'b+',...
        targetx, targety, 'go', barrier(:, 1), barrier(:, 2), 'rs')

    axis([-100, 100, -100, 100]);
    M(t)= getframe;
end