classdef queue < handle
  
% q = queue;
% This function creates a FIFO queue data structure.

 
  
  
  properties(SetAccess = private)
    queueData = nan;
    readptr = 1;
    writeptr = 1;
  end
  methods(Access = public)
    function q = queue()
    end
    
    function val = remove(queue1)
      % val = q.remove()
      % removes the highest priority object from the queue (oldest inserted)
      % deletes the removed item from the queue
      
      if queue1.readptr == queue1.writeptr
        val = [];
      else
        val = queue1.queueData(queue1.readptr);
        queue1.readptr = queue1.readptr + 1;
        if queue1.readptr > length(queue1.queueData)
          queue1.readptr = 1;
        end
      end
    end
    
  function insert(queue1,data)
    % q.insert(data)
    % inserts a data item or row vector of data items in the
    % queue.
    
    rp = queue1.readptr;
    wp = queue1.writeptr;
    lqd = length(queue1.queueData);
    [m,n] = size(data);
    if n == 0
      return
    end
    if m ~= 1
      error('insert routine requires row vector')
    end
    if rp >= wp
      available_space = rp - wp - 1;
    else
      available_space = lqd - wp + rp - 1;
    end
    if available_space < n
      if rp > wp
        queue1.queueData = [queue1.queueData(rp:lqd), ...
          queue1.queueData(1:wp),zeros(1,n+lqd+2)];
        queue1.writeptr = lqd - rp + wp + 1;
        queue1.readptr = 1;
      else
        queue1.queueData = ...
          [queue1.queueData(rp:wp - 1),zeros(1,n+lqd+2)];
        queue1.writeptr = wp - rp + 1;
        queue1.readptr = 1;
      end
    end
    rp = queue1.readptr;
    wp = queue1.writeptr;
    lqd = length(queue1.queueData);
    if wp + n - 1 <= lqd
      queue1.queueData(wp : wp + n - 1) = data;
      queue1.writeptr = wp + n;
    else
      queue1.queueData(wp:lqd) = data(1 : lqd - wp + 1);
      queue1.queueData(1 : wp + n - lqd - 1) = data(lqd - wp + 2 : end);
      queue1.writeptr = wp + n - lqd;
    end
  end 
  
  
  function sz = sized(queue1)
    % sz = q.sized()
    % returns the number of data items in the queue
    
    if queue1.readptr <= queue1.writeptr
      sz = queue1.writeptr - queue1.readptr;
    else
      sz = queue1.writeptr - queue1.readptr + length(queue1.queueData);
    end
  end
    
  function debug_show(queue1)
    queue1.queueData
    queue1.readptr
    queue1.writeptr
  end
  end
end