Working implementation

Source code needs some documentation but it does work.