Added temporary implementation of PriorityQueue.  Fixed findNeighbor method. Can handle endpoints properly but intersection points still buggy.