Cleaned up the code a bit so that it is more readable.
TODO:
  1. Implement RK 4 and compare
  2. Object Orient it
  3. Animate