Simplified a bit of code (ideas taken from the c port), more to follow