Adjust stl_util and add tests