waypoint functionlity added
various fixes + improvements