Adding some features and refactoring some functions. Adding a test(failed though).