Corrected algorithm, Added allowDiagonal control