rows = 10;
columns = 10;
nodes = rows*columns;
xy = zeros(nodes, 2);
Visited = zeros(nodes);
n=0;
maxangle = 18*(pi/180);
v = 0.02; %velocity is constant for robots
K = 3; %number of robots
Qr = -100; %robot repulsion
C = 127;
Vmax = 100;
maxtime = 1000;
dt=1;
robots = zeros(K,maxtime,4);
update = ones(K,nodes);
robots (1,1,2) = 5;
 robots(2,1,2) = 1;
 robots(3,1,2) = 8;
for i = 1:nodes
    if (i==1)
        xy(i,1) = 1;
        xy(i,2) = 1;
    else
        if (xy(i-1, 1) ~= columns)
            xy(i,1) = xy(i-1) + 1;
            xy(i,2) = xy(i-1,2);
        else
            xy(i,1) = 1;
            xy(i,2) = xy(i-1,2) + 1;
        end
    end
end
for t = 2:dt:maxtime
    for i=1:K
        term1 = 0;
        term2 = 0;
        for j = 1:nodes
            if (xy(j,1) <= robots(i,t-1,1) && robots(i,t-1,1) < xy(j,1) + 1) && (xy(j,2) <= robots(i,t-1,2) && robots(i,t-1,2) < xy(j,2) + 1)
                if (update(i,j) == 1)
                    Visited(j) = Visited(j) + 1;
                    update(i,j) = 0;
                end
            else
                update(i,j) = 1;
            end
            cell = [xy(j,1) - robots(i,t-1,1), xy(j,2) - robots(i,t-1,2)];
            term1 = term1 - q(Visited(j),n,C,Vmax)*(cell/(norm(cell)^2));
        end
        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
        F(i, 1:2) = Qr*(term1 + term2);
        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
            robots(i,t,3) = atan(F(i,2)/F(i,1)); 
        elseif F(i,2) >= 0
            robots(i,t,3) = atan(F(i,2)/F(i,1)) + pi;
        else
            robots(i,t,3) = atan(F(i,2)/F(i,1)) - pi;
        end
    end
    ind = find(Visited > n);
    if (size(ind) >= 0.95*nodes)
        n = n +1;
    end
    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+',...
        xy(1:nodes,1), xy(1:nodes,2), 'or')
    grid on;
         
         
        
 
        
    axis([0, 11, 0, 11]);
    M(t)= getframe;
    
end