Fix undefined behavor bugs

Patch provided by @mahkoh