Add more testing code and optimize code