Algorithm now produces correct results