[Edit] Modularized sort_points