Dynamic replanning fully tested and debugged

--Accounts for weight changes relevant to the path
--Pathfinder ready to be templated