Working code (not tested)

First draft