Removed unused code, i++->++i, added cmath include