dt = 1;
n = 6;
v = .02;
theta = 0;
u = 0.00;
lambda = 0;
w = 2;

agent1 = [];
agent2 = [];
agent3 = [];
agent4 = [];
agent5 = [];
agent6 = [];

x1= -2;
y1= 0; 
v1= v;
theta1= theta - pi/4 + pi*rand()/2;
u1 = u;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;

x2= -2;
y2= 0.2; 
v2= v;
theta2= 0;
u2 = u;
agent2(1,1)=x2;
agent2(1,2)=y2;
agent2(1,3)=v2;
agent2(1,4)=theta2;
agent2(1,5)=u2;

x3= -2;
y3= -.2; 
v3= v;
theta3= theta;
u3 = u;
agent3(1,1)=x3;
agent3(1,2)=y3;
agent3(1,3)=v3;
agent3(1,4)=theta3;
agent3(1,5)=u3;

x4= -2.2;
y4= 0; 
v4= v;
theta4= theta;
u4 = u;
agent4(1,1)=x4;
agent4(1,2)=y4;
agent4(1,3)=v4;
agent4(1,4)=theta4;
agent4(1,5)=u4;

x5= -2.2;
y5= 0.2; 
v5= v;
theta5= theta;
u5 = u;
agent5(1,1)=x5;
agent5(1,2)=y5;
agent5(1,3)=v5;
agent5(1,4)=theta5;
agent5(1,5)=u5;

x6= -2.2;
y6= -0.2; 
v6= v;
theta6= theta;
u6 = u;
agent6(1,1)=x6;
agent6(1,2)=y6;
agent6(1,3)=v6;
agent6(1,4)=theta6;
agent6(1,5)=u6;

coords = [];
coords(1, :) = [x1, y1, theta1];
coords(2, :) = [x2, y2, theta2];
coords(3, :) = [x3, y3, theta3];
coords(4, :) = [x4, y4, theta4];
coords(5, :) = [x5, y5, theta5];
coords(6, :) = [x6, y6, theta6];

figure;
n = 1;
targetx = 12;
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 = [4, -.25; 4,0;4, .25];%; 30, 1; 30, 2; 30, 3; 30, 4; 30, 5; 30, 6; 30, 7; 30, -1;...
    %30, -2; 30, -3; 30, -4; 30, -5; 30, -6; 30, -7];

plot(agent1(1,1),agent1(1,2), 'b+', agent2(1,1),agent2(1,2), 'r+',...
    agent3(1,1),agent3(1,2), 'g+', agent4(1,1),agent4(1,2), 'k+', ...
    agent5(1,1),agent5(1,2), 'y+', agent6(1,1),agent6(1,2), 'c+',...
    targetx, targety, 'go', barrier(:, 1), barrier(:, 2), 'rs')

for t=2:dt:1000
    agent1 = SAControl(t, dt, agent1, 1, coords, n, barrier, targetx, targety, w);
    agent2 = SAControl(t, dt, agent2, 2, coords, n, barrier, targetx, targety, w);
    agent3 = SAControl(t, dt, agent3, 3, coords, n, barrier, targetx, targety, w);
    agent4 = SAControl(t, dt, agent4, 4, coords, n, barrier, targetx, targety, w);
    agent5 = SAControl(t, dt, agent5, 5, coords, n, barrier, targetx, targety, w);
    agent6 = SAControl(t, dt, agent6, 6, coords, n, barrier, targetx, targety, w);
     
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];
    coords(2, :) = [agent2(t,1), agent2(t,2), agent2(t,4)];
    coords(3, :) = [agent3(t,1), agent3(t,2), agent3(t,4)];
    coords(4, :) = [agent4(t,1), agent4(t,2), agent4(t,4)];
    coords(5, :) = [agent5(t,1), agent5(t,2), agent5(t,4)];
    coords(6, :) = [agent6(t,1), agent6(t,2), agent6(t,4)];

    plot(agent1(:,1),agent1(:,2), agent1(t,1), agent1(t,2), 'b+',...
        agent2(:,1),agent2(:,2), 'r',agent2(t,1),agent2(t,2), 'r+',...
        agent3(:,1),agent3(:,2), 'g',agent3(t,1),agent3(t,2), 'g+',...
        agent4(:,1),agent4(:,2), 'k',agent4(t,1),agent4(t,2), 'k+',...
        agent5(:,1),agent5(:,2), 'y',agent5(t,1),agent5(t,2), 'y+',...
        agent6(:,1),agent6(:,2), 'c',agent6(t,1),agent6(t,2), 'c+',...
        targetx, targety, 'go', barrier(:, 1), barrier(:, 2), 'rs')

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