(almost) functional algo code implemented. NEEDS TO BE UNIT TESTED!!