* added some test stuff
* made algorithm more generic