"Improved Algorithm and Integrated JUnit Tests" (Algorithm is still not
quite right)

Signed-off-by: fisherjk <John@John-PC>