figure;
dt=1;
xAxisMin = -10;
yAxisMin = -10;
xAxisMax = 10;
yAxisMax = 10;

agent1 = [];
% x1= -8.5 + 2*rand();
% y1= -7.5 + 15*rand(); 
x1 = -7;
y1 = 0;
v1= .02;
% theta1= -pi/2 + pi*rand();
theta1 = 0;
u1 = 0;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;
keepTurning = 0;

coords = [];
coords(1, :) = [x1, y1, theta1];

rect1x = -10;
rect1y = 9;
rect1w = 20;
rect1h = 1;
rect1 = [rect1x rect1y rect1w rect1h];

rect2x = -10;
rect2y = -10;
rect2w = 20;
rect2h = 1;
rect2 = [rect2x rect2y rect2w rect2h];

rect3x = -10;
rect3y = -9;
rect3w = 1;
rect3h = 18;
rect3 = [rect3x rect3y rect3w rect3h];

rect4x = 9;
rect4y = -9;
rect4w = 1;
rect4h = 18;
rect4 = [rect4x rect4y rect4w rect4h];

rect5x = -5;
rect5y = -3;
rect5w = 1;
rect5h = 6;
rect5 = [rect5x rect5y rect5w rect5h];

rect6x = -1;
rect6y = 3;
rect6w = 2;
rect6h = 3;
rect6 = [rect6x rect6y rect6w rect6h];

rect7x = 4.5;
rect7y = -4;
rect7w = 1.5;
rect7h = 8;
rect7 = [rect7x rect7y rect7w rect7h];

rect8x = 0;
rect8y = -7;
rect8w = 2;
rect8h = 4;
rect8 = [rect8x rect8y rect8w rect8h];

rect9x = 6;
rect9y = -9;
rect9w = 3;
rect9h = 2;
rect9 = [rect9x rect9y rect9w rect9h];

rect10x = -2.5;
rect10y = -1;
rect10w = 1;
rect10h = 1;
rect10 = [rect10x rect10y rect10w rect10h];

rect11x = -5;
rect11y = -9;
rect11w = 1;
rect11h = 2;
rect11 = [rect11x rect11y rect11w rect11h];

rect12x = -5;
rect12y = 7;
rect12w = 1;
rect12h = 2;
rect12 = [rect12x rect12y rect12w rect12h];

target = [8, 0];

rectangles = [rect1; rect2; rect3; rect4; rect5; rect6; rect7; rect8; rect9; rect10; rect11; rect12];
numberOfRectangles = 12;

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

for t=2:dt:3500
    [agent1, keepTurning, lastDistStr, lastDistRight, lastDistLeft] = ...
        TestControl(t, dt, agent1, rectangles, numberOfRectangles, target,...
        keepTurning, lastDistStr, lastDistRight, lastDistLeft);
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];
    
    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 (agent1(t,3)==0 || agent1(t,1)>9 || agent1(t,1)<-9 || agent1(t,2)>9 || agent1(t,2)<-9) 
        break;
    end
        
    hold off;
    axis([xAxisMin, xAxisMax, yAxisMin, yAxisMax]);
    M(t)= getframe;
end