Improved speed and added test