temp save, working on "real" costMap (dict) containing costs from PID1 to all PIDS. still having issues in last loop. Will use methods to fix.