It could probably use some optimization and I'm not sure about exact cor...

...rectness of math used, but it should be immeasurably better than what we had before.