Changed casting in tests and organized some more code