Add basic functions for algorithm
Tweak algorithm and code style