%Implement robots as rectangles
%Make sure cars can only see forward (simulate IR sensor).
% Bound angle by 18, use control laws to simulate turning
width = 1;
height = 3;
K = 3; %number of robots
Qr = -150; %robot repulsion
Br = -2000; %Barrier repulsion
target(1,1) = 6;
target(1,2) = 0;
target(2,1) = 6;
target(2,2) = 1;
target(3,1) = 6;
target(3,2) = 2;
C = 1000;
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.04; %velocity is constant for robots
maxtime = 600; %time limit
maxangle = 2*(pi/180);
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, 3; 4, 2; 4,1; 4,0; 4,-1; 4, -2];
 barrier = [...
    4, 1.00; 4, -1.0; 4, 2.0; 4, -2.0; 4, 3.0; 4, -3.0; 4, 4.0; 4, -4.0;...
    4, 1.10; 4, -1.1; 4, 2.1; 4, -2.1; 4, 3.1; 4, -3.1; 4, 4.1; 4, -4.1;...
    4, 1.20; 4, -1.2; 4, 2.2; 4, -2.2; 4, 3.2; 4, -3.2; 4, 4.2; 4, -4.2;...
    4, 1.30; 4, -1.3; 4, 2.3; 4, -2.3; 4, 3.3; 4, -3.3; 4, 4.3; 4, -4.3;...
    4, 1.40; 4, -1.4; 4, 2.4; 4, -2.4; 4, 3.4; 4, -3.4; 4, 4.4; 4, -4.4;...
    4, 1.50; 4, -1.5; 4, 2.5; 4, -2.5; 4, 3.5; 4, -3.5; 4, 4.5; 4, -4.5;...
    4, 1.60; 4, -1.6; 4, 2.6; 4, -2.6; 4, 3.6; 4, -3.6; 4, 4.6; 4, -4.6;...
    4, 1.70; 4, -1.7; 4, 2.7; 4, -2.7; 4, 3.7; 4, -3.7; 4, 4.7; 4, -4.7;...
    4, 1.80; 4, -1.8; 4, 2.8; 4, -2.8; 4, 3.8; 4, -3.8; 4, 4.8; 4, -4.8;...
    4, 1.90; 4, -1.9; 4, 2.9; 4, -2.9; 4, 3.9; 4, -3.9; 4, 4.9; 4, -4.9;...
    4, 0.0; 4, 0.1; 4, 0.2; 4, 0.3; 4, 0.4; 4, 0.5; 4, 0.6; 4, 0.7; 4, 0.8; 4, 0.9;...
    4, -0.1; 4, -0.2; 4, -0.3; 4, -0.4; 4, -0.5; 4, -0.6; 4, -0.7; 4, -0.8; 4, -0.9;...
    4, 5.0; 4, 5.1; 4, 5.2; 4, 5.3; 4, 5.4; 4, 5.5; 4, 5.6; 4, 5.7; 4, 5.8; 4, 5.9;...
    4, -5.0; 4, -5.1; 4, -5.2; 4, -5.3; 4, -5.4; 4, -5.5; 4, -5.6; 4, -5.7; 4, -5.8; 4, -5.9;...
    ];
L = size(barrier,1); %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+',... 
    target(1:K,1), target(1:K,2), 'ro', barrier(:, 1), barrier(:, 2), 'rs')


for t = 2:dt:maxtime
    for i =1:K
        term1 = [0,0];
        term2 = [0,0];
        term3 = [0,0];
        term1 = [target(i,1) - robots(i,t-1,1), target(i,2) - robots(i,t-1,2)];
        targvec = term1;
        term1 = -C*(term1 / (norm(term1)^2));
        for j = 1:K
            if j ~= i
                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)];
            vec = [w*cos(robots(i,t-1,3)),w*sin(robots(i,t-1,3))];
            if norm(b) <= w && dot(vec,b) >= 0
                b = -Br*(b/(norm(b)^2)); %exponential dependence
                b = [-b(2), b(1)];
                term3 = term3 + b;
                 %Figure out way for cars to split their way around bdy,
                 %currently they favor going right instead of left, path of
                 %least resistance
            end
        end
        right = 0;
        left = 0;
        for j=1:K
            if j ~= i
                robvec = [robots(j,t-1,1) - robots(i,t-1,1), robots(j,t-1,2) - robots(i,t-1,2),0];
                destvec = [target(i,1) - robots(i,t-1,1), target(i,2) - robots(i,t-1,2), 0];
                if cross(robvec, destvec) >= 0
                    left = left + 1;
                else
                    right = right + 1;
                end
            end
        end
        if left > right
            term3 = -term3;
        end
        F(i, 1:2) = Qr*(term1 + term2 +term3);
        if (abs(robots(i,t-1,1)-target(i,1)) > eps || abs(robots(i,t-1,2)-target(i,2)) > 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));
            if F(i,1) >= 0
                  placehold = atan(F(i,2)/F(i,1)); 
            elseif F(i,2) >= 0
                  placehold = atan(F(i,2)/F(i,1)) + pi;
            else
                  placehold = atan(F(i,2)/F(i,1)) - pi;
            end
            if (placehold <= -pi/2 && robots(i,t-1,3) >= pi/2)
                placehold = placehold + 2*pi;
            end
            if (robots(i,t-1,3) <= -pi/2 && placehold >= pi/2)
                placehold = placehold - 2*pi;
            end
             if placehold - robots(i,t-1,3) > maxangle
                 robots(i,t,3) = robots(i,t-1,3) + maxangle;
             elseif robots(i,t-1,3) - placehold > maxangle
                 robots(i,t,3) = robots(i,t-1,3) - maxangle;
             else
                 robots(i,t,3) = placehold;
             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);
        end
    end
    plot(robots(1,1:t-1,1),robots(1,1:t-1,2), robots(1,t,1), robots(1,t,2), 'b+',...
        robots(2,1:t-1,1),robots(2,1:t-1,2), 'r',robots(2,t,1),robots(2,t,2), 'r+',...
        robots(3,1:t-1,1),robots(3,1:t-1,2), 'g',robots(3,t,1),robots(3,t,2), 'g+',...
        target(:,1), target(:,2), 'ro', barrier(:, 1), barrier(:, 2), 'rs')
 
        
 
        
    axis([-1, 8, -9, 9]);
    M(t-1) = getframe;
end