figure;
dt=1;
% bottomLeftx = -4.8;
% bottomLefty = -3.2;
% topRightx = 4.8;
% topRighty = 3.2;
bottomLeftx = -5;
bottomLefty = -5;
topRightx = 5;
topRighty = 7;

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

agent2 = [];
x2 = -4 ;
y2 = 0;
v2= .04;
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-.5*sin(theta2);
y1 = y2+.5*cos(theta2);
v1= v2;
theta1 = theta2;
u1 = 0;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;

agent3 = [];
x3 = x2+.5*sin(theta2);
y3 = y2-.5*cos(theta2);
v3= v2;
theta3 = theta2;
u3 = 0;
agent3(1,1)=x3;
agent3(1,2)=y3;
agent3(1,3)=v3;
agent3(1,4)=theta3;
agent3(1,5)=u3;
% 
% 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.5;
% rect5w = .75;
% rect5h = 3;
% 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];
% 
% target = [3.4, .5];
% 
% rectangles = [rect1; rect2; rect3; rect4; rect5; rect6; rect7];
% numberOfRectangles = 7;

rect1x = -1;
rect1y = -3;
rect1w = 2;
rect1h = 5;
rect1 = [rect1x rect1y rect1w rect1h];

target = [4, 0];

rectangles = rect1;
numberOfRectangles = 1;

lastDistStr = -1;
lastDistRight = -1;
lastDistLeft = -1;
lastTurning1 = 0;
lastTurning2 = 0;

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

for t=2:dt:1500
    [agent2, keepTurning2, lastTurning1, lastTurning2, lastDistStr] = ...
        MultipleIRControlStraight(t, dt, agent2, coords, rectangles,...
        numberOfRectangles, target, keepTurning2, lastTurning1, lastTurning2);
    [agent1, lastDistLeft] = MultipleIRControlLeft(t,...
        dt, agent1, coords, rectangles, numberOfRectangles, target,...
        lastDistLeft);
    [agent3, lastDistRight] = MultipleIRControlRight(t,...
        dt, agent3, coords, rectangles, numberOfRectangles, target,...
        lastDistRight);
    
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4), agent1(t,3)];
    coords(2, :) = [agent2(t,1), agent2(t,2), agent2(t,4), agent2(t,3)];
    coords(3, :) = [agent3(t,1), agent3(t,2), agent3(t,4), agent3(t,3)];
    
    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 && agent1(t,3)==0 && agent3(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;
end