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

agent1 = [];
agent2 = [];

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

x2= -8;
y2= -8; 
v2= v;
theta2= pi/2;
u2 = u;
agent2(1,1)=x2;
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:1300
    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 = SimpleControl(t, dt, agent1);
    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