Fix for being able to move diagonally for first tile in path
Added output file for debugging purposes