Working Algorithm 1

Created a working algorithm 1 implementation. Currently has hard coded
values. TODO change to non-hard coded values.