Algorithm general fixes, but still not working