Initial implementation
Make the algorithm more generic