Index: /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.cpp
===================================================================
--- /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.cpp	(revision 18344)
+++ /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.cpp	(revision 18345)
@@ -805,4 +805,24 @@
 	*pMlff=Mlff;
 }/*}}}*/
+void           MasstransportAnalysis::MassMatrix(Matrix<IssmDouble>** pMff,FemModel* femmodel){/*{{{*/
+
+	/*Initialize Mass matrix*/
+	Matrix<IssmDouble> *Mff = NULL;
+	AllocateSystemMatricesx(&Mff,NULL,NULL,NULL,femmodel);
+
+	/*Create and assemble matrix*/
+	for(int i=0;i<femmodel->elements->Size();i++){
+		Element*       element = dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
+		ElementMatrix* MLe     = this->CreateMassMatrix(element);
+		if(MLe){
+			MLe->AddToGlobal(Mff);
+		}
+		delete MLe;
+	}
+	Mff->Assemble();
+
+	/*Assign output pointer*/
+	*pMff=Mff;
+}/*}}}*/
 ElementMatrix* MasstransportAnalysis::CreateMassMatrix(Element* element){/*{{{*/
 
@@ -866,5 +886,10 @@
 	/*Assign output pointer*/
 	*pKff=Kff;
-	*pKfs=Kfs;
+	if(pKfs){
+		*pKfs=Kfs;
+	}
+	else{
+		delete Kfs;
+	}
 }/*}}}*/
 ElementMatrix* MasstransportAnalysis::CreateFctKMatrix(Element* element){/*{{{*/
Index: /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.h
===================================================================
--- /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.h	(revision 18344)
+++ /issm/trunk-jpl/src/c/analyses/MasstransportAnalysis.h	(revision 18345)
@@ -39,4 +39,5 @@
 		/*FCT*/
 		void           LumpedMassMatrix(Vector<IssmDouble>** pMLff,FemModel* femmodel);
+		void           MassMatrix(Matrix<IssmDouble>** pMff,FemModel* femmodel);
 		void           FctKMatrix(Matrix<IssmDouble>** pKff,Matrix<IssmDouble>** pKfs,FemModel* femmodel);
 		ElementMatrix* CreateMassMatrix(Element* element);
Index: /issm/trunk-jpl/src/c/solutionsequences/solutionsequence_fct.cpp
===================================================================
--- /issm/trunk-jpl/src/c/solutionsequences/solutionsequence_fct.cpp	(revision 18344)
+++ /issm/trunk-jpl/src/c/solutionsequences/solutionsequence_fct.cpp	(revision 18345)
@@ -11,10 +11,10 @@
 
 	/*intermediary: */
-	Vector<IssmDouble>*  Mlf = NULL;
-	Matrix<IssmDouble>*  Kff = NULL;
-	Matrix<IssmDouble>*  Kfs = NULL;
-	Vector<IssmDouble>*  ug  = NULL;
-	Vector<IssmDouble>*  uf  = NULL;
-	Vector<IssmDouble>*  ys  = NULL;
+	Vector<IssmDouble>*  Ml = NULL;
+	Matrix<IssmDouble>*  K  = NULL;
+	Matrix<IssmDouble>*  Mc = NULL;
+	Vector<IssmDouble>*  ug = NULL;
+	Vector<IssmDouble>*  uf = NULL;
+	Vector<IssmDouble>*  ys = NULL;
 
 	IssmDouble theta,deltat;
@@ -41,41 +41,43 @@
 
 	/*Create lumped mass matrix*/
-	analysis->LumpedMassMatrix(&Mlf,femmodel);
-	analysis->FctKMatrix(&Kff,&Kfs,femmodel);
-
-	/*Create Dff Matrix*/
+	analysis->LumpedMassMatrix(&Ml,femmodel);
+	analysis->MassMatrix(&Mc,femmodel);
+	analysis->FctKMatrix(&K,NULL,femmodel);
+
+	/*Create D Matrix*/
 	#ifdef _HAVE_PETSC_
-	Mat Kff_transp = NULL;
-	Mat Dff_petsc  = NULL;
-	Mat LHS        = NULL;
-	Mat Kff_petsc  = Kff->pmatrix->matrix;
-	Vec Mlf_petsc  = Mlf->pvector->vector;
-	MatTranspose(Kff_petsc,MAT_INITIAL_MATRIX,&Kff_transp);
-	MatDuplicate(Kff_petsc,MAT_SHARE_NONZERO_PATTERN,&Dff_petsc);
-	MatGetOwnershipRange(Kff_transp,&rstart,&rend);
+	Mat K_transp = NULL;
+	Mat D_petsc  = NULL;
+	Mat LHS      = NULL;
+	Mat K_petsc  = K->pmatrix->matrix;
+	Vec Ml_petsc = Ml->pvector->vector;
+	Mat Mc_petsc = Mc->pmatrix->matrix;
+	MatTranspose(K_petsc,MAT_INITIAL_MATRIX,&K_transp);
+	MatDuplicate(K_petsc,MAT_SHARE_NONZERO_PATTERN,&D_petsc);
+	MatGetOwnershipRange(K_transp,&rstart,&rend);
 	for(int row=rstart; row<rend; row++){
 		diagD = 0.;
-		MatGetRow(Kff_petsc ,row,&ncols, (const int**)&cols, (const double**)&vals);
-		MatGetRow(Kff_transp,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+		MatGetRow(K_petsc ,row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatGetRow(K_transp,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
 		_assert_(ncols==ncols2);
 		for(int j=0; j<ncols; j++) {
 			_assert_(cols[j]==cols2[j]);
 			d = max(max(-vals[j],-vals2[j]),0.);
-			MatSetValues(Dff_petsc,1,&row,1,&cols[j],(const double*)&d,INSERT_VALUES);
+			MatSetValues(D_petsc,1,&row,1,&cols[j],(const double*)&d,INSERT_VALUES);
 			if(cols[j]!=row) diagD -= d;
 		}
-		MatSetValues(Dff_petsc,1,&row,1,&row,(const double*)&diagD,INSERT_VALUES);
-		MatRestoreRow(Kff_petsc, row,&ncols, (const int**)&cols, (const double**)&vals);
-		MatRestoreRow(Kff_transp,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
-	}
-	MatAssemblyBegin(Dff_petsc,MAT_FINAL_ASSEMBLY);
-	MatAssemblyEnd(  Dff_petsc,MAT_FINAL_ASSEMBLY);
-	MatFree(&Kff_transp);
+		MatSetValues(D_petsc,1,&row,1,&row,(const double*)&diagD,INSERT_VALUES);
+		MatRestoreRow(K_petsc, row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatRestoreRow(K_transp,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+	}
+	MatAssemblyBegin(D_petsc,MAT_FINAL_ASSEMBLY);
+	MatAssemblyEnd(  D_petsc,MAT_FINAL_ASSEMBLY);
+	MatFree(&K_transp);
 
 	/*Create LHS: [ML − theta*detlat *(K+D)^n+1]*/
-	MatDuplicate(Kff_petsc,MAT_SHARE_NONZERO_PATTERN,&LHS);
+	MatDuplicate(K_petsc,MAT_SHARE_NONZERO_PATTERN,&LHS);
 	for(int row=rstart; row<rend; row++){
-		MatGetRow(Kff_petsc,row,&ncols, (const int**)&cols, (const double**)&vals);
-		MatGetRow(Dff_petsc,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+		MatGetRow(K_petsc,row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatGetRow(D_petsc,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
 		_assert_(ncols==ncols2);
 		for(int j=0; j<ncols; j++) {
@@ -83,5 +85,5 @@
 			d = -theta*deltat*(vals[j] + vals2[j]);
 			if(cols[j]==row){
-				VecGetValues(Mlf_petsc,1,(const int*)&cols[j],&mi);
+				VecGetValues(Ml_petsc,1,(const int*)&cols[j],&mi);
 				d += mi;
 			}
@@ -89,6 +91,6 @@
 			MatSetValues(LHS,1,&row,1,&cols[j],(const double*)&d,INSERT_VALUES);
 		}
-		MatRestoreRow(Kff_petsc,row,&ncols, (const int**)&cols, (const double**)&vals);
-		MatRestoreRow(Dff_petsc,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+		MatRestoreRow(K_petsc,row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatRestoreRow(D_petsc,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
 	}
 
@@ -118,11 +120,10 @@
 	VecDuplicate(u,&Du);
 	VecDuplicate(u,&RHS);
-	MatMult(Kff_petsc,u,Ku);
-	MatMult(Dff_petsc,u,Du);
-	VecPointwiseMult(RHS,Mlf_petsc,u);
+	MatMult(K_petsc,u,Ku);
+	MatMult(D_petsc,u,Du);
+	VecPointwiseMult(RHS,Ml_petsc,u);
 	VecAXPBYPCZ(RHS,(1-theta)*deltat,(1-theta)*deltat,1,Ku,Du);// RHS = M*u + (1-theta)*deltat*K*u + (1-theta)*deltat*D*u
 	VecFree(&Ku);
 	VecFree(&Du);
-	MatFree(&Dff_petsc);
 	delete uf;
 
@@ -138,4 +139,6 @@
 		}
 	}
+	VecAssemblyBegin(RHS);
+	VecAssemblyEnd(  RHS);
 
 	/*Go solve!*/
@@ -143,4 +146,70 @@
 	MatFree(&LHS);
 	VecFree(&RHS);
+
+	/*Richardson to calculate udot*/
+	Vec udot = NULL;
+	VecDuplicate(u,&udot);
+	VecZeroEntries(udot);
+	Vec temp1 = NULL; VecDuplicate(u,&temp1);
+	Vec temp2 = NULL; VecDuplicate(u,&temp2);
+	for(int i=0;i<5;i++){
+		/*udot_new = udot_old + Ml^-1 (K^(n+1) u -- Mc udot_old)*/
+		MatMult(Mc_petsc,udot,temp1);
+		MatMult(K_petsc, u,   temp2);
+		VecAXPBY(temp2,-1.,1.,temp1); // temp2 = (K^(n+1) u -- Mc udot_old)
+		VecPointwiseDivide(temp1,temp2,Ml_petsc); //temp1 = Ml^-1 temp2
+		VecAXPBY(udot,1.,1.,temp1);
+	}
+	VecFree(&temp1);
+	VecFree(&temp2);
+
+	/*Serialize u and udot*/
+	IssmDouble* udot_serial = NULL;
+	IssmDouble* u_serial    = NULL;
+	VecToMPISerial(&udot_serial,udot,IssmComm::GetComm());
+	VecToMPISerial(&u_serial   ,u   ,IssmComm::GetComm());
+
+
+	/*Anti diffusive fluxes*/
+	Mat F    = NULL;
+	Vec Fbar = NULL;
+	VecDuplicate(u,&Fbar);
+	MatDuplicate(D_petsc,MAT_SHARE_NONZERO_PATTERN,&F);
+	for(int row=rstart; row<rend; row++){
+		MatGetRow(Mc_petsc,row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatGetRow(D_petsc ,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+		_assert_(ncols==ncols2);
+		mi = 0.;
+		for(int j=0; j<ncols; j++) {
+			_assert_(cols[j]==cols2[j]);
+			d = vals[j]*(udot_serial[row] - udot_serial[cols[j]]) + vals2[j]*(u_serial[row] - u_serial[cols[j]]);
+			if(row!=cols[j]) mi += d;
+			MatSetValues(F,1,&row,1,&cols[j],(const double*)&d,INSERT_VALUES);
+
+		}
+		VecSetValue(Fbar,row,(const double)mi,INSERT_VALUES);
+		MatRestoreRow(Mc_petsc, row,&ncols, (const int**)&cols, (const double**)&vals);
+		MatRestoreRow(D_petsc,row,&ncols2,(const int**)&cols2,(const double**)&vals2);
+	}
+	MatAssemblyBegin(F,MAT_FINAL_ASSEMBLY);
+	MatAssemblyEnd(  F,MAT_FINAL_ASSEMBLY);
+	VecAssemblyBegin(Fbar);
+	VecAssemblyEnd(  Fbar);
+
+	MatFree(&D_petsc);
+	MatFree(&F);
+	delete Mc;
+	xDelete<IssmDouble>(udot_serial);
+	xDelete<IssmDouble>(u_serial);
+
+	/*Compute solution u^n+1 = u_L + deltat Ml^-1 fbar*/
+	VecDuplicate(u,&temp1);
+	VecPointwiseDivide(temp1,Fbar,Ml_petsc); //temp1 = Ml^-1 temp2
+	VecAXPBY(udot,1.,1.,temp1);
+	VecAXPY(u,deltat,temp1);
+	VecFree(&Fbar);
+	VecFree(&udot);
+	VecFree(&temp1);
+
 	uf =new Vector<IssmDouble>(u);
 	VecFree(&u);
@@ -154,7 +223,6 @@
 	#endif
 
-	delete Mlf;
-	delete Kff;
-	delete Kfs;
+	delete Ml;
+	delete K;
 	delete analysis;