classdef priorityQueue < handle
  
  properties(SetAccess = private)
    
    heapkeys = [];
    heapindices = [];
    indexposition = [];
    heapsize = 0;
  end
  
  methods(Access = public)
    function pq = priorityQueue(indexmax)
      pq.heapkeys = zeros(indexmax,1);
      pq.heapindices = zeros(indexmax,1);
      pq.indexposition = zeros(indexmax,1);
      pq.heapsize = 0;
    end
  end
  
  methods(Access = private)
    
    function swap(pq,i,j)
      assert(i <= pq.heapsize && i > 0 && j <= pq.heapsize && j > 0 && j ~= i)
      pq.heapkeys([j,i]) = pq.heapkeys([i,j]);
      pq.heapindices([j,i]) = pq.heapindices([i,j]);
      pq.indexposition(pq.heapindices(j)) = j;
      pq.indexposition(pq.heapindices(i)) = i;
    end
    
    function siftup(pq,j)
      assert(j <= pq.heapsize && j > 0)
      while j > 1
        pred = floor(j/2);
        if pq.heapkeys(j) >= pq.heapkeys(pred)
          return;
        end
        pq.swap(j,pred);
        j = pred;
      end
    end
    
    function siftdown(pq,j)
      assert(j <= pq.heapsize && j > 0)
      while j <= pq.heapsize / 2
        succ1 = j * 2;
        if pq.heapkeys(j) > pq.heapkeys(succ1)
          mn = succ1;
        else
          mn = j;
        end
        succ2 = succ1 + 1;
        if succ2 <= pq.heapsize && pq.heapkeys(mn) > pq.heapkeys(succ2)
          mn = succ2;
        end
        if mn == j
          return;
        end
        pq.swap(j,mn);
        j = mn;
      end
    end
  end
    
  methods(Access = public)
    function [minkey, minkeyindex] = deleteMin(pq)
      assert(pq.heapsize > 0);
      minkey = pq.heapkeys(1);
      minkeyindex = pq.heapindices(1);
      if pq.heapsize > 1
        pq.swap(1,pq.heapsize);
      end
      pq.indexposition(pq.heapindices(pq.heapsize)) = 0;
      pq.heapsize = pq.heapsize - 1;
      if pq.heapsize > 1
        pq.siftdown(1);
      end
    end
    
    function insert(pq,key,index)
      pq.heapsize = pq.heapsize + 1;
      pq.heapkeys(pq.heapsize) = key;
      pq.heapindices(pq.heapsize) = index;
      assert(pq.indexposition(index) == 0)
      pq.indexposition(index) = pq.heapsize;
      pq.siftup(pq.heapsize);
    end
    
    function decreaseKey(pq,newkey,index)
      ind = pq.indexposition(index);
      assert(ind > 0 && pq.heapkeys(ind) >= newkey);
      pq.heapkeys(ind) = newkey;
      pq.siftup(ind);
    end
    
    function l = isEmpty(pq)
      l = pq.heapsize == 0;
    end
  end
  
end