function [new_agent, keepTurning, lastDistanceStraight, lastDistanceRight,...
    lastDistanceLeft] = TestControl( t, dt, agent, rectangles, numberOfRectangles, ...
    target, keepTurning, lastDistanceStraight, lastDistanceRight, lastDistanceLeft)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
    uMax = 1;
    ignoreDistance = 3*uMax;
    
    %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) = v;
	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.1 && abs(y - targety) < 0.1)
        agent(t, 3) = 0;
    end
        
    %find distances to barriers, straight, right and left
    distanceStraight = IRSensorAll(rectangles, numberOfRectangles,...
        agent(t-1,1), agent(t-1,2), agent(t-1,4));
    distanceRight = IRSensorRightAll(rectangles, numberOfRectangles,...
        agent(t-1,1), agent(t-1,2), agent(t-1,4));
    distanceLeft = IRSensorLeftAll(rectangles, numberOfRectangles,...
        agent(t-1,1), agent(t-1,2), agent(t-1,4));
    
    %if keep turning is on, see if should be turned off
    if (keepTurning == 1 && (distanceRight > 0 && distanceRight <= ignoreDistance)&&...
        (distanceStraight < 0 || distanceStraight>ignoreDistance))
        keepTurning = 0;
    end
    if (keepTurning == -1 && (distanceLeft > 0 && distanceLeft <= ignoreDistance) &&...
        (distanceStraight < 0 || distanceStraight>ignoreDistance))
        keepTurning = 0;
    end
     
    
    %CALCULATING CURVATURE:
    %if keep turning is on, keep turning with curvature as before
    if keepTurning ~= 0
        agent(t, 5) = agent(t-1, 5);
        
    %if very close on either side, turn (supercedes all other control)
    elseif (distanceLeft > 0 && distanceLeft < 1/(2*uMax))
        agent(t, 5) = -uMax;
        
    elseif (distanceRight > 0 && distanceRight < 1/(2*uMax))
        agent(t, 5) = uMax;
        
    %with keep turning off:
    %if target is left by less than 45 degrees (wnat to go +uMax)
    elseif thetaToTarget < pi/4
        %if nothing is in the way in front...
        if (distanceStraight < 0 || distanceStraight>ignoreDistance)
            %go toward target (left)
            agent(t, 5) = uMax;
%             %go toward target (left), unless something is very close on
%             %the left, in which case, curve away (right)
%             if (distanceLeft > 0 && distanceLeft < 1/(2*uMax))
%                 agent(t, 5) = -uMax;
%             else
%                 agent(t, 5) = uMax;
%             end
        %if something is in front...
        else
            %if there's nothing to the left (where car wants to go), turn
            %left and keep going until can go straight:
            if (distanceLeft < 0 || distanceLeft>ignoreDistance)
                agent(t, 5) = uMax;
                keepTurning = 1;
            %if there's something on the left but nothing on the right,
            %turn right and keep going until can go straight:
            elseif (distanceRight < 0 || distanceRight>ignoreDistance)
                agent(t, 5) = -uMax;
                keepTurning = -1;
%             elseif distanceLeft > lastDistanceLeft
%                 agent(t, 5) = uMax;
%                 keepTurning = 1;
%             else
%                 agent(t, 5) = -uMax;
%                 x
%                 keepTurning = -1;
%             end
            %if there's something on all sides (and not too near either side)...
            else
                %turn and keep turning
                agent(t, 5) = uMax;
%                 keepTurning = 1;
%                 %turn away from whatever side is closest, favoring left
%                 if distanceRight <= distanceLeft
%                     agent(t, 5) = uMax;
% %                     keepTurning = 1;
%                 else
%                     agent(t, 5) = -uMax;
%                 end
            end
        end
    
	%if target is below by less than 45 degrees (wnat to go -uMax)
    elseif thetaToTarget > 7*pi/4
        %if nothing is in the way in front...
        if (distanceStraight < 0 || distanceStraight>ignoreDistance)
            %go toward target (right)
            agent(t, 5) = -uMax;
%             %go toward target (right), unless something is very close on
%             %the right, in which case, curve away (left)
%             if (distanceRight > 0 && distanceRight < 1/(2*uMax))
%                 agent(t, 5) = uMax;
%             else
%                 agent(t, 5) = -uMax;
%             end
        else
            if (distanceRight < 0 || distanceRight>ignoreDistance)
                agent(t, 5) = -uMax;
                keepTurning = -1;
            elseif (distanceLeft < 0 || distanceLeft>ignoreDistance)
                agent(t, 5) = uMax;
                keepTurning = 1;
%             elseif distanceRight > lastDistanceRight
%                 agent(t, 5) = -uMax;
%                 keepTurning = -1;
%             else
%                 agent(t, 5) = uMax;
%                 keepTurning = 1;
%             end
            else
                %turn and keep turning
                agent(t, 5) = -uMax;
%                 keepTurning = -1;
%                  %turn away from whatever side is closest, favoring right
%                 if distanceLeft <= distanceRight
%                     agent(t, 5) = -uMax;
% %                     keepTurning = 1;
%                 else
%                     agent(t, 5) = uMax;
%                 end               
            end
        end
        
    elseif thetaToTarget < pi
        if (distanceLeft < 0 || distanceLeft>ignoreDistance)
            agent(t, 5) = uMax;
        else
            if (distanceStraight < 0 || distanceStraight>ignoreDistance)
                if distanceLeft < lastDistanceLeft
                    agent(t, 5) = -uMax;
%                     keepTurning = -1;
                elseif distanceLeft > lastDistanceLeft
                    agent(t, 5) = uMax;
%                     keepTurning = 1;
                else
                    agent(t, 5) = uMax;
                end 
            elseif (distanceRight < 0 || distanceRight>ignoreDistance)
                agent(t, 5) = -uMax;
%             elseif distanceRight > lastDistanceRight
%                 agent(t, 5) = -uMax;
%             elseif distanceLeft < lastDistanceLeft
%                 agent(t, 5) = uMax;
            else
                agent(t, 5) = uMax;
            end
        end
        
    else
        if (distanceRight < 0 || distanceRight>ignoreDistance)
            agent(t, 5) = -uMax;
        else
            if (distanceStraight < 0 || distanceStraight>ignoreDistance)
                if distanceRight < lastDistanceRight
                    agent(t, 5) = uMax;
                    test = 1
                    x
%                     keepTurning = 1;
                elseif distanceRight > lastDistanceRight
                    agent(t, 5) = -uMax;
                    test = -1
                    x
%                     keepTurning = -1;
                else
                    agent(t, 5) = -uMax;
                end 
            elseif (distanceLeft < 0 || distanceLeft>ignoreDistance)
                agent(t, 5) = uMax;
%             elseif distanceLeft > lastDistanceLeft
%                 agent(t, 5) = uMax;
%             elseif distanceRight < lastDistanceRight
%                 agent(t, 5) = -uMax;
            else
                agent(t, 5) = -uMax;
            end
        end
    end
    
%     if (keepTurning~=0 && x > -2 )
%         keepTurning
%         x
%     end
        
        
    lastDistanceLeft = distanceLeft;
    lastDistanceRight = distanceRight;
    lastDistanceStraight = distanceStraight;

    new_agent = agent;
end