new algorithm based on sorting, removed smooth option, cleaned test code