Final cleanup. Algorithm works as expected. Optimization needed.