Almost working version...

ADD: Tests added.