Fix and add tests