Mostly comments added
Path finding algorithm improvements