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