Add more tests, optimize the code