/*@C
   TaoSetJacobianInequalityRoutine - Sets the function to compute the Jacobian
   (and its inverse) of the constraint function with respect to the inequality variables.
   Used only for pde-constrained optimization.

   Logically collective on Tao

   Input Parameters:
+  tao - the Tao context
.  J - Matrix used for the jacobian
.  Jpre - Matrix that will be used operated on by PETSc preconditioner, can be same as J.
.  jac - Jacobian evaluation routine
-  ctx - [optional] user-defined context for private data for the
         Jacobian evaluation routine (may be NULL)

   Calling sequence of jac:
$    jac (Tao tao,Vec x,Mat *J,Mat *Jpre,void *ctx);

+  tao - the Tao  context
.  x - input vector
.  J - Jacobian matrix
.  Jpre - preconditioner matrix, usually the same as J
-  ctx - [optional] user-defined Jacobian context

   Level: intermediate
.seealse: TaoComputeJacobianInequality(), TaoSetJacobianDesignRoutine(), TaoSetInequalityDesignIS()
@*/
PetscErrorCode TaoSetJacobianInequalityRoutine(Tao tao, Mat J, Mat Jpre, PetscErrorCode (*func)(Tao, Vec, Mat, Mat,void*), void *ctx)
{
    PetscErrorCode ierr;
    PetscFunctionBegin;
    PetscValidHeaderSpecific(tao,TAO_CLASSID,1);
    if (J) {
        PetscValidHeaderSpecific(J,MAT_CLASSID,2);
        PetscCheckSameComm(tao,1,J,2);
    }
    if (Jpre) {
        PetscValidHeaderSpecific(Jpre,MAT_CLASSID,3);
        PetscCheckSameComm(tao,1,Jpre,3);
    }
    if (ctx) {
        tao->user_jac_inequalityP = ctx;
    }
    if (func) {
        tao->ops->computejacobianinequality = func;
    }
    if (J) {
        ierr = PetscObjectReference((PetscObject)J);
        CHKERRQ(ierr);
        ierr = MatDestroy(&tao->jacobian_inequality);
        CHKERRQ(ierr);
        tao->jacobian_inequality = J;
    }
    if (Jpre) {
        ierr = PetscObjectReference((PetscObject)Jpre);
        CHKERRQ(ierr);
        ierr = MatDestroy(&tao->jacobian_inequality_pre);
        CHKERRQ(ierr);
        tao->jacobian_inequality_pre=Jpre;
    }
    PetscFunctionReturn(0);
}

/*@
   DMDACreateNaturalVector - Creates a parallel PETSc vector that
   will hold vector values in the natural numbering, rather than in
   the PETSc parallel numbering associated with the DMDA.

   Collective

   Input Parameter:
.  da - the distributed array

   Output Parameter:
.  g - the distributed global vector

   Level: developer

   Note:
   The output parameter, g, is a regular PETSc vector that should be destroyed
   with a call to VecDestroy() when usage is finished.

   The number of local entries in the vector on each process is the same
   as in a vector created with DMCreateGlobalVector().

.keywords: distributed array, create, global, distributed, vector

.seealso: DMCreateLocalVector(), VecDuplicate(), VecDuplicateVecs(),
          DMDACreate1d(), DMDACreate2d(), DMDACreate3d(), DMGlobalToLocalBegin(),
          DMGlobalToLocalEnd(), DMDALocalToGlobalBegin()
@*/
PetscErrorCode  DMDACreateNaturalVector(DM da,Vec *g)
{
  PetscErrorCode ierr;
  PetscInt       cnt;
  DM_DA          *dd = (DM_DA*)da->data;

  PetscFunctionBegin;
  PetscValidHeaderSpecificType(da,DM_CLASSID,1,DMDA);
  PetscValidPointer(g,2);
  if (dd->natural) {
    ierr = PetscObjectGetReference((PetscObject)dd->natural,&cnt);CHKERRQ(ierr);
    if (cnt == 1) { /* object is not currently used by anyone */
      ierr = PetscObjectReference((PetscObject)dd->natural);CHKERRQ(ierr);
      *g   = dd->natural;
    } else {
      ierr = VecDuplicate(dd->natural,g);CHKERRQ(ierr);
    }
  } else { /* create the first version of this guy */
    ierr = VecCreate(PetscObjectComm((PetscObject)da),g);CHKERRQ(ierr);
    ierr = VecSetSizes(*g,dd->Nlocal,PETSC_DETERMINE);CHKERRQ(ierr);
    ierr = VecSetBlockSize(*g, dd->w);CHKERRQ(ierr);
    ierr = VecSetType(*g,da->vectype);CHKERRQ(ierr);
    ierr = PetscObjectReference((PetscObject)*g);CHKERRQ(ierr);

    dd->natural = *g;
  }
  PetscFunctionReturn(0);
}

