I am trying to implement A* search on a grid in MATLAB. I am currently using a priority queue class I found here, but it's a bit slow. I tried to write this simple priority queue class in MATLAB:

classdef PQ < handle
    
    properties
        nElements
        indx;
        priorityList;
        valueList;
    end
    
    methods
        function thePQueue = PQ()
            thePQueue.nElements = 0;
            thePQueue.priorityList{500} = [];
            thePQueue.valueList{500} = [];

            Index = cellfun('isempty', thePQueue.priorityList);
            thePQueue.priorityList(Index) = {NaN};
            thePQueue.valueList(Index) = {NaN};
            thePQueue.indx = 1;
            thePQueue.nElements = 0;
        end
        
        function push(thePQueue, value, priority)
            thePQueue.priorityList{thePQueue.indx} = priority;
            thePQueue.valueList{thePQueue.indx} = value;
            thePQueue.indx = thePQueue.indx + 1;
            thePQueue.nElements = thePQueue.nElements + 1;
        end
   
    
        function minPriorityElement = pop(thePQueue)
            if ~thePQueue.isEmpty
                thePQueue.nElements = thePQueue.nElements - 1;
                [~, minPriorityIndx] = min(cell2mat(thePQueue.priorityList));
                minPriorityElement = thePQueue.valueList{minPriorityIndx};

                thePQueue.priorityList(minPriorityIndx) = {NaN};
                thePQueue.valueList(minPriorityIndx) = {NaN};
            else
                disp('Queue is empty');
            end
        end
    
        function flagIsEmpty = isEmpty(thePQueue)
        flagIsEmpty = (thePQueue.nElements == 0);
        end
    end
end

But It's giving me different results compared to the priority queue I mentioned above. I'm not even sure which one is the right one. I won't have more than 500 nodes on my grid. That's why I'm using constant length cells. This priority queue is almost 10 times faster than the one I'm using but I'm not 100% sure if it is right. How can I check it? Is there any other way I could implement the same idea and get a faster result?

Edit: My question is on hold right now, the reason being: "Questions containing broken code or asking for advice about code not yet written are off-topic...". My question contains a working not-broken code. Please remove the hold!

Edit2: I found a silly bug and now the algorithm is working much better but it wold be great if I could improve the performance.