Update the code to handle unreachable goals. Refactored the code
and removed expand list which is not required for planning. Added
lot of comments