More optimization and fixed bug from previous commit.
Remove cruft and make some functions static