%Grid nodes indexed left to right from top row to bottom row. Only adjacent
%nodes are connected (top, bottom,left,right)
rows = 10;
columns = 10;
numberrobots = 4;
goals = [12, 22,50,100];
start = [1, 2,3,4];
nodes = rows*columns;
G = zeros(nodes);
xy = zeros(nodes, 2);
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
    if (i > columns)
        G(i, i-columns) = 1;
        G(i-columns, i) = 1;
    end
    if (mod(i,columns) ~= 0 && i > columns)
        G(i, i - columns + 1) = 1;
        G(i - columns + 1, i) = 1;
    end
    if (mod(i,columns) ~= 1 && i > columns)
        G(i, i - columns - 1) = 1;
        G(i - columns - 1, i) = 1;
    end
    if (mod(i,columns) ~= 1)
        G(i, i-1) = 1;
        G(i-1,i) = 1;
    end
end
%Obstacle simulation
for i=1:nodes
    if (rand(1) < 0.2 && i ~= goals(1) && i ~= goals(2) && i ~= goals(3) && i ~= goals(4) && i ~= start(1) && i ~= start(2) && i ~= start(3) && i ~= start(4))
        G(i,1:end) = 0;
        G(1:end, i) = 0;
    end
end
%gplot(G(1:nodes,1:nodes), xy(1:nodes,:), '-ob');
plot(xy(1:nodes,1),xy(1:nodes,2), 'ob');
hold on;
[cost, path]=dijikstra(G,xy,start,goals);
for i = 1:numberrobots
    path1 = path(i,i);
    path1 = cell2mat(path1);
    if (i==1)
        gplot(G(path1(1:end),path1(1:end)), xy(path1(1:end),:), '-or');
    elseif (i==2)
        gplot(G(path1(1:end),path1(1:end)), xy(path1(1:end),:), '-xg');
    elseif (i==3)
        gplot(G(path1(1:end),path1(1:end)), xy(path1(1:end),:), '-xm');
    else
        gplot(G(path1(1:end),path1(1:end)), xy(path1(1:end),:), '-xy');
    end
    hold on;
end