Bug fix and add tests