Bug fixes and optimizations

I ran static code analysis and fixed some bugs + did some optimization fixes.
Please review before accepting.