Got shortest path between two points working!