Refactored and fixed some bugs. Still oscillating badly.