static PetscErrorCode PCView_Redistribute(PC pc,PetscViewer viewer)
{
  PC_Redistribute *red = (PC_Redistribute*)pc->data;
  PetscErrorCode  ierr;
  PetscBool       iascii,isstring;
  PetscInt        ncnt,N;

  PetscFunctionBegin;
  ierr = PetscObjectTypeCompare((PetscObject)viewer,PETSCVIEWERASCII,&iascii);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)viewer,PETSCVIEWERSTRING,&isstring);CHKERRQ(ierr);
  if (iascii) {
    ierr = MPI_Allreduce(&red->dcnt,&ncnt,1,MPIU_INT,MPI_SUM,((PetscObject)pc)->comm);CHKERRQ(ierr);
    ierr = MatGetSize(pc->pmat,&N,PETSC_NULL);CHKERRQ(ierr);
    ierr = PetscViewerASCIIPrintf(viewer,"    Number rows eliminated %D Percentage rows eliminated %g\n",ncnt,100.0*((PetscReal)ncnt)/((PetscReal)N));CHKERRQ(ierr);
    ierr = PetscViewerASCIIPrintf(viewer,"  Redistribute preconditioner: \n");CHKERRQ(ierr);
    ierr = KSPView(red->ksp,viewer);CHKERRQ(ierr);
  } else if (isstring) {
    ierr = PetscViewerStringSPrintf(viewer," Redistribute preconditioner");CHKERRQ(ierr);
    ierr = KSPView(red->ksp,viewer);CHKERRQ(ierr);
  } else SETERRQ1(((PetscObject)pc)->comm,PETSC_ERR_SUP,"Viewer type %s not supported for PC redistribute",((PetscObject)viewer)->type_name);
  PetscFunctionReturn(0);
}

