Cleaned up, modularised, doesn't quite work currently due to lack of boundary conditions