Complete implementation of first algorithm