Some refactoring and unit tests
-removed non-standard c++ (variable length arrays)
-modified some functions, so they are easier to understand(hopefully!)

Signed-off-by: Michal Jarzabek <stiopa@gmail.com>