Finished first pass at algorithm implementation, need to debug.