Added more tests and fixed bugs

Still need some more tests, but basic version is working