Refactors to prepare for multithreading.

Rewrites a very C-esque routine to a more value oriented function. Functionally
equivalent, but now exposes decent opportunities for thread-level parallelism.
Merge pull request #129 from jorgekva/refactor

Refactors to prepare for multithreading.