Beginning code clean up, tweaked some math