Fully tested and working

Accurate to 1e-8.