Optimize algorithm, use sqrt