Fixed integration algorithm.

Not debugged.
Compiles.

Way too long, need to shorten by using functions. A lot of the
code is repeated.