Added some better code and small updates to path finding.