implemented step 4 in algorithm

untested/code could use some clean up