Ok this is working reasonably well. Needs someone who knows maths though :/
Shaping this up for an initial release!