A lot of fixes. Replaced some Java straight translated functions
with from scratch versions.