Warning fixes

Fixes some warnings at compilation, minor stuff.
The original code is completely wrong, this function could never have been tested before..