/*@
   MatFDColoringCreate - Creates a matrix coloring context for finite difference
   computation of Jacobians.

   Collective on Mat

   Input Parameters:
+  mat - the matrix containing the nonzero structure of the Jacobian
-  iscoloring - the coloring of the matrix; usually obtained with MatColoringCreate() or DMCreateColoring()

    Output Parameter:
.   color - the new coloring context

    Level: intermediate

.seealso: MatFDColoringDestroy(),SNESComputeJacobianDefaultColor(), ISColoringCreate(),
          MatFDColoringSetFunction(), MatFDColoringSetFromOptions(), MatFDColoringApply(),
          MatFDColoringView(), MatFDColoringSetParameters(), MatColoringCreate(), DMCreateColoring()
@*/
PetscErrorCode  MatFDColoringCreate(Mat mat,ISColoring iscoloring,MatFDColoring *color)
{
  MatFDColoring  c;
  MPI_Comm       comm;
  PetscErrorCode ierr;
  PetscInt       M,N;

  PetscFunctionBegin;
  PetscValidHeaderSpecific(mat,MAT_CLASSID,1);
  if (!mat->assembled) SETERRQ(PetscObjectComm((PetscObject)mat),PETSC_ERR_ARG_WRONGSTATE,"Matrix must be assembled by calls to MatAssemblyBegin/End();");
  ierr = PetscLogEventBegin(MAT_FDColoringCreate,mat,0,0,0);CHKERRQ(ierr);
  ierr = MatGetSize(mat,&M,&N);CHKERRQ(ierr);
  if (M != N) SETERRQ(PetscObjectComm((PetscObject)mat),PETSC_ERR_SUP,"Only for square matrices");
  ierr = PetscObjectGetComm((PetscObject)mat,&comm);CHKERRQ(ierr);
  ierr = PetscHeaderCreate(c,MAT_FDCOLORING_CLASSID,"MatFDColoring","Jacobian computation via finite differences with coloring","Mat",comm,MatFDColoringDestroy,MatFDColoringView);CHKERRQ(ierr);

  c->ctype = iscoloring->ctype;

  if (mat->ops->fdcoloringcreate) {
    ierr = (*mat->ops->fdcoloringcreate)(mat,iscoloring,c);CHKERRQ(ierr);
  } else SETERRQ1(PetscObjectComm((PetscObject)mat),PETSC_ERR_SUP,"Code not yet written for matrix type %s",((PetscObject)mat)->type_name);

  ierr = MatCreateVecs(mat,NULL,&c->w1);CHKERRQ(ierr);
  ierr = PetscLogObjectParent((PetscObject)c,(PetscObject)c->w1);CHKERRQ(ierr);
  ierr = VecDuplicate(c->w1,&c->w2);CHKERRQ(ierr);
  ierr = PetscLogObjectParent((PetscObject)c,(PetscObject)c->w2);CHKERRQ(ierr);

  c->error_rel    = PETSC_SQRT_MACHINE_EPSILON;
  c->umin         = 100.0*PETSC_SQRT_MACHINE_EPSILON;
  c->currentcolor = -1;
  c->htype        = "wp";
  c->fset         = PETSC_FALSE;
  c->setupcalled  = PETSC_FALSE;

  *color = c;
  ierr   = PetscObjectCompose((PetscObject)mat,"SNESMatFDColoring",(PetscObject)c);CHKERRQ(ierr);
  ierr   = PetscLogEventEnd(MAT_FDColoringCreate,mat,0,0,0);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode  MatISSetPreallocation_IS(Mat B,PetscInt d_nz,const PetscInt d_nnz[],PetscInt o_nz,const PetscInt o_nnz[])
{
  Mat_IS         *matis = (Mat_IS*)(B->data);
  PetscInt       bs,i,nlocalcols;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  if (!matis->A) SETERRQ(PetscObjectComm((PetscObject)B),PETSC_ERR_SUP,"You should first call MatSetLocalToGlobalMapping");
  if (!matis->sf) { /* setup SF if not yet created and allocate rootdata and leafdata */
    ierr = MatISComputeSF_Private(B);CHKERRQ(ierr);
  }
  if (!d_nnz) {
    for (i=0;i<matis->sf_nroots;i++) matis->sf_rootdata[i] = d_nz;
  } else {
    for (i=0;i<matis->sf_nroots;i++) matis->sf_rootdata[i] = d_nnz[i];
  }
  if (!o_nnz) {
    for (i=0;i<matis->sf_nroots;i++) matis->sf_rootdata[i] += o_nz;
  } else {
    for (i=0;i<matis->sf_nroots;i++) matis->sf_rootdata[i] += o_nnz[i];
  }
  ierr = PetscSFBcastBegin(matis->sf,MPIU_INT,matis->sf_rootdata,matis->sf_leafdata);CHKERRQ(ierr);
  ierr = MatGetSize(matis->A,NULL,&nlocalcols);CHKERRQ(ierr);
  ierr = MatGetBlockSize(matis->A,&bs);CHKERRQ(ierr);
  ierr = PetscSFBcastEnd(matis->sf,MPIU_INT,matis->sf_rootdata,matis->sf_leafdata);CHKERRQ(ierr);
  for (i=0;i<matis->sf_nleaves;i++) {
    matis->sf_leafdata[i] = PetscMin(matis->sf_leafdata[i],nlocalcols);
  }
  ierr = MatSeqAIJSetPreallocation(matis->A,0,matis->sf_leafdata);CHKERRQ(ierr);
  for (i=0;i<matis->sf_nleaves/bs;i++) {
    matis->sf_leafdata[i] = matis->sf_leafdata[i*bs]/bs;
  }
  ierr = MatSeqBAIJSetPreallocation(matis->A,bs,0,matis->sf_leafdata);CHKERRQ(ierr);
  for (i=0;i<matis->sf_nleaves/bs;i++) {
    matis->sf_leafdata[i] = matis->sf_leafdata[i]-i;
  }
  ierr = MatSeqSBAIJSetPreallocation(matis->A,bs,0,matis->sf_leafdata);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

static void PETScMatvecGenRowMajor(void *x, PRIMME_INT ldx, void *y, PRIMME_INT ldy,
      int blockSize, int trans, Mat matrix, MPI_Comm comm) {
   PetscInt m, n, mLocal, nLocal;
   PetscErrorCode ierr;
   Mat X, Y;
  
   if (blockSize == 1) { 
      PETScMatvecGenNoBlock(x, ldx, y, ldy, blockSize, trans, matrix, comm);
      return;
   }

   assert(sizeof(PetscScalar) == sizeof(SCALAR));   
   ierr = MatGetSize(matrix, &m, &n); CHKERRABORT(comm, ierr);
   ierr = MatGetLocalSize(matrix, &mLocal, &nLocal); CHKERRABORT(comm, ierr);

   if (trans == 0) {
      ierr = MatMatMult_MPIAIJ_MPIDense0(matrix,(PetscScalar*)x,blockSize,ldx,(PetscScalar*)y,ldy);CHKERRABORT(comm, ierr);
   }
   else {
      ierr = MatHermitianTransposeMatMult_MPIAIJ_MPIDense0(matrix,(PetscScalar*)x,blockSize,ldx,(PetscScalar*)y,ldy);CHKERRABORT(comm, ierr);
   }
}

static PetscErrorCode PCBDDCMatTransposeMatSolve_SeqDense(Mat A,Mat B,Mat X)
{
  Mat_SeqDense      *mat = (Mat_SeqDense*)A->data;
  PetscErrorCode    ierr;
  const PetscScalar *b;
  PetscScalar       *x;
  PetscInt          n;
  PetscBLASInt      nrhs,info,m;
  PetscBool         flg;

  PetscFunctionBegin;
  ierr = PetscBLASIntCast(A->rmap->n,&m);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompareAny((PetscObject)B,&flg,MATSEQDENSE,MATMPIDENSE,NULL);CHKERRQ(ierr);
  if (!flg) SETERRQ(PetscObjectComm((PetscObject)A),PETSC_ERR_ARG_WRONG,"Matrix B must be MATDENSE matrix");
  ierr = PetscObjectTypeCompareAny((PetscObject)X,&flg,MATSEQDENSE,MATMPIDENSE,NULL);CHKERRQ(ierr);
  if (!flg) SETERRQ(PetscObjectComm((PetscObject)A),PETSC_ERR_ARG_WRONG,"Matrix X must be MATDENSE matrix");

  ierr = MatGetSize(B,NULL,&n);CHKERRQ(ierr);
  ierr = PetscBLASIntCast(n,&nrhs);CHKERRQ(ierr);
  ierr = MatDenseGetArrayRead(B,&b);CHKERRQ(ierr);
  ierr = MatDenseGetArray(X,&x);CHKERRQ(ierr);
  ierr = PetscMemcpy(x,b,m*nrhs*sizeof(PetscScalar));CHKERRQ(ierr);
  ierr = MatDenseRestoreArrayRead(B,&b);CHKERRQ(ierr);

  if (A->factortype == MAT_FACTOR_LU) {
#if defined(PETSC_MISSING_LAPACK_GETRS)
    SETERRQ(PETSC_COMM_SELF,PETSC_ERR_SUP,"GETRS - Lapack routine is unavailable.");
#else
    PetscStackCallBLAS("LAPACKgetrs",LAPACKgetrs_("T",&m,&nrhs,mat->v,&mat->lda,mat->pivots,x,&m,&info));
    if (info) SETERRQ(PETSC_COMM_SELF,PETSC_ERR_LIB,"GETRS - Bad solve");
#endif
  } else SETERRQ(PETSC_COMM_SELF,PETSC_ERR_SUP,"Only LU factor supported");

  ierr = MatDenseRestoreArray(X,&x);CHKERRQ(ierr);
  ierr = PetscLogFlops(nrhs*(2.0*m*m - m));CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatMVBlock_ConstructScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, PetscTruth sym )
{
	
	
	if( BA->scaling_exists == PETSC_FALSE ) {
		PetscInt M,N;
		PetscTruth is_block;
		
		/* check A is 2x2 block matrix */
		Stg_PetscObjectTypeCompare( (PetscObject)A, "block", &is_block );
		if (is_block==PETSC_FALSE) {
			Stg_SETERRQ( PETSC_ERR_SUP, "Only valid for MatType = block" );
		}
		MatGetSize( A, &M, &N );
		if ( (M!=2) || (N!=2) ) {
			Stg_SETERRQ2( PETSC_ERR_SUP, "Only valid for 2x2 block. Yours has dimension %Dx%D", M,N );
		}
		
		
		
		VecDuplicate( x, &BA->Lz ); 
		VecDuplicate( x, &BA->Rz );
		
		BA->scaling_exists = PETSC_TRUE;
	}
	
//	if( BA->user_build_scaling != PETSC_NULL ) {
//		BA->user_build_scaling( A,b,x,BA->scaling_ctx);
//	}
//	else {
	BSSCR_MatStokesMVBlockDefaultBuildScaling(BA,A,b,x, sym);
//	}
	
	BA->scalings_have_been_inverted = PETSC_FALSE;
	
	PetscFunctionReturn(0);
}

static PetscErrorCode getSumSquares(Mat matrix, double *diag) {
   PetscErrorCode ierr;
   int i, j;
   double *sumr, *sumc;
   PetscInt n, mLocal, nLocal, low, high;
   PetscReal *aux;

   PetscFunctionBegin;

   ierr = MatGetSize(matrix, NULL, &n); CHKERRQ(ierr);
   ierr = MatGetLocalSize(matrix, &mLocal, &nLocal); CHKERRQ(ierr);
   sumr = diag; sumc = &diag[mLocal];

   ierr = PetscMalloc1(n, &aux); CHKERRQ(ierr);
   ierr = MatGetColumnNorms(matrix, NORM_2, aux); CHKERRQ(ierr);
   ierr = MatGetOwnershipRangeColumn(matrix, &low, &high);CHKERRQ(ierr);  
   for (i=low; i<high; i++) {
      sumc[i-low] = aux[i]*aux[i];
   }
   ierr = PetscFree(aux); CHKERRQ(ierr);

   ierr = MatGetOwnershipRange(matrix, &low, &high); CHKERRQ(ierr);
   for (i=low; i<high; i++) {
     PetscInt          ncols;
     const PetscInt    *cols;
     const PetscScalar *vals;

     sumr[i-low] = 0.0;
     ierr = MatGetRow(matrix, i, &ncols, &cols, &vals); CHKERRQ(ierr);
     for (j = 0; j < ncols; j++) {
       sumr[i-low] += PetscRealPart(vals[j]*PetscConj(vals[j]));
     }
     ierr = MatRestoreRow(matrix, i, &ncols, &cols, &vals); CHKERRQ(ierr);
   }

   PetscFunctionReturn(0);
}

/*
name[] is operator name
*/
PetscErrorCode BSSCR_MatInfoLog(PetscViewer v,Mat A,const char name[])
{
	MatInfo i;
	PetscReal nrm_1,nrm_f,nrm_inf;
	MatType mtype;
	PetscInt M,N;
	PetscViewerType vtype;
	PetscTruth isascii;
	
	PetscViewerGetType( v, &vtype );
	Stg_PetscObjectTypeCompare( (PetscObject)v,PETSC_VIEWER_ASCII,&isascii );
	if (!isascii) { PetscFunctionReturn(0); }
	
	MatGetSize( A, &M,&N );
	MatGetInfo(A,MAT_GLOBAL_SUM,&i);
	
	MatGetType( A, &mtype );
	MatNorm( A, NORM_1, &nrm_1 );
	MatNorm( A, NORM_FROBENIUS, &nrm_f );
	MatNorm( A, NORM_INFINITY, &nrm_inf );
	
	PetscViewerASCIIPrintf( v, "MatInfo: %s \n", name );
	PetscViewerASCIIPushTab(v);
	
	PetscViewerASCIIPrintf( v, "type=%s \n", mtype );
	PetscViewerASCIIPrintf( v, "dimension=%Dx%D \n", M,N );
	PetscViewerASCIIPrintf( v, "nnz=%D (total)\n", (PetscInt)i.nz_used );
	PetscViewerASCIIPrintf( v, "nnz=%D (allocated)\n", (PetscInt)i.nz_allocated );
	PetscViewerASCIIPrintf( v, "|A|_1         = %1.12e\n", nrm_1 );
	PetscViewerASCIIPrintf( v, "|A|_frobenius = %1.12e\n", nrm_f );
	PetscViewerASCIIPrintf( v, "|A|_inf       = %1.12e\n", nrm_inf );
	
	PetscViewerASCIIPopTab(v);
	
	PetscFunctionReturn(0);
}

/*
     MatDumpSPAI - Dumps a PETSc matrix to a file in an ASCII format
  suitable for the SPAI code of Stephen Barnard to solve. This routine
  is simply here to allow testing of matrices directly with the SPAI
  code, rather then through the PETSc interface.

*/
PetscErrorCode  MatDumpSPAI(Mat A,FILE *file)
{
  const PetscScalar *vals;
  PetscErrorCode    ierr;
  int               i,j,n,size,nz;
  const int         *cols;
  MPI_Comm          comm;

  PetscObjectGetComm((PetscObject)A,&comm);

  MPI_Comm_size(comm,&size);
  if (size > 1) SETERRQ(PetscObjectComm((PetscObject)A),PETSC_ERR_SUP,"Only single processor dumps");

  ierr = MatGetSize(A,&n,&n);CHKERRQ(ierr);

  /* print the matrix */
  fprintf(file,"%d\n",n);
  for (i=0; i<n; i++) {
    ierr = MatGetRow(A,i,&nz,&cols,&vals);CHKERRQ(ierr);
    for (j=0; j<nz; j++) fprintf(file,"%d %d %16.14e\n",i+1,cols[j]+1,vals[j]);
    ierr = MatRestoreRow(A,i,&nz,&cols,&vals);CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}

static int PetscML_getrow(ML_Operator *ML_data, int N_requested_rows, int requested_rows[],int allocated_space, int columns[], double values[], int row_lengths[])
{
  PetscErrorCode ierr;
  PetscInt       m,i,j,k=0,row,*aj;
  PetscScalar    *aa;
  FineGridCtx    *ml=(FineGridCtx*)ML_Get_MyGetrowData(ML_data);
  Mat_SeqAIJ     *a = (Mat_SeqAIJ*)ml->Aloc->data;

  ierr = MatGetSize(ml->Aloc,&m,PETSC_NULL); if (ierr) return(0);
  for (i = 0; i<N_requested_rows; i++) {
    row   = requested_rows[i];
    row_lengths[i] = a->ilen[row]; 
    if (allocated_space < k+row_lengths[i]) return(0);
    if ( (row >= 0) || (row <= (m-1)) ) {
      aj = a->j + a->i[row];
      aa = a->a + a->i[row];
      for (j=0; j<row_lengths[i]; j++){
        columns[k]  = aj[j];
        values[k++] = aa[j];
      }
    }
  }
  return(1);
}

// -------------------------------------------------------------
// MatDuplicate_DenseGA
// -------------------------------------------------------------
static
PetscErrorCode
MatDuplicate_DenseGA(Mat mat, MatDuplicateOption op, Mat *M)
{
  PetscErrorCode ierr = 0;
  struct MatGACtx *ctx, *newctx;
  ierr = MatShellGetContext(mat, &ctx); CHKERRQ(ierr);

  MPI_Comm comm;
  ierr = PetscObjectGetComm((PetscObject)mat, &comm); CHKERRQ(ierr);

  PetscInt lrows, grows, lcols, gcols;
  ierr = MatGetSize(mat, &grows, &gcols); CHKERRQ(ierr);
  ierr = MatGetLocalSize(mat, &lrows, &lcols); CHKERRQ(ierr);

  ierr = PetscMalloc(sizeof(struct MatGACtx), &newctx); CHKERRQ(ierr);
  newctx->gaGroup = ctx->gaGroup;
  newctx->ga = GA_Duplicate(ctx->ga, "PETSc Dense Matrix");

  ierr = MatCreateShell(comm, lrows, lcols, grows, gcols, newctx, M); CHKERRQ(ierr);
  ierr = MatSetOperations_DenseGA(*M);

  PetscScalar z(0.0);
  switch (op) {
  case (MAT_COPY_VALUES):
    GA_Copy(ctx->ga, newctx->ga);
    break;
  default:
    GA_Fill(newctx->ga, &z);
    break;
  }

  GA_Pgroup_sync(newctx->gaGroup);

  return ierr;
}

/*
   PCGAMGCreateGraph - create simple scaled scalar graph from matrix

 Input Parameter:
 . Amat - matrix
 Output Parameter:
 . a_Gmaat - eoutput scalar graph (symmetric?)
 */
PetscErrorCode PCGAMGCreateGraph(Mat Amat, Mat *a_Gmat)
{
  PetscErrorCode ierr;
  PetscInt       Istart,Iend,Ii,jj,kk,ncols,nloc,NN,MM,bs;
  MPI_Comm       comm;
  Mat            Gmat;
  MatType        mtype;

  PetscFunctionBegin;
  ierr = PetscObjectGetComm((PetscObject)Amat,&comm);CHKERRQ(ierr);
  ierr = MatGetOwnershipRange(Amat, &Istart, &Iend);CHKERRQ(ierr);
  ierr = MatGetSize(Amat, &MM, &NN);CHKERRQ(ierr);
  ierr = MatGetBlockSize(Amat, &bs);CHKERRQ(ierr);
  nloc = (Iend-Istart)/bs;

#if defined PETSC_GAMG_USE_LOG
  ierr = PetscLogEventBegin(petsc_gamg_setup_events[GRAPH],0,0,0,0);CHKERRQ(ierr);
#endif

  if (bs > 1) {
    const PetscScalar *vals;
    const PetscInt    *idx;
    PetscInt          *d_nnz, *o_nnz,*w0,*w1,*w2;
    PetscBool         ismpiaij,isseqaij;

    /*
       Determine the preallocation needed for the scalar matrix derived from the vector matrix.
    */

    ierr = PetscObjectBaseTypeCompare((PetscObject)Amat,MATSEQAIJ,&isseqaij);CHKERRQ(ierr);
    ierr = PetscObjectBaseTypeCompare((PetscObject)Amat,MATMPIAIJ,&ismpiaij);CHKERRQ(ierr);
    ierr = PetscMalloc2(nloc, &d_nnz,isseqaij ? 0 : nloc, &o_nnz);CHKERRQ(ierr);

    if (isseqaij) {
      PetscInt       max_d_nnz;

      /*
          Determine exact preallocation count for (sequential) scalar matrix
      */
      ierr = MatSeqAIJGetMaxRowNonzeros(Amat,&max_d_nnz);CHKERRQ(ierr);
      max_d_nnz = PetscMin(nloc,bs*max_d_nnz);CHKERRQ(ierr);
      ierr = PetscMalloc3(max_d_nnz, &w0,max_d_nnz, &w1,max_d_nnz, &w2);CHKERRQ(ierr);
      for (Ii = 0, jj = 0; Ii < Iend; Ii += bs, jj++) {
        ierr = MatCollapseRows(Amat,Ii,bs,w0,w1,w2,&d_nnz[jj],NULL);CHKERRQ(ierr);
      }
      ierr = PetscFree3(w0,w1,w2);CHKERRQ(ierr);

    } else if (ismpiaij) {
      Mat            Daij,Oaij;
      const PetscInt *garray;
      PetscInt       max_d_nnz;

      ierr = MatMPIAIJGetSeqAIJ(Amat,&Daij,&Oaij,&garray);CHKERRQ(ierr);

      /*
          Determine exact preallocation count for diagonal block portion of scalar matrix
      */
      ierr = MatSeqAIJGetMaxRowNonzeros(Daij,&max_d_nnz);CHKERRQ(ierr);
      max_d_nnz = PetscMin(nloc,bs*max_d_nnz);CHKERRQ(ierr);
      ierr = PetscMalloc3(max_d_nnz, &w0,max_d_nnz, &w1,max_d_nnz, &w2);CHKERRQ(ierr);
      for (Ii = 0, jj = 0; Ii < Iend - Istart; Ii += bs, jj++) {
        ierr = MatCollapseRows(Daij,Ii,bs,w0,w1,w2,&d_nnz[jj],NULL);CHKERRQ(ierr);
      }
      ierr = PetscFree3(w0,w1,w2);CHKERRQ(ierr);

      /*
         Over estimate (usually grossly over), preallocation count for off-diagonal portion of scalar matrix
      */
      for (Ii = 0, jj = 0; Ii < Iend - Istart; Ii += bs, jj++) {
        o_nnz[jj] = 0;
        for (kk=0; kk<bs; kk++) { /* rows that get collapsed to a single row */
          ierr = MatGetRow(Oaij,Ii+kk,&ncols,0,0);CHKERRQ(ierr);
          o_nnz[jj] += ncols;
          ierr = MatRestoreRow(Oaij,Ii+kk,&ncols,0,0);CHKERRQ(ierr);
        }
        if (o_nnz[jj] > (NN/bs-nloc)) o_nnz[jj] = NN/bs-nloc;
      }

    } else SETERRQ(PETSC_COMM_WORLD,PETSC_ERR_USER,"Require AIJ matrix type");

    /* get scalar copy (norms) of matrix */
    ierr = MatGetType(Amat,&mtype);CHKERRQ(ierr);
    ierr = MatCreate(comm, &Gmat);CHKERRQ(ierr);
    ierr = MatSetSizes(Gmat,nloc,nloc,PETSC_DETERMINE,PETSC_DETERMINE);CHKERRQ(ierr);
    ierr = MatSetBlockSizes(Gmat, 1, 1);CHKERRQ(ierr);
    ierr = MatSetType(Gmat, mtype);CHKERRQ(ierr);
    ierr = MatSeqAIJSetPreallocation(Gmat,0,d_nnz);CHKERRQ(ierr);
    ierr = MatMPIAIJSetPreallocation(Gmat,0,d_nnz,0,o_nnz);CHKERRQ(ierr);
    ierr = PetscFree2(d_nnz,o_nnz);CHKERRQ(ierr);

    for (Ii = Istart; Ii < Iend; Ii++) {
      PetscInt dest_row = Ii/bs;
//.........这里部分代码省略.........

PetscErrorCode Create(MPI_Comm comm,Mat *inA,IS *is0,IS *is1)
{
    PetscErrorCode ierr;
    Mat            A;
    PetscInt       r,rend,M;
    PetscMPIInt    rank;

    PetscFunctionBeginUser;
    *inA = 0;
    ierr = MatCreate(comm,&A);
    CHKERRQ(ierr);
    ierr = MatSetSizes(A,4,4,PETSC_DETERMINE,PETSC_DETERMINE);
    CHKERRQ(ierr);
    ierr = MatSetFromOptions(A);
    CHKERRQ(ierr);
    ierr = MatSetUp(A);
    CHKERRQ(ierr);
    ierr = MatGetOwnershipRange(A,&r,&rend);
    CHKERRQ(ierr);
    ierr = MatGetSize(A,&M,NULL);
    CHKERRQ(ierr);

    ierr = ISCreateStride(comm,2,r,1,is0);
    CHKERRQ(ierr);
    ierr = ISCreateStride(comm,2,r+2,1,is1);
    CHKERRQ(ierr);

    ierr = MPI_Comm_rank(comm,&rank);
    CHKERRQ(ierr);

    {
        PetscInt    rows[4],cols0[5],cols1[5],cols2[3],cols3[3];
        PetscScalar RR = 1000.*rank,vals0[5],vals1[4],vals2[3],vals3[3];

        rows[0]            = r;
        rows[1]            = r+1;
        rows[2]            = r+2;
        rows[3]            = r+3;

        cols0[0]           = r+0;
        cols0[1]           = r+1;
        cols0[2]           = r+3;
        cols0[3]           = (r+4)%M;
        cols0[4]           = (r+M-4)%M;

        cols1[0]           = r+1;
        cols1[1]           = r+2;
        cols1[2]           = (r+4+1)%M;
        cols1[3]           = (r+M-4+1)%M;

        cols2[0]           = r;
        cols2[1]           = r+2;
        cols2[2]           = (r+4+2)%M;

        cols3[0]           = r+1;
        cols3[1]           = r+3;
        cols3[2]           = (r+4+3)%M;

        vals0[0] = RR+1.;
        vals0[1] = RR+2.;
        vals0[2] = RR+3.;
        vals0[3] = RR+4.;
        vals0[4] = RR+5.;

        vals1[0] = RR+6.;
        vals1[1] = RR+7.;
        vals1[2] = RR+8.;
        vals1[3] = RR+9.;

        vals2[0] = RR+10.;
        vals2[1] = RR+11.;
        vals2[2] = RR+12.;

        vals3[0] = RR+13.;
        vals3[1] = RR+14.;
        vals3[2] = RR+15.;
        ierr = MatSetValues(A,1,&rows[0],5,cols0,vals0,INSERT_VALUES);
        CHKERRQ(ierr);
        ierr = MatSetValues(A,1,&rows[1],4,cols1,vals1,INSERT_VALUES);
        CHKERRQ(ierr);
        ierr = MatSetValues(A,1,&rows[2],3,cols2,vals2,INSERT_VALUES);
        CHKERRQ(ierr);
        ierr = MatSetValues(A,1,&rows[3],3,cols3,vals3,INSERT_VALUES);
        CHKERRQ(ierr);
    }
    ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY);
    CHKERRQ(ierr);
    ierr = MatAssemblyEnd  (A,MAT_FINAL_ASSEMBLY);
    CHKERRQ(ierr);
    *inA = A;
    PetscFunctionReturn(0);
}

static PetscErrorCode PCSetUp_PARMS(PC pc)
{
  Mat               pmat;
  PC_PARMS          *parms = (PC_PARMS*)pc->data;
  const PetscInt    *mapptr0;
  PetscInt          n, lsize, low, high, i, pos, ncols, length;
  int               *maptmp, *mapptr, *ia, *ja, *ja1, *im;
  PetscScalar       *aa, *aa1;
  const PetscInt    *cols;
  PetscInt          meth[8];
  const PetscScalar *values;
  PetscErrorCode    ierr;
  MatInfo           matinfo;
  PetscMPIInt       rank, npro;

  PetscFunctionBegin;

  /* Get preconditioner matrix from PETSc and setup pARMS structs */
  ierr = PCGetOperators(pc,PETSC_NULL,&pmat,PETSC_NULL);CHKERRQ(ierr);
  MPI_Comm_size(((PetscObject)pmat)->comm,&npro);
  MPI_Comm_rank(((PetscObject)pmat)->comm,&rank);

  ierr = MatGetSize(pmat,&n,PETSC_NULL);CHKERRQ(ierr);
  ierr = PetscMalloc((npro+1)*sizeof(int),&mapptr);CHKERRQ(ierr);
  ierr = PetscMalloc(n*sizeof(int),&maptmp);CHKERRQ(ierr);
  ierr = MatGetOwnershipRanges(pmat,&mapptr0);CHKERRQ(ierr);
  low = mapptr0[rank];
  high = mapptr0[rank+1];
  lsize = high - low;

  for (i=0; i<npro+1; i++)
    mapptr[i] = mapptr0[i]+1;
  for (i = 0; i<n; i++)
    maptmp[i] = i+1;

  /* if created, destroy the previous map */
  if (parms->map) {
    parms_MapFree(&parms->map);
    parms->map = PETSC_NULL;
  }

  /* create pARMS map object */
  parms_MapCreateFromPtr(&parms->map,(int)n,maptmp,mapptr,((PetscObject)pmat)->comm,1,NONINTERLACED);

  /* if created, destroy the previous pARMS matrix */
  if (parms->A) {
    parms_MatFree(&parms->A);
    parms->A = PETSC_NULL;
  }

  /* create pARMS mat object */
  parms_MatCreate(&parms->A,parms->map);

  /* setup and copy csr data structure for pARMS */
  ierr = PetscMalloc((lsize+1)*sizeof(int),&ia);CHKERRQ(ierr);
  ia[0] = 1;
  ierr = MatGetInfo(pmat,MAT_LOCAL,&matinfo);CHKERRQ(ierr);
  length = matinfo.nz_used;
  ierr = PetscMalloc(length*sizeof(int),&ja);CHKERRQ(ierr);
  ierr = PetscMalloc(length*sizeof(PetscScalar),&aa);CHKERRQ(ierr);

  for (i = low; i<high; i++) {
    pos = ia[i-low]-1;
    ierr = MatGetRow(pmat,i,&ncols,&cols,&values);CHKERRQ(ierr);
    ia[i-low+1] = ia[i-low] + ncols;

    if (ia[i-low+1] >= length) {
      length += ncols;
      ierr = PetscMalloc(length*sizeof(int),&ja1);CHKERRQ(ierr);
      ierr = PetscMemcpy(ja1,ja,(ia[i-low]-1)*sizeof(int));CHKERRQ(ierr);
      ierr = PetscFree(ja);CHKERRQ(ierr);
      ja = ja1;
      ierr = PetscMalloc(length*sizeof(PetscScalar),&aa1);CHKERRQ(ierr);
      ierr = PetscMemcpy(aa1,aa,(ia[i-low]-1)*sizeof(PetscScalar));CHKERRQ(ierr);
      ierr = PetscFree(aa);CHKERRQ(ierr);
      aa = aa1;
    }
    ierr = PetscMemcpy(&ja[pos],cols,ncols*sizeof(int));CHKERRQ(ierr);
    ierr = PetscMemcpy(&aa[pos],values,ncols*sizeof(PetscScalar));CHKERRQ(ierr);
    ierr = MatRestoreRow(pmat,i,&ncols,&cols,&values);CHKERRQ(ierr);
  }

  /* csr info is for local matrix so initialize im[] locally */
  ierr = PetscMalloc(lsize*sizeof(int),&im);CHKERRQ(ierr);
  ierr = PetscMemcpy(im,&maptmp[mapptr[rank]-1],lsize*sizeof(int));CHKERRQ(ierr);

  /* 1-based indexing */
  for (i=0; i<ia[lsize]-1; i++)
    ja[i] = ja[i]+1;

  /* Now copy csr matrix to parms_mat object */
  parms_MatSetValues(parms->A,(int)lsize,im,ia,ja,aa,INSERT);

  /* free memory */
  ierr = PetscFree(maptmp);CHKERRQ(ierr);
  ierr = PetscFree(mapptr);CHKERRQ(ierr);
  ierr = PetscFree(aa);CHKERRQ(ierr);
  ierr = PetscFree(ja);CHKERRQ(ierr);
  ierr = PetscFree(ia);CHKERRQ(ierr);
  ierr = PetscFree(im);CHKERRQ(ierr);
//.........这里部分代码省略.........

/*@C
   PCGAMGFilterGraph - filter (remove zero and possibly small values from the) graph and make it symmetric if requested

   Collective on Mat

   Input Parameter:
+   a_Gmat - the graph
.   vfilter - threshold paramter [0,1)
-   symm - make the result symmetric

   Level: developer

   Notes:
    This is called before graph coarsers are called.

.seealso: PCGAMGSetThreshold()
@*/
PetscErrorCode PCGAMGFilterGraph(Mat *a_Gmat,PetscReal vfilter,PetscBool symm)
{
  PetscErrorCode    ierr;
  PetscInt          Istart,Iend,Ii,jj,ncols,nnz0,nnz1, NN, MM, nloc;
  PetscMPIInt       rank;
  Mat               Gmat  = *a_Gmat, tGmat, matTrans;
  MPI_Comm          comm;
  const PetscScalar *vals;
  const PetscInt    *idx;
  PetscInt          *d_nnz, *o_nnz;
  Vec               diag;
  MatType           mtype;

  PetscFunctionBegin;
#if defined PETSC_GAMG_USE_LOG
  ierr = PetscLogEventBegin(petsc_gamg_setup_events[GRAPH],0,0,0,0);CHKERRQ(ierr);
#endif
  /* scale Gmat for all values between -1 and 1 */
  ierr = MatCreateVecs(Gmat, &diag, 0);CHKERRQ(ierr);
  ierr = MatGetDiagonal(Gmat, diag);CHKERRQ(ierr);
  ierr = VecReciprocal(diag);CHKERRQ(ierr);
  ierr = VecSqrtAbs(diag);CHKERRQ(ierr);
  ierr = MatDiagonalScale(Gmat, diag, diag);CHKERRQ(ierr);
  ierr = VecDestroy(&diag);CHKERRQ(ierr);

  if (vfilter < 0.0 && !symm) {
    /* Just use the provided matrix as the graph but make all values positive */
    MatInfo     info;
    PetscScalar *avals;
    PetscBool isaij,ismpiaij;
    ierr = PetscObjectBaseTypeCompare((PetscObject)Gmat,MATSEQAIJ,&isaij);CHKERRQ(ierr);
    ierr = PetscObjectBaseTypeCompare((PetscObject)Gmat,MATMPIAIJ,&ismpiaij);CHKERRQ(ierr);
    if (!isaij && !ismpiaij) SETERRQ(PETSC_COMM_WORLD,PETSC_ERR_USER,"Require (MPI)AIJ matrix type");
    if (isaij) {
      ierr = MatGetInfo(Gmat,MAT_LOCAL,&info);CHKERRQ(ierr);
      ierr = MatSeqAIJGetArray(Gmat,&avals);CHKERRQ(ierr);
      for (jj = 0; jj<info.nz_used; jj++) avals[jj] = PetscAbsScalar(avals[jj]);
      ierr = MatSeqAIJRestoreArray(Gmat,&avals);CHKERRQ(ierr);
    } else {
      Mat_MPIAIJ  *aij = (Mat_MPIAIJ*)Gmat->data;
      ierr = MatGetInfo(aij->A,MAT_LOCAL,&info);CHKERRQ(ierr);
      ierr = MatSeqAIJGetArray(aij->A,&avals);CHKERRQ(ierr);
      for (jj = 0; jj<info.nz_used; jj++) avals[jj] = PetscAbsScalar(avals[jj]);
      ierr = MatSeqAIJRestoreArray(aij->A,&avals);CHKERRQ(ierr);
      ierr = MatGetInfo(aij->B,MAT_LOCAL,&info);CHKERRQ(ierr);
      ierr = MatSeqAIJGetArray(aij->B,&avals);CHKERRQ(ierr);
      for (jj = 0; jj<info.nz_used; jj++) avals[jj] = PetscAbsScalar(avals[jj]);
      ierr = MatSeqAIJRestoreArray(aij->B,&avals);CHKERRQ(ierr);
    }
#if defined PETSC_GAMG_USE_LOG
    ierr = PetscLogEventEnd(petsc_gamg_setup_events[GRAPH],0,0,0,0);CHKERRQ(ierr);
#endif
    PetscFunctionReturn(0);
  }

  ierr = PetscObjectGetComm((PetscObject)Gmat,&comm);CHKERRQ(ierr);
  ierr = MPI_Comm_rank(comm,&rank);CHKERRQ(ierr);
  ierr = MatGetOwnershipRange(Gmat, &Istart, &Iend);CHKERRQ(ierr);
  nloc = Iend - Istart;
  ierr = MatGetSize(Gmat, &MM, &NN);CHKERRQ(ierr);

  if (symm) {
    ierr = MatTranspose(Gmat, MAT_INITIAL_MATRIX, &matTrans);CHKERRQ(ierr);
  }

  /* Determine upper bound on nonzeros needed in new filtered matrix */
  ierr = PetscMalloc2(nloc, &d_nnz,nloc, &o_nnz);CHKERRQ(ierr);
  for (Ii = Istart, jj = 0; Ii < Iend; Ii++, jj++) {
    ierr      = MatGetRow(Gmat,Ii,&ncols,NULL,NULL);CHKERRQ(ierr);
    d_nnz[jj] = ncols;
    o_nnz[jj] = ncols;
    ierr      = MatRestoreRow(Gmat,Ii,&ncols,NULL,NULL);CHKERRQ(ierr);
    if (symm) {
      ierr       = MatGetRow(matTrans,Ii,&ncols,NULL,NULL);CHKERRQ(ierr);
      d_nnz[jj] += ncols;
      o_nnz[jj] += ncols;
      ierr       = MatRestoreRow(matTrans,Ii,&ncols,NULL,NULL);CHKERRQ(ierr);
    }
    if (d_nnz[jj] > nloc) d_nnz[jj] = nloc;
    if (o_nnz[jj] > (MM-nloc)) o_nnz[jj] = MM - nloc;
  }
  ierr = MatGetType(Gmat,&mtype);CHKERRQ(ierr);
  ierr = MatCreate(comm, &tGmat);CHKERRQ(ierr);
//.........这里部分代码省略.........

/*
  MatConvert_Basic - Converts from any input format to another format. For
  parallel formats, the new matrix distribution is determined by PETSc.

  Does not do preallocation so in general will be slow
 */
PetscErrorCode MatConvert_Basic(Mat mat, MatType newtype,MatReuse reuse,Mat *newmat)
{
  Mat               M;
  const PetscScalar *vwork;
  PetscErrorCode    ierr;
  PetscInt          i,j,nz,m,n,rstart,rend,lm,ln,prbs,pcbs,cstart,cend,*dnz,*onz;
  const PetscInt    *cwork;
  PetscBool         isseqsbaij,ismpisbaij,isseqbaij,ismpibaij,isseqdense,ismpidense;

  PetscFunctionBegin;
  ierr = MatGetSize(mat,&m,&n);CHKERRQ(ierr);
  ierr = MatGetLocalSize(mat,&lm,&ln);CHKERRQ(ierr);

  if (ln == n) ln = PETSC_DECIDE; /* try to preserve column ownership */

  ierr = MatCreate(PetscObjectComm((PetscObject)mat),&M);CHKERRQ(ierr);
  ierr = MatSetSizes(M,lm,ln,m,n);CHKERRQ(ierr);
  ierr = MatSetBlockSizes(M,mat->rmap->bs,mat->cmap->bs);CHKERRQ(ierr);
  ierr = MatSetType(M,newtype);CHKERRQ(ierr);
  ierr = MatGetOwnershipRange(mat,&rstart,&rend);CHKERRQ(ierr);

  ierr = PetscObjectTypeCompare((PetscObject)M,MATSEQSBAIJ,&isseqsbaij);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)M,MATMPISBAIJ,&ismpisbaij);CHKERRQ(ierr);
  if (isseqsbaij || ismpisbaij) {ierr = MatSetOption(M,MAT_IGNORE_LOWER_TRIANGULAR,PETSC_TRUE);CHKERRQ(ierr);}
  ierr = PetscObjectTypeCompare((PetscObject)M,MATSEQBAIJ,&isseqbaij);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)M,MATMPIBAIJ,&ismpibaij);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)M,MATSEQDENSE,&isseqdense);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)M,MATMPIDENSE,&ismpidense);CHKERRQ(ierr);

  if (isseqdense) {
    ierr = MatSeqDenseSetPreallocation(M,NULL);CHKERRQ(ierr);
  } else if (ismpidense) {
    ierr = MatMPIDenseSetPreallocation(M,NULL);CHKERRQ(ierr);
  } else {
    /* Preallocation block sizes.  (S)BAIJ matrices will have one index per block. */
    prbs = (isseqbaij || ismpibaij || isseqsbaij || ismpisbaij) ? M->rmap->bs : 1;
    pcbs = (isseqbaij || ismpibaij || isseqsbaij || ismpisbaij) ? M->cmap->bs : 1;

    ierr = PetscMalloc2(lm/prbs,&dnz,lm/prbs,&onz);CHKERRQ(ierr);
    ierr = MatGetOwnershipRangeColumn(mat,&cstart,&cend);CHKERRQ(ierr);
    for (i=0; i<lm; i+=prbs) {
      ierr = MatGetRow(mat,rstart+i,&nz,&cwork,NULL);CHKERRQ(ierr);
      dnz[i] = 0;
      onz[i] = 0;
      for (j=0; j<nz; j+=pcbs) {
        if ((isseqsbaij || ismpisbaij) && cwork[j] < rstart+i) continue;
        if (cstart <= cwork[j] && cwork[j] < cend) dnz[i]++;
        else                                       onz[i]++;
      }
      ierr = MatRestoreRow(mat,rstart+i,&nz,&cwork,NULL);CHKERRQ(ierr);
    }
    ierr = MatXAIJSetPreallocation(M,M->rmap->bs,dnz,onz,dnz,onz);CHKERRQ(ierr);
    ierr = PetscFree2(dnz,onz);CHKERRQ(ierr);
  }

  for (i=rstart; i<rend; i++) {
    ierr = MatGetRow(mat,i,&nz,&cwork,&vwork);CHKERRQ(ierr);
    ierr = MatSetValues(M,1,&i,nz,cwork,vwork,INSERT_VALUES);CHKERRQ(ierr);
    ierr = MatRestoreRow(mat,i,&nz,&cwork,&vwork);CHKERRQ(ierr);
  }
  ierr = MatAssemblyBegin(M,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd(M,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);

  if (reuse == MAT_REUSE_MATRIX) {
    ierr = MatHeaderReplace(mat,M);CHKERRQ(ierr);
  } else {
    *newmat = M;
  }
  PetscFunctionReturn(0);
}

