Substantially expanded pathing method

Using wavefront planning method for pathing, general framework in place