Create algo_util.py

Some algo related utility function and classes so that I can use re-use these for algo problems.