figure;
dt=1;
bottomLeftx = -4.8;
bottomLefty = -3.2;
topRightx = 5.5;
topRighty = 3.2;

xAxisMin = bottomLeftx - 1;
xAxisMax = topRightx + 1;
yAxisMin = bottomLefty - 1;
yAxisMax = topRighty + 1;

uMax = 1;

agent2 = [];
x2 = -4.5 ;
y2 = 5*rand() - 2.5;
v2= .03;
theta2 = 0;
u2 = 0;
agent2(1,1)=x2;
agent2(1,2)=y2;
agent2(1,3)=v2;
agent2(1,4)=theta2;
agent2(1,5)=u2;
keepTurning2 = 0;

agent1 = [];
x1 = x2;
y1 = y2+.3;
v1= .03;
theta1 = theta2;
u1 = 0;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;
keepTurning1 = 0;

agent3 = [];
x3 = x2;
y3 = y2-.3;
v3= .03;
theta3 = theta2;
u3 = 0;
agent3(1,1)=x3;
agent3(1,2)=y3;
agent3(1,3)=v3;
agent3(1,4)=theta3;
agent3(1,5)=u3;
keepTurning3 = 0;

rect1x = bottomLeftx - 1;
rect1y = topRighty;
rect1w = topRightx - bottomLeftx + 2;
rect1h = 1;
rect1 = [rect1x rect1y rect1w rect1h];

rect2x = bottomLeftx - 1;
rect2y = bottomLefty - 1;
rect2w = topRightx - bottomLeftx + 2;
rect2h = 1;
rect2 = [rect2x rect2y rect2w rect2h];

rect3x = bottomLeftx - 1;
rect3y = bottomLefty;
rect3w = 1;
rect3h = topRighty - bottomLefty;
rect3 = [rect3x rect3y rect3w rect3h];

rect4x = topRightx;
rect4y = bottomLefty;
rect4w = 1;
rect4h = topRighty - bottomLefty;
rect4 = [rect4x rect4y rect4w rect4h];

rect5x = -2.2;
rect5y = -1;
rect5w = .75;
rect5h = 2;
rect5 = [rect5x rect5y rect5w rect5h];

rect6x = 1.2;
rect6y = 1.2;
rect6w = 1;
rect6h = 3;
rect6 = [rect6x rect6y rect6w rect6h];

rect7x = 1.2;
rect7y = -4.2;
rect7w = 1;
rect7h = 3;
rect7 = [rect7x rect7y rect7w rect7h];
% 
% rect8x = 5;
% rect8y = -1;
% rect8w = 2;
% rect8h = 4;
% rect8 = [rect8x rect8y rect8w rect8h];


test = [];
test(1, :) = [0, 0, 0, 0, 0,0, 0];

target = [3.8, 5*rand() - 2.5];

rectangles = [rect1; rect2; rect3; rect4; rect5; rect6; rect7];
numberOfRectangles = 7;

lastDistStr = -1;
lastDistRight = -1;
lastDistLeft = -1;

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

for t=1:dt:1500
    if (t>1)
        [agent2, keepTurning2, lastDistStr, lastDistRight, lastDistLeft, test] = ...
            MultipleIRControl(t, dt, agent2, coords, rectangles, numberOfRectangles, target,...
            keepTurning2, lastDistStr, lastDistRight, lastDistLeft, uMax, test);
        agent1 = ControlFollower(t, dt, agent1, 1, coords, 3, 2, uMax);
        agent3 = ControlFollower(t, dt, agent3, 3, coords, 3, 2, uMax);
    
        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)];
    end
    
    plot(agent1(:,1),agent1(:,2), agent1(t,1), agent1(t,2), 'b+',...
        agent2(:,1),agent2(:,2), 'g' , agent2(t,1), agent2(t,2), 'g+',...
        agent3(:,1),agent3(:,2), 'r' , agent3(t,1), agent3(t,2), 'r+',...
        target(1), target(2), 'go')
%     plot(agent1(:,1),agent1(:,2), agent1(t,1), agent1(t,2), 'b+', target(1), target(2), 'go')

    hold on;
    for i=1:numberOfRectangles
        rectangle('Position', rectangles(i,:));
    end
    if (agent2(t,3)==0 || agent2(t,1)>topRightx || agent2(t,1)<bottomLeftx ...
            || agent2(t,2)>topRighty || agent2(t,2)<bottomLefty) 
        break;
    end
        
    hold off;
    axis([xAxisMin, xAxisMax, yAxisMin, yAxisMax]);
    M(:, t)= getframe(figure(16), [0, 0, 560, 420]);
end