Add algorithm and test for stable_partition

Added algorithm and test for stable_partition.
Changed partition to use it for now till a
better implementation is found