Implemented count_if and a little rearrangement in algorithm.hpp