Cleaned up bugs, wrote tests and docstring. Should work fine
Cleaned up code quality