Fixed the math in this file too
Added timing and done a bit of code cleanup