Bugfix: non-void function misses return statement.
Furthermore I made some functions const to help the optimizer.