Testing framework

Build a unit test framework to test the more separated FEM routines.
Might require some reworking of the architecture, but nothing too major.

Also, when this is done, perhaps integrate it with the benchmark tests we can run