dt = 1;
n = 6;
v = .1;
theta = pi + pi/4;
u = 0.00;
lambda = 0;
w = 3;

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

x1= 20*rand() - 10;
y1= 20*rand() - 10; 
v1= v;
theta1= 2*pi*rand();
u1 = u;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;
agent1(2,1)=x1;
agent1(2,2)=y1;
agent1(2,3)=v1;
agent1(2,4)=theta1;
agent1(2,5)=u1;
% agent1 = [-128.591986873467,-7.20532548463755,0.100000000000000,32.5630693282286,-0.265621296129694;-128.550877500858,-7.11416617637094,0.100000000000000,32.5365071986157,-0.251850683004155;];
% agent2 = [-124.666752015099,-1.96081378191167,0.100000000000000,54.3585453830383,0.0678244240928800;-124.724800667012,-2.04224076767842,0.100000000000000,54.3653278254476,0.0735294709259059;];
% agent3 = [-121.654722224355,0.805839025998681,0.100000000000000,49.7180233179798,-0.765676352574277;-121.569337239950,0.753787086187173,0.100000000000000,49.6414556827223,-0.778292571561338;];
% agent4 = [-125.547735328133,-3.61360701028611,0.100000000000000,4.72242870690426,0.450829858871440;-125.546731372347,-3.71360197052301,0.100000000000000,4.76751169279140,0.457314481938605;];
% agent5 = [-123.485932592867,-7.34538727422873,0.100000000000000,1.74778986801871,0.240301691155307;-123.503539681194,-7.24694952516549,0.100000000000000,1.77182003713424,0.240720214606261;];
% agent6 = [-125.086966349329,-4.11921306987487,0.100000000000000,-115.144130038092,0.187716096490105;-125.132788938853,-4.20809664703810,0.100000000000000,-115.125358428443,0.176515458940353;];


x2= 20*rand() - 10;
y2= 20*rand() - 10; 
v2= v;
theta2= 2*pi*rand();
u2 = u;
agent2(1,1)=x2;
agent2(1,2)=y2;
agent2(1,3)=v2;
agent2(1,4)=theta2;
agent2(1,5)=u2;
agent2(2,1)=x2;
agent2(2,2)=y2;
agent2(2,3)=v2;
agent2(2,4)=theta2;
agent2(2,5)=u2;

x3= 20*rand() - 10;
y3= 20*rand() - 10; 
v3= v;
theta3= 2*pi*rand();
u3 = u;
agent3(1,1)=x3;
agent3(1,2)=y3;
agent3(1,3)=v3;
agent3(1,4)=theta3;
agent3(1,5)=u3;
agent3(2,1)=x3;
agent3(2,2)=y3;
agent3(2,3)=v3;
agent3(2,4)=theta3;
agent3(2,5)=u3;

x4= 20*rand() - 10;
y4= 20*rand() - 10; 
v4= v;
theta4= 2*pi*rand();
u4 = u;
agent4(1,1)=x4;
agent4(1,2)=y4;
agent4(1,3)=v4;
agent4(1,4)=theta4;
agent4(1,5)=u4;
agent4(2,1)=x4;
agent4(2,2)=y4;
agent4(2,3)=v4;
agent4(2,4)=theta4;
agent4(2,5)=u4;

x5= 20*rand() - 10;
y5= 20*rand() - 10; 
v5= v;
theta5= 2*pi*rand();
u5 = u;
agent5(1,1)=x5;
agent5(1,2)=y5;
agent5(1,3)=v5;
agent5(1,4)=theta5;
agent5(1,5)=u5;
agent5(2,1)=x5;
agent5(2,2)=y5;
agent5(2,3)=v5;
agent5(2,4)=theta5;
agent5(2,5)=u5;

x6= 20*rand() - 10;
y6= 20*rand() - 10; 
v6= v;
theta6= 2*pi*rand();
u6 = u;
agent6(1,1)=x6;
agent6(1,2)=y6;
agent6(1,3)=v6;
agent6(1,4)=theta6;
agent6(1,5)=u6;
agent6(2,1)=x6;
agent6(2,2)=y6;
agent6(2,3)=v6;
agent6(2,4)=theta6;
agent6(2,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;
plot(agent1(1,1),agent1(1,2), 'b+', agent2(1,1),agent2(1,2), 'c+',...
    agent3(1,1),agent3(1,2), 'g+', agent4(1,1),agent4(1,2), 'y+', ...
    agent5(1,1),agent5(1,2), 'r+', agent6(1,1),agent6(1,2), 'k+')

for t=2:dt:1500
    agent1(1, :) = agent1(2, :);
    agent2(1, :) = agent2(2, :);
    agent3(1, :) = agent3(2, :);
    agent4(1, :) = agent4(2, :);
    agent5(1, :) = agent5(2, :);
    agent6(1, :) = agent6(2, :);
    
    agent1 = ControlLeaderFollower(dt, agent1, 1, coords, n, lambda, w);
    agent2 = ControlLeaderFollower(dt, agent2, 2, coords, n, lambda, w);
    agent3 = ControlLeaderFollower(dt, agent3, 3, coords, n, lambda, w);
    agent4 = ControlLeaderFollower(dt, agent4, 4, coords, n, lambda, w);
    agent5 = ControlLeaderFollower(dt, agent5, 5, coords, n, lambda, w);
    agent6 = ControlLeaderFollower(dt, agent6, 6, coords, n, lambda, w);
     
    coords(1, :) = [agent1(2,1), agent1(2,2), agent1(2,4)];
	coords(2, :) = [agent2(2,1), agent2(2,2), agent2(2,4)];
	coords(3, :) = [agent3(2,1), agent3(2,2), agent3(2,4)];
    coords(4, :) = [agent4(2,1), agent4(2,2), agent4(2,4)];
	coords(5, :) = [agent5(2,1), agent5(2,2), agent5(2,4)];
	coords(6, :) = [agent6(2,1), agent6(2,2), agent6(2,4)];

    plot(agent1(2,1), agent1(2,2), 'b+',agent2(2,1),agent2(2,2), 'c+',...
        agent3(2,1),agent3(2,2), 'g+',agent4(2,1),agent4(2,2), 'y+',...
        agent5(2,1),agent5(2,2), 'r+',agent6(2,1),agent6(2,2), 'k+')
    
    axis([-40, 40, -40, 40]);
    axis manual;
    pause(.001)
    M(t)= getframe;
end