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

agent1 = [];
x1= -1;
y1= 0; 
v1= .02;
theta1= pi/5;
u1 = 0;
avoidSwitch1=0;
avoidStartTheta1=0;
avoidEndTheta1=0;
avoidDistanceLeft1=0;
firstDistance1=0;
secondDistance1=0;
agent1(1,1)=x1;
agent1(1,2)=y1;
agent1(1,3)=v1;
agent1(1,4)=theta1;
agent1(1,5)=u1;
agent1(1,6)=avoidSwitch1;
agent1(1,7)=avoidStartTheta1;
agent1(1,8)=avoidEndTheta1;
agent1(1,9)=firstDistance1;
agent1(1,10)=secondDistance1;
agent1(1,11)=avoidDistanceLeft1;

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

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

d=[];
d(1) = 0;

rect2x = -6;
rect2y = -5;
rect2w = 2;
rect2h = 1;
rect2 = [rect2x rect2y rect2w rect2h];

rectangles = [rect1; rect2];
numberOfRectangles = 2;

for t=2:dt:1300
    agent1 = IRControl(t, dt, agent1, rectangles, numberOfRectangles);
    coords(1, :) = [agent1(t,1), agent1(t,2), agent1(t,4)];
    
%     [distance, pointx, pointy] = IRSensorAll(rectangles, numberOfRectangles,...
%         agent1(t-1,1), agent1(t-1,2), agent1(t-1,4));
    
    plot(agent1(:,1),agent1(:,2), agent1(t,1), agent1(t,2), 'b+')
    hold on;
    for i=1:numberOfRectangles
        rectangle('Position', rectangles(i,:));
    end
        
    hold off;
    axis([xAxisMin, xAxisMax, yAxisMin, yAxisMax]);
    M(t)= getframe;
end