function [new_agent, lastDistanceRight] = ...
    MultipleIRControlRight( t, dt, agent, coords, rectangles, ...
    numberOfRectangles, target, lastDistanceRight)
%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
    distanceRightWithoutNoise = IRSensorRight45All(coords, rectangles, numberOfRectangles,...
        x, y, theta1);
    
    %see if in front
    car1x = coords(1,1);
    car1y = coords(1,2);
    car2x = coords(2,1);
    car2y = coords(2,2);
    distanceToCar1 = sqrt((car1x - x)^2 + (car1y - y)^2);
    distanceToCar2 = sqrt((car2x - x)^2 + (car2y - y)^2);
    thetaToCar2 = atan((car2y-y)/(car2x-x)) - theta1;
    if (car2x - x) < 0
        thetaToCar2 = thetaToCar2 + pi;
    end
    thetaToCar2 = mod(thetaToCar2, 2*pi);
    thetaToCar1 = atan((car1y-y)/(car1x-x)) - theta1;
    if (car1x - x) < 0
        thetaToCar1 = thetaToCar1 + pi;
    end
    thetaToCar1 = mod(thetaToCar1, 2*pi);
    
    %see if car1 is right
    if sin(thetaToCar1) < 0
        car1isRight = 1;
    else
        car1isRight = 0;
    end   
    
%     %see if car is in front:
%     if (cos(thetaToCar1) < 0) && (cos(thetaToCar2) < 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);
    distanceRight = distanceRightWithoutNoise;
    
    
    if (thetaToTarget < pi/4 - pi/10 || thetaToTarget > 7*pi/4 + pi/10)
        leader = 2;
    elseif (thetaToTarget < pi)
        leader = 1;
    else
        leader = 3;
    end
    
    %if leader is behind, stop
    if (leader == 1 && cos(thetaToCar1)<0 && coords(1, 3) > 0)
        agent(t, 3) = 0;
    elseif (leader == 2 && cos(thetaToCar2)<0 && coords(2, 3) > 0)
        agent(t, 3) = 0;
    end
    
    %if something is in the way right, and it's getting closer, turn away
    %(left)
    if ((distanceRight >= 0 && distanceRight<=ignoreDistance) && ...
            (distanceRight < lastDistanceRight))
        curvature = uMax;
      
    %if right is leader (and nothing on the right is getting closer), swarm
    %with cars toward target
    elseif (leader == 3)
        curvature = 10*Ujk(1,3,coords) + 10*Ujk(2,3,coords);
        curvature = curvature + targetCoeff*sin(thetaToTarget);
        
    %if close to anyone, move away
    elseif (distanceToCar1 < .3 || distanceToCar2 < .3)
        curvature = leaderCoeff*Ujk(1,3,coords) + leaderCoeff*Ujk(2,3,coords);
         
%     %if out of formation with car1 right, go right
%     elseif (car1isRight ==1)
%         curvature = -uMax;
        
    %if straight is leader, swarm with 2 as leader
    elseif (leader == 2 )
        curvature = leaderCoeff*Ujk(2,3,coords) + Ujk(1,3,coords);

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

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

    new_agent = agent;
end