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

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

x1= 1;
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= -1;
y2= 1; 
v2= v;
theta2= theta;% - pi/4 + pi*rand()/2;
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= 1; 
v3= v;
theta3= theta;% - pi/4 + pi*rand()/2;
u3 = u;
agent3(1,1)=x3;
agent3(1,2)=y3;
agent3(1,3)=v3;
agent3(1,4)=theta3;
agent3(1,5)=u3;
% 
% x4= -1;
% y4= 0; 
% v4= v;
% theta4= theta - pi/4 + pi*rand()/2;
% u4 = u;
% agent4(1,1)=x4;
% agent4(1,2)=y4;
% agent4(1,3)=v4;
% agent4(1,4)=theta4;
% agent4(1,5)=u4;
% 
% x5= 0;
% y5= 0; 
% v5= v;
% theta5= theta - pi/4 + pi*rand()/2;
% u5 = u;
% agent5(1,1)=x5;
% agent5(1,2)=y5;
% agent5(1,3)=v5;
% agent5(1,4)=theta5;
% agent5(1,5)=u5;
% 
% x6= 0;
% y6= 1; 
% v6= v;
% theta6= theta - pi/4 + pi*rand()/2;
% 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;
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+')

for t=2:dt:1500
    if (t > 350 && t <= 400)
        lambda = lambda + 1/50;
    elseif(t>600 && t <= 650)
        lambda = lambda - 1/50;
    end
    
    agent1 = ControlHomotopyLeader(t, dt, agent1, 1, coords, n, lambda, w);
    agent2 = ControlHomotopyLeader(t, dt, agent2, 2, coords, n, lambda, w);
    agent3 = ControlHomotopyFollower(t, dt, agent3, 3, coords, n, lambda, w);
%     agent4 = ControlHomotopyFollower(t, dt, agent4, 4, coords, n, lambda, w);
%     agent5 = ControlHomotopyFollower(t, dt, agent5, 5, coords, n, lambda, w);
%     agent6 = ControlHomotopyFollower(t, dt, agent6, 6, coords, n, lambda, 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+')

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