Updated with improved algorithm.
Added tests target to makefile.
Updated with simpler algorithm that gives better results.
Removed clock frequency change.