Algorithm full impl... fixes required!