finished body of algorithm with helper methods, haven't tried compiling