Major fixing, added shortest path search