C++ BFS/Priority queue, beats 100% - Swim in Rising Water