First fully working version.

So much faster than the python version it was ported from!