Fixed solveForH and removed big timestep in first iteration