Some success

Was able to see some success with this version of the code. I still need
to revise the method when pointA and pointB nodes are inserted into
final path when can't be found.