Cleaned up a bit. Math still ugly