Cleaned up and optimized fit code. Nearly at a 1.0 version of the basic algorithm.