added cost weighting, path tracking and various other improvements