Improved solution. Added a bit of documentation.
Added test application + CMakeLists