Minor Improvements to Some Algos

Found some improvements to the algos.
More organization in AlgoDemo