dt = 1;
n = 2;
v = .1;
u = 0.00;
r0 = 5;

agent1 = [];
agent2 = [];

x1= 0;
y1= 0; 
v1= 0;
theta1= 0;
u1 = u;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;

agent2(1,2)=y2;
agent2(1,3)=v2;
agent2(1,4)=theta2;
agent2(1,5)=u2;

coords = [];
coords(1, :) = [x1, y1, theta1];
coords(2, :) = [x2, y2, theta2];

figure;
plot(agent1(1,1),agent1(1,2), 'b+', agent2(1,1),agent2(1,2), 'r+')

count = 0;
c = -100;

for t=2:dt:600
    count = count + 1;
%     if (abs(mod(agent2(t-1, 4), 2*pi) - 5*pi/4) < pi/100 && r0 >=2 && (count - c) >= 5)
%         r0 = r0 - 1;
%         c = count;
%     end
    
    agent1 = Controlr0(t, dt, agent1, 1, coords, n, r0);
    agent2 = Controlr0(t, dt, agent2, 2, coords, n, r0);

    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];
	coords(2, :) = [agent2(t,1), agent2(t,2), agent2(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+')

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