Improved next point calculation
Reorganized code