Algorithm complete but bugged