Fix some bugs and add optimizations.

This is just some bugs getting fixed and some optimizations added. Also
there are a few more tests.

Signed-off-by: Ignas Anikevicius (gns_ank) <anikevicius@gmail.com>