Adjusted algorithm, cleaned up, ready for testing. Need to solve 1 compiling error in calculation of b