dt = 1;
n = 1;
v = .05;
theta = pi/4;
u = 0.00;
w = 3;

uMax = 1/3;

agent1 = [];

x1= -7;
y1= -7; 
v1= v;
theta1= pi/2;
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 = 7;
targety = 7;
% 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 = [30, 10; 35, 10; 40, 10; 45, 10; 50, 10;...
%     30, -10; 35, -10; 40, -10; 45, -10; 50, -10;...
%     50, 15; 50, 20; 50, 25; 50, -15; 50, -20; 50, -25;...
%     70, 0; 70, 5; 70, 10; 70, 15; 70, 20; 70, 25; 70, -5;...
%     70, -10; 70, -15; 70, -20; 70, -25; 75, -25; 80, -25;...
%     75, 25; 80, 25];
% barrier = [0,0; -3, 3; -5,-3; -2, 4; 1,7; 4,-8; 10, 10; 6,-2; 2, -2; 5. 3 ];
barrier = [-5, -3; 0, 0; -3, 4; 6, -2; 3, 6];

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

for t=2:dt:470
    agent1 = SAControl(t, dt, agent1, 1, coords, n, barrier, targetx + 1, targety, w);
    if agent1(t, 5) > uMax
        agent1(t, 5) = uMax;
    elseif agent1(t, 5) < -uMax
        agent1(t, 5) = -uMax;
    end
     
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];

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

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