Commits first working implementation

Still very dirty, but seems to do the job correctly!