function [new_agent, keepTurning, lastTurning1, lastTurning2, lastDistanceStraight] = ...
    MultipleIRControlStraight( t, dt, agent, coords, rectangles, ...
    numberOfRectangles, target, keepTurning, lastTurning1, lastTurning2)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
    uMax = 1;
    ignoreDistance = 2/uMax;
    noise = 6;
    leaderCoeff = 100000;
    targetCoeff = 50;
    
    %get agent's infor
    x = agent(t-1, 1);
    y = agent(t-1, 2);
    v = agent(t-1, 3);
    theta1 = agent(t-1, 4);  
    u = agent(t-1, 5);  
    
    %calculate new info except curvature
    theta2 = theta1 + u*v*dt;
    agent(t, 1) = x + v*dt*cos(theta1);
    agent(t, 2) = y + v*dt*sin(theta1);
	agent(t, 3) = agent(1, 3);
	agent(t, 4) = theta2;
    
    %calculate theta to target
    targetx = target(1);
    targety = target(2);
    thetaToTarget = atan((targety-y)/(targetx-x)) - theta1;
    if (targetx - x) < 0
        thetaToTarget = thetaToTarget + pi;
    end
    thetaToTarget = mod(thetaToTarget, 2*pi);
    %stop if very near target
    if (abs(x - targetx) < 0.3 && abs(y - targety) < 0.3)
        agent(t, 3) = 0;
    end
        
    %find distances to barriers, straight, right and left
    distanceStraightWithoutNoise = IRSensorAll(coords, rectangles, numberOfRectangles, ...
        x, y, theta1);
    
    %find if car1 is right and if car3 is left
    car1x = coords(1,1);
    car1y = coords(1,2);
    car3x = coords(3,1);
    car3y = coords(3,2);
    distanceToCar1 = sqrt((car1x - x)^2 + (car1y - y)^2);
    distanceToCar3 = sqrt((car3x - x)^2 + (car3y - y)^2);
    thetaToCar1 = atan((car1y-y)/(car1x-x)) - theta1;
    if (car1x - x) < 0
        thetaToCar1 = thetaToCar1 + pi;
    end
    thetaToCar1 = mod(thetaToCar1, 2*pi);
    thetaToCar3 = atan((car3y-y)/(car3x-x)) - theta1;
    if (car3x - x) < 0
        thetaToCar3 = thetaToCar3 + pi;
    end
    thetaToCar3 = mod(thetaToCar3, 2*pi);
   
    if sin(thetaToCar1) < 0
        car1isRight = 1;
    else
        car1isRight = 0;
    end
    if sin(thetaToCar3) > 0
        car3isLeft = 1;
    else
        car3isLeft = 0;
    end     
    
%     %see if car is in front:
%     if (cos(thetaToCar1) < 0) && (cos(thetaToCar3) < 0)
%         agent(t, 3) = agent(1,3)/2;
%     else
%         agent(t, 3) = agent(1,3);
%     end
    
    %find distances, taking into account noise
%     readingStraight = (7600/distanceStraightWithoutNoise) -noise/2 + noise*rand();
%     distanceStraight = (7600/readingStraight);
%     readingLeft = (7600/distanceLeftWithoutNoise) - noise/2 + noise*rand();
%     distanceLeft = (7600/readingLeft);
%     readingRight = (7600/distanceRightWithoutNoise) - noise/2 + noise*rand();
%     distanceRight = (7600/readingRight);
    distanceStraight = distanceStraightWithoutNoise;
    
    
    if (thetaToTarget < pi/4|| thetaToTarget > 7*pi/4 )
        leader = 2;
    elseif (thetaToTarget < pi)
        leader = 1;
    else
        leader = 3;
    end
    
    %if leader is behind and leader is moving, stop
    if (leader == 1 && cos(thetaToCar1)<0 && coords(1, 4) > 0)
        agent(t, 3) = 0;
    elseif (leader == 3 && cos(thetaToCar3)<0 && coords(3, 4) > 0)
        agent(t, 3) = 0;
    end
        
    %if keep turning is on, see if should be turned off
    if (keepTurning == 1 && (distanceStraight < 0 || distanceStraight>ignoreDistance))
        keepTurning = 0;
        lastTurning1 = 1;
        lastTurning2 = 1;
    end
    if (keepTurning == -1 && (distanceStraight < 0 || distanceStraight>ignoreDistance))
        keepTurning = 0;
        lastTurning1 = -1;
        lastTurning2 = -1;
    end
    
    %if ever facing target, reset lastTurning1 to zero
    if (thetaToTarget < pi/20|| thetaToTarget > 39*pi/20 )
        lastTurning1 = 0;
    end
   
    
    %if keep turning is on, keep turning with curvature as before
    if keepTurning ~= 0
        curvature = u;      
        
    %if straight is leader and something in the way, turn toward target and keep
    %turning
    elseif (leader == 2 && (distanceStraight >= 0 && distanceStraight<=ignoreDistance))
        if lastTurning1 == 0
            if (thetaToTarget < pi)
                sign = -1;  %MAYBE
            else
                sign = 1;
            end
        else
            sign = lastTurning1;
        end
        curvature = sign*uMax;
        keepTurning = sign;
        
    %if close to anyone, move away
    elseif (distanceToCar1 < .3 || distanceToCar3 < .3)
        curvature = leaderCoeff*Ujk(1,2,coords) + leaderCoeff*Ujk(3,2,coords);
        
    %if straight is leader and nothing in way...
    elseif (leader == 2 && (distanceStraight < 0 || distanceStraight>ignoreDistance))
        %if just turning, swarm with guy who is turning
        if lastTurning2 == 1
            curvature = leaderCoeff*Ujk(3,2,coords);
            
        elseif lastTurning2 == -1
            curvature = leaderCoeff*Ujk(1,2,coords);
        
        %otherwise, swarm toward target:
        else
            curvature = Ujk(1,2,coords) + Ujk(3,2,coords);
            curvature = curvature + targetCoeff*sin(thetaToTarget);
        end
    
%     %if nothing is in the way and out of left, straight, right orientation,
%     %reorient:
%     %if car1 and 2 both out of order, curve away from target
%     elseif (car1isRight == 1 && car3isLeft == 1)
%         if (thetaToTarget < pi)
%             curvature = -uMax;
%         else
%             curvature = uMax;
%         end
%         
%     %if car1 is right, curve right
%     elseif (car1isRight ==1)
%         curvature = -uMax;
        
    %if car3 is left, curve left
    elseif (car3isLeft ==1)
        curvature = uMax;
        
    %if left is leader, swarm with left as leader
    elseif (leader == 1)
        curvature = leaderCoeff*Ujk(1,2,coords) + Ujk(3,2,coords);
        lastTurning2 = 0;

    %if right is leader, swarm with right as leader
    elseif (leader == 3)
        curvature = Ujk(1,2,coords) + leaderCoeff*Ujk(3,2,coords);
        lastTurning2 = 0;
    else
        curvature = 0;
        error = 1
    end

    if curvature > uMax
        curvature = uMax;
    elseif curvature < -uMax;
        curvature = -uMax;
    end
    agent(t,5) = curvature; 
    
    lastDistanceStraight = distanceStraight;

    new_agent = agent;
end