/** Set rotation for certain nodes in a function space
*
* @param fs the function space
* @param is index set of nodes to rotate, sequential, with respect blocks of local vector
* @param rot Rotation matrices at all nodes in \a is.  Should have length \c bs*bs*size(is).
* @param ns number of dofs to enforce strongly at each node (every entry must have 0<=ns[i]<=bs)
* @param v Vector of values for strongly enforced dofs
*
* @example Consider 2D flow over a bed with known melt rates.  Suppose the local velocity vector is
*
*   [u0x,u0y; u1x,u1y; u2x,u2y; u3x,u3y | u4x,u4y]
*
* (4 owned blocks, one ghosted block) and nodes 1,4 are on the slip boundary with normal and tangent vectors n1,t1,n4,t4
* and melt rates r1,r4.  To enforce the melt rate strongly, use
*
* \a is = [1,4]
* \a rot = [n10,n11,t10,t11, n40,n41,t40,t41]
* \a ns = [1,1]
* \a v = [r1,r4]
*
* The rotated vector will become (. = \cdot)
*
*   [u0x,u0y; u1.n1,u1.t1; u2x,u2y; u3x,u3y | u4.n4,u4.t4]
*
* and strongly enforcing melt rate produces the global vector
*
*   [u0x,u0y; r1,u1.t1; u2x,u2y; u3x,u3y | r4,u4.t4] .
*
* This is what the solver sees, the Jacobian will always have rows and columns of the identity corresponding to the
* strongly enforced components (2,8 of the local vector) and the residual will always be 0 in these components.  Hence
* the Newton step v will always be of the form
*
*   [v0x,v0y; 0,v1y; v2x,v2y; v3x,v3y | 0,v4y] .
**/
dErr dFSRotationCreate(dFS fs,IS is,dReal rmat[],dInt ns[],Vec v,dFSRotation *inrot)
{
  dFSRotation rot;
  dInt bs,n;
  dErr err;

  dFunctionBegin;
  dValidHeader(fs,DM_CLASSID,1);
  dValidHeader(is,IS_CLASSID,2);
  dValidRealPointer(rmat,3);
  dValidIntPointer(ns,4);
  dValidHeader(v,VEC_CLASSID,5);
  dValidPointer(inrot,6);
  *inrot = 0;
  err = PetscHeaderCreate(rot,_p_dFSRotation,struct _dFSRotationOps,dFSROT_CLASSID,"dFSRotation","Local function space rotation","FS",PETSC_COMM_SELF,dFSRotationDestroy,dFSRotationView);dCHK(err);

  err = dFSGetBlockSize(fs,&bs);dCHK(err);
  rot->bs = bs;
  err = ISGetSize(is,&n);dCHK(err);
  rot->n = n;
  err = PetscObjectReference((PetscObject)is);dCHK(err);
  rot->is = is;
  err = PetscObjectReference((PetscObject)v);dCHK(err);
  rot->strong = v;
  for (dInt i=0; i<n; i++) {
    if (ns[i] < 0 || bs < ns[i]) dERROR(PETSC_COMM_SELF,1,"Number of strong dofs must be between 0 and bs=%d (inclusive)",bs);
    /* \todo Check that every rmat is orthogonal */
  }
  err = dMallocA2(n*bs*bs,&rot->rmat,n,&rot->nstrong);dCHK(err);
  err = dMemcpy(rot->rmat,rmat,n*bs*bs*sizeof rmat[0]);dCHK(err);
  err = dMemcpy(rot->nstrong,ns,n*sizeof ns[0]);dCHK(err);
  *inrot = rot;
  dFunctionReturn(0);
}

PETSC_EXTERN PetscErrorCode TaoLMVMSetH0(Tao tao, Mat H0)
{
  TAO_LMVM       *lmP;
  TAO_BLMVM      *blmP;
  const TaoType  type;
  PetscBool is_lmvm, is_blmvm;

  PetscErrorCode ierr;

  ierr = TaoGetType(tao, &type);CHKERRQ(ierr);
  ierr = PetscStrcmp(type, TAOLMVM,  &is_lmvm);CHKERRQ(ierr);
  ierr = PetscStrcmp(type, TAOBLMVM, &is_blmvm);CHKERRQ(ierr);

  if (is_lmvm) {
    lmP = (TAO_LMVM *)tao->data;
    ierr = PetscObjectReference((PetscObject)H0);CHKERRQ(ierr);
    lmP->H0 = H0;
  } else if (is_blmvm) {
    blmP = (TAO_BLMVM *)tao->data;
    ierr = PetscObjectReference((PetscObject)H0);CHKERRQ(ierr);
    blmP->H0 = H0;
  } else SETERRQ(PetscObjectComm((PetscObject)tao), PETSC_ERR_ARG_WRONGSTATE, "This routine applies to TAO_LMVM and TAO_BLMVM.");

  PetscFunctionReturn(0);
}

/*@
   ISGetNonlocalIS - Gather all nonlocal indices for this IS and present
                     them as another sequential index set.


   Collective on IS

   Input Parameter:
.  is - the index set

   Output Parameter:
.  complement - sequential IS with indices identical to the result of
                ISGetNonlocalIndices()

   Level: intermediate

   Notes: complement represents the result of ISGetNonlocalIndices as an IS.
          Therefore scalability issues similar to ISGetNonlocalIndices apply.
          The resulting IS must be restored using ISRestoreNonlocalIS().

   Concepts: index sets^getting nonlocal indices
.seealso: ISGetNonlocalIndices(), ISRestoreNonlocalIndices(),  ISAllGather(), ISGetSize()
@*/
PetscErrorCode  ISGetNonlocalIS(IS is, IS *complement)
{
    PetscErrorCode ierr;

    PetscFunctionBegin;
    PetscValidHeaderSpecific(is,IS_CLASSID,1);
    PetscValidPointer(complement,2);
    /* Check if the complement exists already. */
    if (is->complement) {
        *complement = is->complement;
        ierr = PetscObjectReference((PetscObject)(is->complement));
        CHKERRQ(ierr);
    } else {
        PetscInt       N, n;
        const PetscInt *idx;
        ierr = ISGetSize(is, &N);
        CHKERRQ(ierr);
        ierr = ISGetLocalSize(is,&n);
        CHKERRQ(ierr);
        ierr = ISGetNonlocalIndices(is, &idx);
        CHKERRQ(ierr);
        ierr = ISCreateGeneral(PETSC_COMM_SELF, N-n,idx, PETSC_USE_POINTER, &(is->complement));
        CHKERRQ(ierr);
        ierr = PetscObjectReference((PetscObject)is->complement);
        CHKERRQ(ierr);
        *complement = is->complement;
    }
    PetscFunctionReturn(0);
}

