Completed maze path finding algorithm