Fixing bug in solve, unittests