The path finding is not optimal (it finds it twice) but this algorithm should work.