Kind of finished version. Probably need some tinkering still. The algorithm still has the issues with the initial conditions.