Finish implementation of main algorithm