First finished version. Seems to work, but might need to do more tests.