//.........这里部分代码省略.........
    if (alpha <= 0.0) {
      ksp->reason = KSP_DIVERGED_BREAKDOWN;
      PetscFunctionReturn(0);
    }
    alpha = PetscSqrtReal(alpha);
    ierr  = VecScale(Z,1.0/alpha);CHKERRQ(ierr);
  }
  ierr = VecScale(V,1.0/alpha);CHKERRQ(ierr);

  if (nopreconditioner) {
    ierr = VecCopy(V,W);CHKERRQ(ierr);
  } else {
    ierr = VecCopy(Z,W);CHKERRQ(ierr);
  }

  lsqr->arnorm = alpha * beta;
  phibar       = beta;
  rhobar       = alpha;
  i            = 0;
  do {
    if (nopreconditioner) {
      ierr = KSP_MatMult(ksp,Amat,V,U1);CHKERRQ(ierr);
    } else {
      ierr = KSP_MatMult(ksp,Amat,Z,U1);CHKERRQ(ierr);
    }
    ierr = VecAXPY(U1,-alpha,U);CHKERRQ(ierr);
    ierr = VecNorm(U1,NORM_2,&beta);CHKERRQ(ierr);
    if (beta == 0.0) {
      ksp->reason = KSP_DIVERGED_BREAKDOWN;
      break;
    }
    ierr = VecScale(U1,1.0/beta);CHKERRQ(ierr);

    ierr = KSP_MatMultTranspose(ksp,Amat,U1,V1);CHKERRQ(ierr);
    ierr = VecAXPY(V1,-beta,V);CHKERRQ(ierr);
    if (nopreconditioner) {
      ierr = VecNorm(V1,NORM_2,&alpha);CHKERRQ(ierr);
    } else {
      ierr = PCApply(ksp->pc,V1,Z);CHKERRQ(ierr);
      ierr = VecDotRealPart(V1,Z,&alpha);CHKERRQ(ierr);
      if (alpha <= 0.0) {
        ksp->reason = KSP_DIVERGED_BREAKDOWN;
        break;
      }
      alpha = PetscSqrtReal(alpha);
      ierr  = VecScale(Z,1.0/alpha);CHKERRQ(ierr);
    }
    ierr   = VecScale(V1,1.0/alpha);CHKERRQ(ierr);
    rho    = PetscSqrtScalar(rhobar*rhobar + beta*beta);
    c      = rhobar / rho;
    s      = beta / rho;
    theta  = s * alpha;
    rhobar = -c * alpha;
    phi    = c * phibar;
    phibar = s * phibar;
    tau    = s * phi;

    ierr = VecAXPY(X,phi/rho,W);CHKERRQ(ierr);  /*    x <- x + (phi/rho) w   */

    if (SE) {
      ierr = VecCopy(W,W2);CHKERRQ(ierr);
      ierr = VecSquare(W2);CHKERRQ(ierr);
      ierr = VecScale(W2,1.0/(rho*rho));CHKERRQ(ierr);
      ierr = VecAXPY(SE, 1.0, W2);CHKERRQ(ierr); /* SE <- SE + (w^2/rho^2) */
    }
    if (nopreconditioner) {
      ierr = VecAYPX(W,-theta/rho,V1);CHKERRQ(ierr);  /* w <- v - (theta/rho) w */
    } else {
      ierr = VecAYPX(W,-theta/rho,Z);CHKERRQ(ierr);   /* w <- z - (theta/rho) w */
    }

    lsqr->arnorm = alpha*PetscAbsScalar(tau);
    rnorm        = PetscRealPart(phibar);

    ierr = PetscObjectAMSTakeAccess((PetscObject)ksp);CHKERRQ(ierr);
    ksp->its++;
    ksp->rnorm = rnorm;
    ierr       = PetscObjectAMSGrantAccess((PetscObject)ksp);CHKERRQ(ierr);
    ierr = KSPLogResidualHistory(ksp,rnorm);CHKERRQ(ierr);
    ierr = KSPMonitor(ksp,i+1,rnorm);CHKERRQ(ierr);
    ierr = (*ksp->converged)(ksp,i+1,rnorm,&ksp->reason,ksp->cnvP);CHKERRQ(ierr);
    if (ksp->reason) break;
    SWAP(U1,U,TMP);
    SWAP(V1,V,TMP);

    i++;
  } while (i<ksp->max_it);
  if (i >= ksp->max_it && !ksp->reason) ksp->reason = KSP_DIVERGED_ITS;

  /* Finish off the standard error estimates */
  if (SE) {
    tmp  = 1.0;
    ierr = MatGetSize(Amat,&size1,&size2);CHKERRQ(ierr);
    if (size1 > size2) tmp = size1 - size2;
    tmp  = rnorm / PetscSqrtScalar(tmp);
    ierr = VecSqrtAbs(SE);CHKERRQ(ierr);
    ierr = VecScale(SE,tmp);CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}

