Implement a simple search algorithm by myself, because the A* was not working (and it was too hard to fix it)