Algorithm mostly working