Coded up the algorithm. Compiles but have not tested the it yet.