Little refactoring in two functions
Added some test functions for debug purposes