PetscErrorCode BearQuery(PetscInt s, PetscScalar c, Mat invL1, Mat invU1, Mat invL2, Mat invU2, Mat H12, Mat H21, Vec order, Vec or){
    PetscErrorCode err;
    PetscInt n1, n2, n;
    PetscInt oseed;
    PetscScalar val, one = 1.0;
    Vec r = NULL;
    Vec r1 = NULL, q1 = NULL, t1_1 = NULL, t1_2 = NULL, t1_3 = NULL, t1_4 = NULL, t1_5 = NULL; // dimension: n1
    Vec r2 = NULL, q2 = NULL, q_tilda = NULL, t2_1 = NULL, t2_2 = NULL, t2_3 = NULL; // dimension: n2


    err = MatGetSize(H12, &n1, &n2); CHKERRQ(err);
    n = n1 + n2;
    //    err = PetscPrintf(PETSC_COMM_WORLD, "n1: %d, n2: %d\n", n1, n2); CHKERRQ(err);

    err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n, &r); CHKERRQ(err);
    err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n1, &q1); CHKERRQ(err);
    err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n2, &q2); CHKERRQ(err);
    err = VecSet(q1, 0); CHKERRQ(err);
    err = VecSet(q2, 0); CHKERRQ(err);

    s = s - 1; // shift -1 for zero-based index
    err = VecGetValues(order, 1, &s, &val); CHKERRQ(err);
    oseed = (PetscInt) val;
    //    err = PetscPrintf(PETSC_COMM_WORLD, "Given seed: %d, Reorered seed: %d (0 ~ n-1)\n", s, oseed); CHKERRQ(err);

    if(oseed < n1){
        err = VecSetValues(q1, 1, &oseed, &one, INSERT_VALUES); CHKERRQ(err);
    }else{
        oseed = oseed - n1;
        err = VecSetValues(q2, 1, &oseed, &one, INSERT_VALUES); CHKERRQ(err);
        //err = printVecSum(q2);
    }
    err = VecAssemblyBegin(q1); CHKERRQ(err);
    err = VecAssemblyBegin(q2); CHKERRQ(err);
    err = VecAssemblyEnd(q1); CHKERRQ(err);
    err = VecAssemblyEnd(q2); CHKERRQ(err);

    err = VecDuplicate(q1, &r1); CHKERRQ(err);
    err = VecDuplicate(q1, &t1_1); CHKERRQ(err);
    err = VecDuplicate(q1, &t1_2); CHKERRQ(err);
    err = VecDuplicate(q1, &t1_3); CHKERRQ(err);
    err = VecDuplicate(q1, &t1_4); CHKERRQ(err);
    err = VecDuplicate(q1, &t1_5); CHKERRQ(err);

    err = VecDuplicate(q2, &r2); CHKERRQ(err);
    err = VecDuplicate(q2, &q_tilda); CHKERRQ(err);
    err = VecDuplicate(q2, &t2_1); CHKERRQ(err);
    err = VecDuplicate(q2, &t2_2); CHKERRQ(err);
    err = VecDuplicate(q2, &t2_3); CHKERRQ(err);

    // Start matrix-vec multiplications
    err = MatMult(invL1, q1, t1_1); CHKERRQ(err);
    err = MatMult(invU1, t1_1, t1_2); CHKERRQ(err);
    err = MatMult(H21, t1_2, t2_1); CHKERRQ(err);
    err = VecAXPBYPCZ(q_tilda, 1.0, -1.0, 0.0, q2, t2_1); CHKERRQ(err);
    err = MatMult(invL2, q_tilda, t2_2); CHKERRQ(err);
    err = MatMult(invU2, t2_2, r2); CHKERRQ(err);

    err = MatMult(H12, r2, t1_3); CHKERRQ(err);
    err = VecAXPBYPCZ(t1_4, 1.0, -1.0, 0.0, q1, t1_3); CHKERRQ(err);
    err = MatMult(invL1, t1_4, t1_5); CHKERRQ(err);
    err = MatMult(invU1, t1_5, r1); CHKERRQ(err);
    //err = printVe