Revamp printing/writing, minor fixes to algorithm