dt = 1;
n = 1;
v = .05;
theta = 0;
u = 0.00;
w = 3;

agent1 = [];

x1= -2;
y1= 0; 
v1= v;
theta1= theta;
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 = 10;
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 = [...
    4, 1.00; 4, -1.0; 4, 2.0; 4, -2.0; 4, 3.0; 4, -3.0; 4, 4.0; 4, -4.0;...
    4, 1.10; 4, -1.1; 4, 2.1; 4, -2.1; 4, 3.1; 4, -3.1; 4, 4.1; 4, -4.1;...
    4, 1.20; 4, -1.2; 4, 2.2; 4, -2.2; 4, 3.2; 4, -3.2; 4, 4.2; 4, -4.2;...
    4, 1.30; 4, -1.3; 4, 2.3; 4, -2.3; 4, 3.3; 4, -3.3; 4, 4.3; 4, -4.3;...
    4, 1.40; 4, -1.4; 4, 2.4; 4, -2.4; 4, 3.4; 4, -3.4; 4, 4.4; 4, -4.4;...
    4, 1.50; 4, -1.5; 4, 2.5; 4, -2.5; 4, 3.5; 4, -3.5; 4, 4.5; 4, -4.5;...
    4, 1.60; 4, -1.6; 4, 2.6; 4, -2.6; 4, 3.6; 4, -3.6; 4, 4.6; 4, -4.6;...
    4, 1.70; 4, -1.7; 4, 2.7; 4, -2.7; 4, 3.7; 4, -3.7; 4, 4.7; 4, -4.7;...
    4, 1.80; 4, -1.8; 4, 2.8; 4, -2.8; 4, 3.8; 4, -3.8; 4, 4.8; 4, -4.8;...
    4, 1.90; 4, -1.9; 4, 2.9; 4, -2.9; 4, 3.9; 4, -3.9; 4, 4.9; 4, -4.9;...
    4, 0.0; 4, 0.1; 4, 0.2; 4, 0.3; 4, 0.4; 4, 0.5; 4, 0.6; 4, 0.7; 4, 0.8; 4, 0.9;...
    4, -0.1; 4, -0.2; 4, -0.3; 4, -0.4; 4, -0.5; 4, -0.6; 4, -0.7; 4, -0.8; 4, -0.9;...
    4, 5.0; 4, 5.1; 4, 5.2; 4, 5.3; 4, 5.4; 4, 5.5; 4, 5.6; 4, 5.7; 4, 5.8; 4, 5.9;...
    4, -5.0; 4, -5.1; 4, -5.2; 4, -5.3; 4, -5.4; 4, -5.5; 4, -5.6; 4, -5.7; 4, -5.8; 4, -5.9;...
    ];
% barrier = [4, 0];

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

for t=2:dt:600
    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([-5, 15, -10, 10]);
    M(t)= getframe;
end