Rewrite test function and fix compliation warnings