Cleaned up trapezoidal method