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