%Implement robots as rectangles w/ back, front repulsive force from other
%paper. Use k constants
%Make sure cars can only see forward (simulate IR sensor)
% Bound angle by 18, use control laws to simulate turning
K = 3; %number of robots
Qr = -1000; %robot repulsion
Br = -3000; %Barrier repulsion
targetx = 6;
targety = -1;
C = 12700;
w = 2; %barrier detection length
%robots (i,j,k) denotes robot i at time j w/ characteristic k
% k =1:2 are (x,y) position
% k =3 is angle
%k=4 determines if it has reached target (0 for no, 1 for yes)
v = 0.02; %velocity is constant for robots
maxtime = 1000; %time limit
robots = zeros(K,maxtime,4);
robots (1,1,2) = 0;
robots(2,1,2) = 1;
robots(3,1,2) = 2;
dt = 1; %time iteration length
eps = 0.1; %error bound for determining final pt
barrier = [4, 2; 4,0;4, -2; 0, 5; 2, 5; 4,5];
L = 3; %number of barriers
plot(robots(1,1,1), robots(1,1,2), 'b+',...
        robots(2,1,1),robots(2,1,2), 'r+',...
        robots(3,1,1),robots(3,1,2), 'g+',...
        targetx, targety, 'ro', barrier(:, 1), barrier(:, 2), 'rs')

for t = 2:dt:maxtime
    term2 = 0;
    term3 = 0;
    for i =1:K
        term1 = [targetx - robots(i,t-1,1), targety - robots(i,t-1,2)];
        term1 = -C*(term1 / (norm(term1)^2));
        for j = 1:K
            if j ~= i && robots(j,t-1,4) == 0
                r = [robots(j,t-1,1) - robots(i,t-1,1),robots(j,t-1,2) - robots(i,t-1,2)];
                r = -Qr*(r/(norm(r)^2));
                term2 = term2 + r;
            end
        end
        for j =1:L
            b = [barrier(j,1) - robots(i,t-1,1),barrier(j,2) - robots(i,t-1,2)];
            if norm(b) <= w
                b = -Br*(b/(norm(b)^2));
                b = [-b(2), b(1)];
                if ((b(1) - term1(1))^2 + (b(2) - term1(2))^2 < ((-b(1) - term1(1))^2 + (-b(2) - term1(2))^2))
                    term3 = term3 + b;
                else
                    term3 = term3 - b;
                end
            end
        end
        F(i, 1:2) = Qr*(term1 + term2 +term3);
        if (abs(robots(i,t-1,1)-targetx) > eps || abs(robots(i,t-1,2)-targety) > eps)
            robots(i,t,1) = robots(i,t-1,1) + v*dt*cos(robots(i,t-1,3));
            robots(i,t,2) = robots(i,t-1,2) + v*dt*sin(robots(i,t-1,3));
            robots(i,t,3) = atan(F(i,2)/F(i,1));
            if targetx <= robots(i,t,1)
                robots(i,t,3) = robots(i,t,3) + pi;
            end
        else
            robots(i,t,1) = robots(i,t-1,1);
            robots(i,t,2) = robots(i,t-1,2);
            robots(i,t,3) = robots(i,t-1,3);
            robots(i,t,4) = 1;
        end
    end
    %adjust angles of still moving robots if barrier is in front and how
    %far away barrier is (assume cars can communicate all this info)
    plot(robots(1,:,1),robots(1,:,2), robots(1,t,1), robots(1,t,2), 'b+',...
        robots(2,:,1),robots(2,:,2), 'r',robots(2,t,1),robots(2,t,2), 'r+',...
        robots(3,:,1),robots(3,:,2), 'g',robots(3,t,1),robots(3,t,2), 'g+',...
        targetx, targety, 'ro', barrier(:, 1), barrier(:, 2), 'rs')
    rectangle('Position',[0,0,5,5])
    axis([-10, 10, -10, 10]);
    M(t)= getframe;
end