Improved cost function with distance measurement, added new args