function [ output_args ] = GPS(T)
%GPS Summary of this function goes here
%   Detailed explanation goes here
if(nargin==0),
    T = 250;
end;
window = 100; %Sliding window size
maxY = 50;    %Upper limit of Y axis
Queues =[[8, -1];[4, 10]];     %Initial Queues
QueueSizes = [sum(Queues(Queues(1,:)>0),2);sum(Queues(Queues(2,:)>0),2)];

subplot(2,1,1);  %Parameters are: "number of plots to fit vertically, number of plots to fit horizontally, index of current plot"
                 % 2, 1, 1 : 2 figures one on top of the other (2 lines, 1 column), plot next on the 1st figure.
title('GPS');
ylabel('Queue size');
xlabel('Time');
h1 = animatedline('Color','red'); %handles for animated line for figure 1.
h2 = animatedline('Color','blue'); 
legend('Transmitter 1', 'Transmitter 2', 'Location', 'northwest');
axis([0 window -1 maxY]) %define axis limits
axis1 = gca;
subplot(2,1,2);   %See above
title('PGPS');
ylabel('Queue size');
xlabel('Time');
g1 = animatedline('Color','red'); %handle for animated line for figure 2.
g2 = animatedline('Color','blue');
legend('Transmitter 1', 'Transmitter 2', 'Location', 'northwest');
axis([0 window -1 maxY]) %define axis limits
axis2 = gca;


step = 1; %WARNING : must be of form 1/Integer
x = 0:step:T ; %point on X axis


nextUpdatePGPS = 0;

PGPS_Schedule = [];

for k = 1:length(x)
    
    if(k*step>window) %Sliding window for viewing
        if(ishandle(axis1) && ishandle(axis2)),
            xlim(axis1,[k*step-window k*step]);
            xlim(axis2,[k*step-window k*step]);
        else return;
        end;
        
    end;
    
    if(mod(k,1/step) == 0),
        [QueueSizes, Queues] = updateQueues(Queues, QueueSizes);
    end;
    
    if(QueueSizes(1,1) <= 0)
        addpoints(h1,x(k),QueueSizes(1));
        if(QueueSizes(2) > 0)
            QueueSizes(2) = QueueSizes(2)-step;
            addpoints(h2, x(k), QueueSizes(2));
        else
            addpoints(h2, x(k), 0);
        end;
    else
        if(QueueSizes(2) > 0),
            QueueSizes(2) = QueueSizes(2)-step/2;
            QueueSizes(1) = QueueSizes(1)-step/2;
            addpoints(h1, x(k), QueueSizes(1));
            addpoints(h2, x(k), QueueSizes(2));
        else
            QueueSizes(1,1) = QueueSizes(1)-step;
            addpoints(h1, x(k), QueueSizes(1));
            addpoints(h2, x(k), QueueSizes(2));
        end;
    end;
    
    if(nextUpdatePGPS <= 0)
        PGPS_Schedule = updatePGPS(Queues, step);
        nextUpdatePGPS = sum(sum(Queues(Queues>0)));
        
    end;
        
    Queues(1,1) = Queues(1,1) - PGPS_Schedule(1,1)*step;
    Queues(2,1) = Queues(2,1) - PGPS_Schedule(2,1)*step;
    
    PGPS_Schedule = PGPS_Schedule(:,2:end);
    
    
    aux = Queues(1,:);
    addpoints(g1, x(k), sum(aux(aux>0)));
    aux = Queues(2,:);
    addpoints(g2, x(k), sum(aux(aux>0)));
    
    if(Queues(1,1)==0)
        Queues(1,:) = [Queues(1,2:end), -1];
    end;
    if(Queues(2,1)==0)
        Queues(2,:) = [Queues(2,2:end),-1];
    end;
    if(size(Queues,2)>1 && Queues(1, end) == Queues(2,end) && Queues(1,end) == -1)
        Queues = Queues(:, 1:end-1);
    end;
    nextUpdatePGPS = nextUpdatePGPS - step;
    
    drawnow
end

end

function [UpdatedQueueSizes, UpdatedQueues] = updateQueues(Queues, QueueSizes)
packet1 = generatePacket(Queues, 1);
packet2 = generatePacket(Queues, 2);

if(packet1>0)
    found1 = 0;
    for j=1:size(Queues,2),
        if(Queues(1,j) == -1),
            Queues(1,j) = packet1;
            found1 = 1;
            break;
        end;
    end;
    if(found1 == 0)
        Queues = [Queues, [packet1; -1]];
    end;
end;

if(packet2>0)
    found2 = 0;
    for j=1:size(Queues,2),
        if(Queues(2,j) == -1),
            Queues(2,j) = packet2;
            found2 = 1;
            break;
        end;
    end;
    if(found2 == 0)
        Queues = [Queues, [-1; packet2]];
    end;
end;
UpdatedQueueSizes = QueueSizes+[packet1; packet2];
UpdatedQueues = Queues;
end

function [packet] = generatePacket(Queues, i)
    packet = 0;
    rnd = rand(1,1);
    if rnd > 0.925,
        packet = floor(rand(1,1)*10+1);
    end;
end

function [PGPS_Updated] = updatePGPS(Queues1, step)
Queues = Queues1;
L = sum(sum(Queues(Queues>0)))/step;

PGPS_Updated = zeros(2,L);
if(L==0)
    PGPS_Updated = zeros(2,1);
    return;
end;


Queues2 = Queues1;

order = [];
for i=1:L,
    if(Queues(1,1) <= 0 && Queues(2,1)<=0)
        Queues1
        order
        Queues
    end;
    speed2 = step;
    if(Queues(1,1) <= 0)
        speed1 = 0;
    else
        if(Queues(2,1)<=0),
            speed1 = step;
            speed2 = 0;
        else
            speed2 = step/2;
            speed1 = step/2;
        end;
    end;
    if(speed1+speed2>step)
        [speed1 , speed2]
    end;
    Queues(1,1) = Queues(1,1)-speed1;
    Queues(2,1) = Queues(2,1)-speed2;
    if(Queues(1,1) <= 0 && speed1>0)
        order = [order, 1];
        Queues(1,:) = [Queues(1,2:end), -1];
    end;
    if(Queues(2,1) <= 0 && speed2>0)
        order = [order, 2];
        Queues(2,:) = [Queues(2,2:end), -1];
    end;
end;

currentSender = order(1);
order = order(2:end);

for i=1:L,
    if(currentSender == 1)
        PGPS_Updated(:,i) = [1,0];
        Queues2(1,1) = Queues2(1,1)-step;
        if(Queues2(1,1) == 0)
            Queues2(1,:) = [Queues2(1, 2:end),-1];
            if(length(order)==0)
                return;
            end;
            currentSender = order(1);
            order = order(2:end);
        end;
    else
        PGPS_Updated(:,i) = [0,1];
        Queues2(2,1) = Queues2(2,1)-step;
        if(Queues2(2,1) == 0)
            Queues2(2,:) = [Queues2(2, 2:end),-1];
            if(length(order)==0)
                return;
            end;
            currentSender = order(1);
            order = order(2:end);
        end;
    end;           
end;

end