/*@C
   TaoSetHessianRoutine - Sets the function to compute the Hessian as well as the location to store the matrix.

   Logically collective on Tao

   Input Parameters:
+  tao - the Tao context
.  H - Matrix used for the hessian
.  Hpre - Matrix that will be used operated on by preconditioner, can be same as H
.  hess - Hessian evaluation routine
-  ctx - [optional] user-defined context for private data for the
         Hessian evaluation routine (may be NULL)

   Calling sequence of hess:
$    hess (Tao tao,Vec x,Mat H,Mat Hpre,void *ctx);

+  tao - the Tao  context
.  x - input vector
.  H - Hessian matrix
.  Hpre - preconditioner matrix, usually the same as H
-  ctx - [optional] user-defined Hessian context

   Level: beginner

@*/
PetscErrorCode TaoSetHessianRoutine(Tao tao, Mat H, Mat Hpre, PetscErrorCode (*func)(Tao, Vec, Mat, Mat, void*), void *ctx)
{
  PetscErrorCode ierr;

  PetscFunctionBegin;
  PetscValidHeaderSpecific(tao,TAO_CLASSID,1);
  if (H) {
    PetscValidHeaderSpecific(H,MAT_CLASSID,2);
    PetscCheckSameComm(tao,1,H,2);
  }
  if (Hpre) {
    PetscValidHeaderSpecific(Hpre,MAT_CLASSID,3);
    PetscCheckSameComm(tao,1,Hpre,3);
  }
  if (ctx) {
    tao->user_hessP = ctx;
  }
  if (func) {
    tao->ops->computehessian = func;
  }
  if (H) {
    ierr = PetscObjectReference((PetscObject)H);CHKERRQ(ierr);
    ierr = MatDestroy(&tao->hessian);CHKERRQ(ierr);
    tao->hessian = H;
  }
  if (Hpre) {
    ierr = PetscObjectReference((PetscObject)Hpre);CHKERRQ(ierr);
    ierr = MatDestroy(&tao->hessian_pre);CHKERRQ(ierr);
    tao->hessian_pre = Hpre;
  }
  PetscFunctionReturn(0);
}

