This push contains the debugged version with the mass and stiffness matrices. I think there are couple function calls which could somehow be moved for speed, but we can worry about that a little bit later.