Added functions to algorithm

Working on functions to execute algorithm, still in progress.