Working implementation of Crank-Nicolson scheme. Needs some refactoring