Remove magic numbers
Path finding improvement