Fixing path finding

* The bitwise And with 0x8ff had mapped different
tiles to the same index.
* Setting a higher cost for diagonal movement. It is
still cheaper than 2 straight moves. But the penalty for
zigzag moves are now higher than what could be gained
through the lowering of the Manhattan distance.