Fix logic mistakes; add test, comments