The first solver is working, it is finally time to add a scheme. The solver seems to run extremely slow, however, possibly due to dereferenceing/virtual function overhead. Will look into this later by using an optimizer.