Move io handling functions to util package
And improve algorithm