static PetscErrorCode PCBDDCScalingSetUp_Deluxe_Private(PC pc)
{
  PC_BDDC                *pcbddc=(PC_BDDC*)pc->data;
  PCBDDCDeluxeScaling    deluxe_ctx=pcbddc->deluxe_ctx;
  PCBDDCSubSchurs        sub_schurs = pcbddc->sub_schurs;
  PetscErrorCode         ierr;

  PetscFunctionBegin;
  if (!sub_schurs->n_subs) {
    PetscFunctionReturn(0);
  }

  /* Create work vectors for sequential part of deluxe */
  ierr = MatCreateVecs(sub_schurs->S_Ej_all,&deluxe_ctx->seq_work1,&deluxe_ctx->seq_work2);CHKERRQ(ierr);

  /* Compute deluxe sequential scatter */
  if (sub_schurs->reuse_mumps && !sub_schurs->is_dir) {
    PCBDDCReuseMumps reuse_mumps = sub_schurs->reuse_mumps;
    ierr = PetscObjectReference((PetscObject)reuse_mumps->correction_scatter_B);CHKERRQ(ierr);
    deluxe_ctx->seq_scctx = reuse_mumps->correction_scatter_B;
  } else {
    ierr = VecScatterCreate(pcbddc->work_scaling,sub_schurs->is_Ej_all,deluxe_ctx->seq_work1,NULL,&deluxe_ctx->seq_scctx);CHKERRQ(ierr);
  }

  /* Create Mat object for deluxe scaling */
  ierr = PetscObjectReference((PetscObject)sub_schurs->S_Ej_all);CHKERRQ(ierr);
  deluxe_ctx->seq_mat = sub_schurs->S_Ej_all;
  if (sub_schurs->sum_S_Ej_all) { /* if this matrix is present, then we need to create the KSP object to invert it */
    PC               pc_temp;
    MatSolverPackage solver=NULL;
    char             ksp_prefix[256];
    size_t           len;

    ierr = KSPCreate(PETSC_COMM_SELF,&deluxe_ctx->seq_ksp);CHKERRQ(ierr);
    ierr = KSPSetOperators(deluxe_ctx->seq_ksp,sub_schurs->sum_S_Ej_all,sub_schurs->sum_S_Ej_all);CHKERRQ(ierr);
    ierr = KSPSetType(deluxe_ctx->seq_ksp,KSPPREONLY);CHKERRQ(ierr);
    ierr = KSPGetPC(deluxe_ctx->seq_ksp,&pc_temp);CHKERRQ(ierr);
    ierr = PCSetType(pc_temp,PCLU);CHKERRQ(ierr);
    ierr = KSPGetPC(pcbddc->ksp_D,&pc_temp);CHKERRQ(ierr);
    ierr = PCFactorGetMatSolverPackage(pc_temp,(const MatSolverPackage*)&solver);CHKERRQ(ierr);
    if (solver) {
      PC     new_pc;
      PCType type;

      ierr = PCGetType(pc_temp,&type);CHKERRQ(ierr);
      ierr = KSPGetPC(deluxe_ctx->seq_ksp,&new_pc);CHKERRQ(ierr);
      ierr = PCSetType(new_pc,type);CHKERRQ(ierr);
      ierr = PCFactorSetMatSolverPackage(new_pc,solver);CHKERRQ(ierr);
    }
    ierr = PetscStrlen(((PetscObject)(pcbddc->ksp_D))->prefix,&len);CHKERRQ(ierr);
    len -= 10; /* remove "dirichlet_" */
    ierr = PetscStrncpy(ksp_prefix,((PetscObject)(pcbddc->ksp_D))->prefix,len+1);CHKERRQ(ierr);
    ierr = PetscStrcat(ksp_prefix,"deluxe_");CHKERRQ(ierr);
    ierr = KSPSetOptionsPrefix(deluxe_ctx->seq_ksp,ksp_prefix);CHKERRQ(ierr);
    ierr = KSPSetFromOptions(deluxe_ctx->seq_ksp);CHKERRQ(ierr);
    ierr = KSPSetUp(deluxe_ctx->seq_ksp);CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}

PetscErrorCode DMAKKTGetFieldDecomposition(DM dm, PetscInt *n, char*** names, IS **iss, DM **dms) {
  PetscBool iskkt;
  DM_AKKT *kkt = (DM_AKKT*)(dm->data);
  PetscInt i;
  PetscErrorCode ierr;
  PetscFunctionBegin;
  PetscValidHeaderSpecific(dm, DM_CLASSID,1);
  PetscValidPointer(names,3);
  PetscValidPointer(iss,4);
  PetscValidPointer(dms,5);
  ierr = PetscObjectTypeCompare((PetscObject)dm, DMAKKT, &iskkt); CHKERRQ(ierr);
  if(!iskkt) SETERRQ(((PetscObject)dm)->comm, PETSC_ERR_ARG_WRONG, "DM not of type DMAKKT");
  if(n) *n = 2;
  if(names) {
    if(kkt->names) {
      ierr = PetscMalloc(sizeof(char*)*2, names); CHKERRQ(ierr);
    }
    else {
      *names = PETSC_NULL;
    }
  }
  if(iss) {
    if(kkt->isf) {
      ierr = PetscMalloc(sizeof(IS)*2, iss); CHKERRQ(ierr);
    }
    else {
      *iss = PETSC_NULL;
    }
  }
  if(dms) {
    if(kkt->dmf) {
      ierr = PetscMalloc(sizeof(DM)*2, dms); CHKERRQ(ierr);
    }
    else {
      *dms = PETSC_NULL;
    }
  }
  for(i = 0; i < 2; ++i) {
    if(names && kkt->names){ 
      ierr = PetscStrallocpy(kkt->names[i],(*names)+i); CHKERRQ(ierr);
    }
    if(iss && kkt->isf) {
      ierr = PetscObjectReference((PetscObject)kkt->isf[i]); CHKERRQ(ierr);
      (*iss)[i] = kkt->isf[i];
    }
    if(dms && kkt->dmf) {
      ierr = PetscObjectReference((PetscObject)kkt->dmf[i]); CHKERRQ(ierr);
      (*dms)[i] = kkt->dmf[i];
    }
  }
  PetscFunctionReturn(0);
}

/*@
   TaoSetStateDesignIS - Indicate to the Tao which variables in the
   solution vector are state variables and which are design.  Only applies to
   pde-constrained optimization.

   Logically Collective on Tao

   Input Parameters:
+  tao - The Tao context
.  s_is - the index set corresponding to the state variables
-  d_is - the index set corresponding to the design variables

   Level: intermediate

.seealso: TaoSetJacobianStateRoutine(), TaoSetJacobianDesignRoutine()
@*/
PetscErrorCode TaoSetStateDesignIS(Tao tao, IS s_is, IS d_is)
{
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = PetscObjectReference((PetscObject)s_is);CHKERRQ(ierr);
  ierr = ISDestroy(&tao->state_is);CHKERRQ(ierr);
  tao->state_is = s_is;
  ierr = PetscObjectReference((PetscObject)(d_is));CHKERRQ(ierr);
  ierr = ISDestroy(&tao->design_is);CHKERRQ(ierr);
  tao->design_is = d_is;
  PetscFunctionReturn(0);
}

/*@
      MatSchurComplementSet - Sets the matrices that define the Schur complement

   Collective on Mat

   Input Parameter:
+   N - matrix obtained with MatCreate() and MatSetType(MATSCHURCOMPLEMENT);
-   A00,A01,A10,A11  - the four parts of the original matrix (A00 is optional)

   Level: intermediate

   Notes: The Schur complement is NOT actually formed! Rather this
          object performs the matrix-vector product by using the the formula for
          the Schur complement and a KSP solver to approximate the action of inv(A)

          All four matrices must have the same MPI communicator

          A00 and  A11 must be square matrices

.seealso: MatCreateNormal(), MatMult(), MatCreate(), MatSchurComplementGetKSP(), MatSchurComplementUpdate(), MatCreateTranspose(), MatGetSchurComplement()

@*/
PetscErrorCode  MatSchurComplementSet(Mat N,Mat A00,Mat Ap00,Mat A01,Mat A10,Mat A11)
{
  PetscErrorCode       ierr;
  PetscInt             m,n;
  Mat_SchurComplement  *Na = (Mat_SchurComplement*)N->data;

  PetscFunctionBegin;
  if (N->assembled) SETERRQ(((PetscObject)N)->comm,PETSC_ERR_ARG_WRONGSTATE,"Use MatSchurComplementUpdate() for already used matrix");
  PetscValidHeaderSpecific(A00,MAT_CLASSID,1);
  PetscValidHeaderSpecific(Ap00,MAT_CLASSID,2);
  PetscValidHeaderSpecific(A01,MAT_CLASSID,3);
  PetscValidHeaderSpecific(A10,MAT_CLASSID,4);
  PetscCheckSameComm(A00,1,Ap00,2);
  PetscCheckSameComm(A00,1,A01,3);
  PetscCheckSameComm(A00,1,A10,4);
  if (A00->rmap->n != A00->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A00 %D do not equal local columns %D",A00->rmap->n,A00->cmap->n);
  if (A00->rmap->n != Ap00->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A00 %D do not equal local rows of Ap00 %D",A00->rmap->n,Ap00->rmap->n);
  if (Ap00->rmap->n != Ap00->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of Ap00 %D do not equal local columns %D",Ap00->rmap->n,Ap00->cmap->n);
  if (A00->cmap->n != A01->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local columns of A00 %D do not equal local rows of A01 %D",A00->cmap->n,A01->rmap->n);
  if (A10->cmap->n != A00->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local columns of A10 %D do not equal local rows of A00 %D",A10->cmap->n,A00->rmap->n);
  if (A11) {
    PetscValidHeaderSpecific(A11,MAT_CLASSID,5);
    PetscCheckSameComm(A00,1,A11,5);
    if (A11->rmap->n != A11->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A11 %D do not equal local columns %D",A11->rmap->n,A11->cmap->n);
    if (A10->rmap->n != A11->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A10 %D do not equal local rows A11 %D",A10->rmap->n,A11->rmap->n);
  }

  ierr = MatGetLocalSize(A01,PETSC_NULL,&n);CHKERRQ(ierr);
  ierr = MatGetLocalSize(A10,&m,PETSC_NULL);CHKERRQ(ierr);
  ierr = MatSetSizes(N,m,n,PETSC_DECIDE,PETSC_DECIDE);CHKERRQ(ierr);
  ierr      = PetscObjectReference((PetscObject)A00);CHKERRQ(ierr);
  ierr      = PetscObjectReference((PetscObject)Ap00);CHKERRQ(ierr);
  ierr      = PetscObjectReference((PetscObject)A01);CHKERRQ(ierr);
  ierr      = PetscObjectReference((PetscObject)A10);CHKERRQ(ierr);
  Na->A     = A00;
  Na->Ap    = Ap00;
  Na->B     = A01;
  Na->C     = A10;
  Na->D     = A11;
  if (A11) {
    ierr = PetscObjectReference((PetscObject)A11);CHKERRQ(ierr);
  }
  N->assembled           = PETSC_TRUE;
  N->preallocated        = PETSC_TRUE;

  ierr = PetscLayoutSetUp((N)->rmap);CHKERRQ(ierr);
  ierr = PetscLayoutSetUp((N)->cmap);CHKERRQ(ierr);
  ierr = KSPSetOperators(Na->ksp,A00,Ap00,SAME_NONZERO_PATTERN);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode SNESNASMSetSubdomains_NASM(SNES snes,PetscInt n,SNES subsnes[],VecScatter iscatter[],VecScatter oscatter[],VecScatter gscatter[])
{
  PetscInt       i;
  PetscErrorCode ierr;
  SNES_NASM      *nasm = (SNES_NASM*)snes->data;

  PetscFunctionBegin;
  if (snes->setupcalled) SETERRQ(PetscObjectComm((PetscObject)snes),PETSC_ERR_ARG_WRONGSTATE,"SNESNASMSetSubdomains() should be called before calling SNESSetUp().");

  /* tear down the previously set things */
  ierr = SNESReset(snes);CHKERRQ(ierr);

  nasm->n = n;
  if (oscatter) {
    for (i=0; i<n; i++) {ierr = PetscObjectReference((PetscObject)oscatter[i]);CHKERRQ(ierr);}
  }
  if (iscatter) {
    for (i=0; i<n; i++) {ierr = PetscObjectReference((PetscObject)iscatter[i]);CHKERRQ(ierr);}
  }
  if (gscatter) {
    for (i=0; i<n; i++) {ierr = PetscObjectReference((PetscObject)gscatter[i]);CHKERRQ(ierr);}
  }
  if (oscatter) {
    ierr = PetscMalloc(n*sizeof(IS),&nasm->oscatter);CHKERRQ(ierr);
    for (i=0; i<n; i++) {
      nasm->oscatter[i] = oscatter[i];
    }
  }
  if (iscatter) {
    ierr = PetscMalloc(n*sizeof(IS),&nasm->iscatter);CHKERRQ(ierr);
    for (i=0; i<n; i++) {
      nasm->iscatter[i] = iscatter[i];
    }
  }
  if (gscatter) {
    ierr = PetscMalloc(n*sizeof(IS),&nasm->gscatter);CHKERRQ(ierr);
    for (i=0; i<n; i++) {
      nasm->gscatter[i] = gscatter[i];
    }
  }

  if (subsnes) {
    ierr = PetscMalloc(n*sizeof(SNES),&nasm->subsnes);CHKERRQ(ierr);
    for (i=0; i<n; i++) {
      nasm->subsnes[i] = subsnes[i];
    }
    nasm->same_local_solves = PETSC_FALSE;
  }
  PetscFunctionReturn(0);
}

PetscErrorCode MatSMFResetRowColumn(Mat mat,IS Rows,IS Cols){
  MatSubMatFreeCtx ctx;
  PetscErrorCode   ierr;

  PetscFunctionBegin;
  ierr = MatShellGetContext(mat,(void **)&ctx);CHKERRQ(ierr);
  ierr = ISDestroy(&ctx->Rows);CHKERRQ(ierr);
  ierr = ISDestroy(&ctx->Cols);CHKERRQ(ierr);
  ierr = PetscObjectReference((PetscObject)Rows);CHKERRQ(ierr);
  ierr = PetscObjectReference((PetscObject)Cols);CHKERRQ(ierr);
  ctx->Cols=Cols;
  ctx->Rows=Rows;
  PetscFunctionReturn(0);
}

/*@
      MatSchurComplementUpdate - Updates the Schur complement matrix object with new submatrices

   Collective on Mat

   Input Parameters:
+   N - the matrix obtained with MatCreateSchurComplement()
.   A,B,C,D  - the four parts of the original matrix (D is optional)
-   str - either SAME_NONZERO_PATTERN,DIFFERENT_NONZERO_PATTERN,SAME_PRECONDITIONER


   Level: intermediate

   Notes: All four matrices must have the same MPI communicator

          A and  D must be square matrices

          All of the matrices provided must have the same sizes as was used with MatCreateSchurComplement() or MatSchurComplementSet()
          though they need not be the same matrices

.seealso: MatCreateNormal(), MatMult(), MatCreate(), MatSchurComplementGetKSP(), MatCreateSchurComplement()

@*/
PetscErrorCode  MatSchurComplementUpdate(Mat N,Mat A,Mat Ap,Mat B,Mat C,Mat D,MatStructure str)
{
  PetscErrorCode       ierr;
  Mat_SchurComplement  *Na = (Mat_SchurComplement*)N->data;

  PetscFunctionBegin;
 if (!N->assembled) SETERRQ(((PetscObject)N)->comm,PETSC_ERR_ARG_WRONGSTATE,"Use MatSchurComplementSet() for new matrix");
  PetscValidHeaderSpecific(A,MAT_CLASSID,1);
  PetscValidHeaderSpecific(B,MAT_CLASSID,2);
  PetscValidHeaderSpecific(C,MAT_CLASSID,3);
  PetscCheckSameComm(A,1,Ap,2);
  PetscCheckSameComm(A,1,B,3);
  PetscCheckSameComm(A,1,C,4);
  if (A->rmap->n != A->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A %D do not equal local columns %D",A->rmap->n,A->cmap->n);
  if (A->rmap->n != Ap->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of A %D do not equal local rows of Ap %D",A->rmap->n,Ap->rmap->n);
  if (Ap->rmap->n != Ap->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of Ap %D do not equal local columns %D",Ap->rmap->n,Ap->cmap->n);
  if (A->cmap->n != B->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local columns of A %D do not equal local rows of B %D",A->cmap->n,B->rmap->n);
  if (C->cmap->n != A->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local columns of C %D do not equal local rows of A %D",C->cmap->n,A->rmap->n);
  if (D) {
    PetscValidHeaderSpecific(D,MAT_CLASSID,5);
    PetscCheckSameComm(A,1,D,5);
    if (D->rmap->n != D->cmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of D %D do not equal local columns %D",D->rmap->n,D->cmap->n);
    if (C->rmap->n != D->rmap->n) SETERRQ2(PETSC_COMM_SELF,PETSC_ERR_ARG_SIZ,"Local rows of C %D do not equal local rows D %D",C->rmap->n,D->rmap->n);
  }

  ierr   = PetscObjectReference((PetscObject)A);CHKERRQ(ierr);
  ierr   = PetscObjectReference((PetscObject)Ap);CHKERRQ(ierr);
  ierr   = PetscObjectReference((PetscObject)B);CHKERRQ(ierr);
  ierr   = PetscObjectReference((PetscObject)C);CHKERRQ(ierr);
  if (D) {
    ierr = PetscObjectReference((PetscObject)D);CHKERRQ(ierr);
  }

  ierr   = MatDestroy(&Na->A);CHKERRQ(ierr);
  ierr   = MatDestroy(&Na->Ap);CHKERRQ(ierr);
  ierr   = MatDestroy(&Na->B);CHKERRQ(ierr);
  ierr   = MatDestroy(&Na->C);CHKERRQ(ierr);
  ierr   = MatDestroy(&Na->D);CHKERRQ(ierr);

  Na->A  = A;
  Na->Ap = Ap;
  Na->B  = B;
  Na->C  = C;
  Na->D  = D;

  ierr = KSPSetOperators(Na->ksp,A,Ap,str);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/*@C
  MatCreateSubMatrixFree - Creates a reduced matrix by masking a
  full matrix.

   Collective on matrix

   Input Parameters:
+  mat - matrix of arbitrary type
.  Rows - the rows that will be in the submatrix
-  Cols - the columns that will be in the submatrix

   Output Parameters:
.  J - New matrix

   Notes:
   The user provides the input data and is responsible for destroying
   this data after matrix J has been destroyed.

   Level: developer

.seealso: MatCreate()
@*/
PetscErrorCode MatCreateSubMatrixFree(Mat mat,IS Rows, IS Cols, Mat *J)
{
  MPI_Comm         comm=PetscObjectComm((PetscObject)mat);
  MatSubMatFreeCtx ctx;
  PetscErrorCode   ierr;
  PetscMPIInt      size;
  PetscInt         mloc,nloc,m,n;

  PetscFunctionBegin;
  ierr = PetscNew(&ctx);CHKERRQ(ierr);
  ctx->A=mat;
  ierr = MatGetSize(mat,&m,&n);CHKERRQ(ierr);
  ierr = MatGetLocalSize(mat,&mloc,&nloc);CHKERRQ(ierr);
  ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
  if (size == 1) {
    ierr = VecCreateSeq(comm,n,&ctx->VC);CHKERRQ(ierr);
  } else {
    ierr = VecCreateMPI(comm,nloc,n,&ctx->VC);CHKERRQ(ierr);
  }
  ctx->VR=ctx->VC;
  ierr =  PetscObjectReference((PetscObject)mat);CHKERRQ(ierr);


  ctx->Rows = Rows;
  ctx->Cols = Cols;
  ierr = PetscObjectReference((PetscObject)Rows);CHKERRQ(ierr);
  ierr = PetscObjectReference((PetscObject)Cols);CHKERRQ(ierr);
  ierr = MatCreateShell(comm,mloc,nloc,m,n,ctx,J);CHKERRQ(ierr);

  ierr = MatShellSetOperation(*J,MATOP_MULT,(void(*)(void))MatMult_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_DESTROY,(void(*)(void))MatDestroy_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_VIEW,(void(*)(void))MatView_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_MULT_TRANSPOSE,(void(*)(void))MatMultTranspose_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_DIAGONAL_SET,(void(*)(void))MatDiagonalSet_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_SHIFT,(void(*)(void))MatShift_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_EQUAL,(void(*)(void))MatEqual_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_SCALE,(void(*)(void))MatScale_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_TRANSPOSE,(void(*)(void))MatTranspose_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_GET_DIAGONAL,(void(*)(void))MatGetDiagonal_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_GET_SUBMATRICES,(void(*)(void))MatGetSubMatrices_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_NORM,(void(*)(void))MatNorm_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_DUPLICATE,(void(*)(void))MatDuplicate_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_GET_SUBMATRIX,(void(*)(void))MatGetSubMatrix_SMF);CHKERRQ(ierr);
  ierr = MatShellSetOperation(*J,MATOP_GET_ROW_MAX,(void(*)(void))MatDuplicate_SMF);CHKERRQ(ierr);

  ierr = PetscLogObjectParent((PetscObject)mat,(PetscObject)(*J));CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/*
     DMSetVI - Marks a DM as associated with a VI problem. This causes the interpolation/restriction operators to
               be restricted to only those variables NOT associated with active constraints.

*/
PetscErrorCode  DMSetVI(DM dm,IS inactive)
{
  PetscErrorCode          ierr;
  PetscContainer          isnes;
  DM_SNESVI               *dmsnesvi;

  PetscFunctionBegin;
  if (!dm) PetscFunctionReturn(0);

  ierr = PetscObjectReference((PetscObject)inactive);CHKERRQ(ierr);

  ierr = PetscObjectQuery((PetscObject)dm,"VI",(PetscObject *)&isnes);CHKERRQ(ierr);
  if (!isnes) {
    ierr = PetscContainerCreate(((PetscObject)dm)->comm,&isnes);CHKERRQ(ierr);
    ierr = PetscContainerSetUserDestroy(isnes,(PetscErrorCode (*)(void*))DMDestroy_SNESVI);CHKERRQ(ierr);
    ierr = PetscNew(DM_SNESVI,&dmsnesvi);CHKERRQ(ierr);
    ierr = PetscContainerSetPointer(isnes,(void*)dmsnesvi);CHKERRQ(ierr);
    ierr = PetscObjectCompose((PetscObject)dm,"VI",(PetscObject)isnes);CHKERRQ(ierr);
    ierr = PetscContainerDestroy(&isnes);CHKERRQ(ierr);
    dmsnesvi->createinterpolation   = dm->ops->createinterpolation;
    dm->ops->createinterpolation    = DMCreateInterpolation_SNESVI;
    dmsnesvi->coarsen            = dm->ops->coarsen;
    dm->ops->coarsen             = DMCoarsen_SNESVI;
    dmsnesvi->createglobalvector = dm->ops->createglobalvector;
    dm->ops->createglobalvector  = DMCreateGlobalVector_SNESVI;
  } else {
    ierr = PetscContainerGetPointer(isnes,(void**)&dmsnesvi);CHKERRQ(ierr);
    ierr = ISDestroy(&dmsnesvi->inactive);CHKERRQ(ierr);
  }
  ierr = DMClearGlobalVectors(dm);CHKERRQ(ierr);
  ierr = ISGetLocalSize(inactive,&dmsnesvi->n);CHKERRQ(ierr);
  dmsnesvi->inactive = inactive;
  dmsnesvi->dm       = dm;
  PetscFunctionReturn(0);
}

/*@
      MatCreateTranspose - Creates a new matrix object that behaves like A'

   Collective on Mat

   Input Parameter:
.   A  - the (possibly rectangular) matrix

   Output Parameter:
.   N - the matrix that represents A'

   Level: intermediate

   Notes:
    The transpose A' is NOT actually formed! Rather the new matrix
          object performs the matrix-vector product by using the MatMultTranspose() on
          the original matrix

.seealso: MatCreateNormal(), MatMult(), MatMultTranspose(), MatCreate()

@*/
PetscErrorCode  MatCreateTranspose(Mat A,Mat *N)
{
  PetscErrorCode ierr;
  PetscInt       m,n;
  Mat_Transpose  *Na;

  PetscFunctionBegin;
  ierr = MatGetLocalSize(A,&m,&n);CHKERRQ(ierr);
  ierr = MatCreate(PetscObjectComm((PetscObject)A),N);CHKERRQ(ierr);
  ierr = MatSetSizes(*N,n,m,PETSC_DECIDE,PETSC_DECIDE);CHKERRQ(ierr);
  ierr = PetscLayoutSetUp((*N)->rmap);CHKERRQ(ierr);
  ierr = PetscLayoutSetUp((*N)->cmap);CHKERRQ(ierr);
  ierr = PetscObjectChangeTypeName((PetscObject)*N,MATTRANSPOSEMAT);CHKERRQ(ierr);

  ierr       = PetscNewLog(*N,&Na);CHKERRQ(ierr);
  (*N)->data = (void*) Na;
  ierr       = PetscObjectReference((PetscObject)A);CHKERRQ(ierr);
  Na->A      = A;

  (*N)->ops->destroy          = MatDestroy_Transpose;
  (*N)->ops->mult             = MatMult_Transpose;
  (*N)->ops->multadd          = MatMultAdd_Transpose;
  (*N)->ops->multtranspose    = MatMultTranspose_Transpose;
  (*N)->ops->multtransposeadd = MatMultTransposeAdd_Transpose;
  (*N)->ops->duplicate        = MatDuplicate_Transpose;
  (*N)->ops->getvecs          = MatCreateVecs_Transpose;
  (*N)->ops->axpy             = MatAXPY_Transpose;
  (*N)->assembled             = PETSC_TRUE;

  ierr = PetscObjectComposeFunction((PetscObject)(*N),"MatTransposeGetMat_C",MatTransposeGetMat_Transpose);CHKERRQ(ierr);
  ierr = MatSetBlockSizes(*N,PetscAbs(A->cmap->bs),PetscAbs(A->rmap->bs));CHKERRQ(ierr);
  ierr = MatSetUp(*N);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

static PetscErrorCode VecSetUp_Nest_Private(Vec V,PetscInt nb,Vec x[])
{
  Vec_Nest       *ctx = (Vec_Nest*)V->data;
  PetscInt       i;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  if (ctx->setup_called) PetscFunctionReturn(0);

  ctx->nb = nb;
  if (ctx->nb < 0) SETERRQ(PetscObjectComm((PetscObject)V),PETSC_ERR_ARG_WRONG,"Cannot create VECNEST with < 0 blocks.");

  /* Create space */
  ierr = PetscMalloc1(ctx->nb,&ctx->v);CHKERRQ(ierr);
  for (i=0; i<ctx->nb; i++) {
    ctx->v[i] = x[i];
    ierr = PetscObjectReference((PetscObject)x[i]);CHKERRQ(ierr);
    /* Do not allocate memory for internal sub blocks */
  }

  ierr = PetscMalloc1(ctx->nb,&ctx->is);CHKERRQ(ierr);

  ctx->setup_called = PETSC_TRUE;
  PetscFunctionReturn(0);
}

/*@
  DMPlexInterpolate - Take in a cell-vertex mesh and return one with all intermediate faces, edges, etc.

  Collective on DM

  Input Parameter:
. dm - The DMPlex object with only cells and vertices

  Output Parameter:
. dmInt - The complete DMPlex object

  Level: intermediate

.keywords: mesh
.seealso: DMPlexUninterpolate(), DMPlexCreateFromCellList()
@*/
PetscErrorCode DMPlexInterpolate(DM dm, DM *dmInt)
{
  DM             idm, odm = dm;
  PetscInt       depth, dim, d;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = PetscLogEventBegin(DMPLEX_Interpolate,dm,0,0,0);CHKERRQ(ierr);
  ierr = DMPlexGetDepth(dm, &depth);CHKERRQ(ierr);
  ierr = DMPlexGetDimension(dm, &dim);CHKERRQ(ierr);
  if (dim <= 1) {
    ierr = PetscObjectReference((PetscObject) dm);CHKERRQ(ierr);
    idm  = dm;
  }
  for (d = 1; d < dim; ++d) {
    /* Create interpolated mesh */
    ierr = DMCreate(PetscObjectComm((PetscObject)dm), &idm);CHKERRQ(ierr);
    ierr = DMSetType(idm, DMPLEX);CHKERRQ(ierr);
    ierr = DMPlexSetDimension(idm, dim);CHKERRQ(ierr);
    if (depth > 0) {ierr = DMPlexInterpolateFaces_Internal(odm, 1, idm);CHKERRQ(ierr);}
    if (odm != dm) {ierr = DMDestroy(&odm);CHKERRQ(ierr);}
    odm  = idm;
  }
  *dmInt = idm;
  ierr = PetscLogEventEnd(DMPLEX_Interpolate,dm,0,0,0);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/*@
    VecGhostGetLocalForm - Obtains the local ghosted representation of
    a parallel vector (obtained with VecCreateGhost(), VecCreateGhostWithArray()
    or VecCreateSeq()). Returns PETSC_NULL if the Vec is not ghosted.

    Not Collective

    Input Parameter:
.   g - the global vector

    Output Parameter:
.   l - the local (ghosted) representation, PETSC_NULL if g is not ghosted

    Notes:
    This routine does not actually update the ghost values, but rather it
    returns a sequential vector that includes the locations for the ghost
    values and their current values. The returned vector and the original
    vector passed in share the same array that contains the actual vector data.

    To update the ghost values from the locations on the other processes one must call
    VecGhostUpdateBegin() and VecGhostUpdateEnd() before accessing the ghost values. Thus normal
    usage is
$     VecGhostUpdateBegin(x,INSERT_VALUES,SCATTER_FORWARD);
$     VecGhostUpdateEnd(x,INSERT_VALUES,SCATTER_FORWARD);
$     VecGhostGetLocalForm(x,&xlocal);
$     VecGetArray(xlocal,&xvalues);
$        // access the non-ghost values in locations xvalues[0:n-1] and ghost values in locations xvalues[n:n+nghost];
$     VecRestoreArray(xlocal,&xvalues);
$     VecGhostRestoreLocalForm(x,&xlocal);

    One should call VecGhostRestoreLocalForm() or VecDestroy() once one is
    finished using the object.

    Level: advanced

   Concepts: vectors^ghost point access

.seealso: VecCreateGhost(), VecGhostRestoreLocalForm(), VecCreateGhostWithArray()

@*/
PetscErrorCode  VecGhostGetLocalForm(Vec g,Vec *l)
{
  PetscErrorCode ierr;
  PetscBool      isseq,ismpi;

  PetscFunctionBegin;
  PetscValidHeaderSpecific(g,VEC_CLASSID,1);
  PetscValidPointer(l,2);

  ierr = PetscObjectTypeCompare((PetscObject)g,VECSEQ,&isseq);CHKERRQ(ierr);
  ierr = PetscObjectTypeCompare((PetscObject)g,VECMPI,&ismpi);CHKERRQ(ierr);
  if (ismpi) {
    Vec_MPI *v  = (Vec_MPI*)g->data;
    *l = v->localrep;
  } else if (isseq) {
    *l = g;
  } else {
    *l = PETSC_NULL;
  }
  if (*l) {
    ierr = VecGhostStateSync_Private(g,*l);CHKERRQ(ierr);
    ierr = PetscObjectReference((PetscObject)*l);CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}