dt = 1;
n = 6;
v = .03;
theta = 0;
u = 0.00;
lambda = 0;
w = 3;

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

x1= -1;
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;

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

x3= -1;
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= -1.2;
y4= .2; 
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= -1.2;
y5= -.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= -1.2;
y6= 0; 
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 = 25;
targety = 0;

% barrier = vertcat(VerticalWall(4,0), VerticalWall(4,1), VerticalWall(4,-1),...
%     VerticalWall(4,-2),VerticalWall(4,2),VerticalWall(4,-3),VerticalWall(4,3),...
%     VerticalWall(4,-4),VerticalWall(4,4),VerticalWall(4,-5),VerticalWall(4,5),...
%     VerticalWall(4,-6),VerticalWall(4,6),VerticalWall(4,-7),VerticalWall(4,7),...
%     VerticalWall(4,-8),VerticalWall(4,8),VerticalWall(4,-9),VerticalWall(4,9),...
%     VerticalWall(4,-10),VerticalWall(4,10),VerticalWall(4,-11),VerticalWall(4,11),...
%     VerticalWall(4,-12),VerticalWall(4,12),VerticalWall(4,-13),VerticalWall(4,13),...
%     VerticalWall(4,-14),VerticalWall(4,14),VerticalWall(4,-15),VerticalWall(4,15)...
% );
barrier = vertcat(ThickVerticalWall(4,0), ThickVerticalWall(4,1), ThickVerticalWall(4,-1),...
    ThickVerticalWall(4,-2),ThickVerticalWall(4,2),ThickVerticalWall(4,-3),ThickVerticalWall(4,3),...
    ThickVerticalWall(4,-4),ThickVerticalWall(4,4),ThickVerticalWall(4,-5),ThickVerticalWall(4,5),...
    ThickVerticalWall(4,-6),ThickVerticalWall(4,6),ThickVerticalWall(4,-7),ThickVerticalWall(4,7),...
    ThickVerticalWall(4,-8),ThickVerticalWall(4,8),ThickVerticalWall(4,-9),ThickVerticalWall(4,9),...
    ThickVerticalWall(4,-10),ThickVerticalWall(4,10),ThickVerticalWall(4,-11),ThickVerticalWall(4,11),...
    ThickVerticalWall(4,-12),ThickVerticalWall(4,12),ThickVerticalWall(4,-13),ThickVerticalWall(4,13),...
    ThickVerticalWall(4,-14),ThickVerticalWall(4,14),ThickVerticalWall(4,-15),ThickVerticalWall(4,15),...
    ThickHorizontalWall(3,-15),ThickHorizontalWall(2,-15),ThickHorizontalWall(1,-15),ThickHorizontalWall(0,-15),...
    ThickHorizontalWall(-1,-15),ThickHorizontalWall(-2,-15),ThickHorizontalWall(-3,-15),ThickHorizontalWall(-4,-15),...
    ThickHorizontalWall(-5,-15),ThickHorizontalWall(-6,-15),ThickHorizontalWall(-7,-15),ThickHorizontalWall(-8,-15),...
    ThickHorizontalWall(-9,-15),ThickHorizontalWall(-10,-15),...
    ThickVerticalWall(-10,-2),ThickVerticalWall(-10,2),ThickVerticalWall(-10,-3),ThickVerticalWall(-10,3),...
    ThickVerticalWall(-10,-4),ThickVerticalWall(-10,4),ThickVerticalWall(-10,-5),ThickVerticalWall(-10,5),...
    ThickVerticalWall(-10,-6),ThickVerticalWall(-10,6),ThickVerticalWall(-10,-7),ThickVerticalWall(-10,7),...
    ThickVerticalWall(-10,-8),ThickVerticalWall(-10,8),ThickVerticalWall(-10,-9),ThickVerticalWall(-10,9),...
    ThickVerticalWall(-10, 10),ThickVerticalWall(-10,-10),ThickVerticalWall(-10,-11),ThickVerticalWall(-10,11),...
    ThickVerticalWall(-10,-12),ThickVerticalWall(-10,12),ThickVerticalWall(-10,-13),ThickVerticalWall(-10,13),...
    ThickVerticalWall(-10,-14),ThickVerticalWall(-10,14),ThickVerticalWall(-10,-15),ThickVerticalWall(-10,15),...
    ThickVerticalWall(-10,0), ThickVerticalWall(-10,1), ThickVerticalWall(-10,-1),...
    ThickHorizontalWall(3,16),ThickHorizontalWall(2,16),ThickHorizontalWall(1,16),ThickHorizontalWall(0,16),...
    ThickHorizontalWall(-1,16),ThickHorizontalWall(-2,16),ThickHorizontalWall(-3,16),ThickHorizontalWall(-4,16),...
    ThickHorizontalWall(-5,16),ThickHorizontalWall(-6,16),ThickHorizontalWall(-7,16),ThickHorizontalWall(-8,16),...
    ThickHorizontalWall(-9,16),ThickHorizontalWall(-10,16)...
);
% barrier = vertcat(ThinVerticalWall(4,0), ThinVerticalWall(4,1), ThinVerticalWall(4,-1),...
%     ThinVerticalWall(4,-2),ThinVerticalWall(4,2),ThinVerticalWall(4,-3),ThinVerticalWall(4,3),...
%     ThinVerticalWall(4,-4),ThinVerticalWall(4,4),ThinVerticalWall(4,-5),ThinVerticalWall(4,5),...
%     ThinVerticalWall(4,-6),ThinVerticalWall(4,6),ThinVerticalWall(4,-7),ThinVerticalWall(4,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:2000
    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([-12, 6, -20, 20]);
    M(t)= getframe;
end