Added some testing code and modified algorithm