Fix one bug for the cg step. However, there is still bug in the test cases in file test_solver.h