Index: /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.cpp
===================================================================
--- /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.cpp	(revision 5692)
+++ /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.cpp	(revision 5693)
@@ -4,5 +4,4 @@
 
 #include "./SystemMatricesx.h"
-
 #include "../../shared/shared.h"
 #include "../../include/include.h"
@@ -10,42 +9,37 @@
 #include "../../EnumDefinitions/EnumDefinitions.h"
 
-void SystemMatricesx(Mat* pKgg, Vec* ppg,Elements* elements,Nodes* nodes, Vertices* vertices,Loads* loads,Materials* materials, Parameters* parameters, int kflag,int pflag){
+void SystemMatricesx(Mat* pKgg, Vec* ppg,double* pkmax,Elements* elements,Nodes* nodes, Vertices* vertices,Loads* loads,Materials* materials, Parameters* parameters,bool kflag,bool pflag,bool penalty_kflag,bool penalty_pflag){
 	
 	/*intermediary: */
-	int gsize;
-	int i,j;
-	int connectivity;
-	int numberofdofspernode;
-	Element* element=NULL;
-	Load*    load=NULL;
+	int      i,j,gsize;
+	int      connectivity, numberofdofspernode;
+	int      analysis_type, configuration_type;
+	Element *element = NULL;
+	Load    *load    = NULL;
 	
 	/*output: */
-	Mat Kgg=NULL;
-	Vec pg=NULL;
-
-	int analysis_type;
-	int configuration_type;
-
-	/*Display message*/
-	int verbose; parameters->FindParam(&verbose,VerboseEnum);
-	if (verbose) printf("   Generating matrices\n");
+	Mat    Kgg  = NULL;
+	Vec    pg   = NULL;
+	double kmax = 0;
 
 	/*retrive parameters: */
 	parameters->FindParam(&analysis_type,AnalysisTypeEnum);
 	parameters->FindParam(&configuration_type,ConfigurationTypeEnum);
-
-	/*Recover parameters: */
 	parameters->FindParam(&connectivity,ConnectivityEnum);
 
 	/*Get size of matrix: */
 	gsize=nodes->NumberOfDofs(configuration_type);
-
-	/*Get numberofdofspernode: */
 	numberofdofspernode=nodes->MaxNumDofs(configuration_type);
 
-	/*Compute stiffness matrix*/
+	/*Checks in debugging mode {{{1*/
+	ISSMASSERT(*pKgg==NULL);
+	ISSMASSERT(*ppg==NULL);
+	if(penalty_kflag)ISSMASSERT(kflag);
+	if(penalty_pflag)ISSMASSERT(pflag);
+	/*}}}*/
+
+	/*Compute penalty free mstiffness matrix and load vector*/
 	if(kflag){
 
-		/*Allocate Kgg*/
 		Kgg=NewMat(gsize,gsize,NULL,&connectivity,&numberofdofspernode);
 
@@ -67,9 +61,6 @@
 		MatCompress(Kgg);
 	}
-
-	/*Compute Load vector*/
 	if(pflag){
 
-		/*Allocate pg*/
 		pg=NewVec(gsize);
 
@@ -86,13 +77,40 @@
 		}
 
-		/*Assemble right hand side: */
 		VecAssemblyBegin(pg);
 		VecAssemblyEnd(pg);
 	}
 
+	/*Now, deal with penalties*/
+	if(penalty_kflag){
+
+		/*Now, figure out maximum value of K_gg, so that we can penalize it correctly: */
+		MatNorm(Kgg,NORM_INFINITY,&kmax);
+
+		/*Fill stiffness matrix from loads: */
+		for (i=0;i<loads->Size();i++){
+			load=(Load*)loads->GetObjectByOffset(i);
+			if (load->InAnalysis(configuration_type)) load->PenaltyCreateKMatrix(Kgg,kmax);
+		}
+
+		/*Assemble matrix and compress matrix to save memory: */
+		MatAssemblyBegin(Kgg,MAT_FINAL_ASSEMBLY);
+		MatAssemblyEnd(Kgg,MAT_FINAL_ASSEMBLY);
+		MatCompress(Kgg);
+	}
+	if(penalty_pflag){
+
+		/*Fill right hand side vector, from loads: */
+		for (i=0;i<loads->Size();i++){
+			load=(Load*)loads->GetObjectByOffset(i);
+			if (load->InAnalysis(configuration_type)) load->PenaltyCreatePVector(pg,kmax);
+		}
+
+		VecAssemblyBegin(pg);
+		VecAssemblyEnd(pg);
+	}
 	
 	/*Assign output pointers: */
 	*pKgg=Kgg;
 	*ppg=pg;
-
+	if(pkmax) *pkmax=kmax;
 }
Index: /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.h
===================================================================
--- /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.h	(revision 5692)
+++ /issm/trunk/src/c/modules/SystemMatricesx/SystemMatricesx.h	(revision 5693)
@@ -10,6 +10,6 @@
 
 /* local prototypes: */
-void SystemMatricesx(Mat* pKgg, Vec* ppg,Elements* elements,Nodes* nodes, Vertices* vertices,Loads* loads,Materials* materials, Parameters* parameters, int kflag,int pflag);
+void SystemMatricesx(Mat* pKgg, Vec* ppg,double* pkmax,Elements* elements,Nodes* nodes, Vertices* vertices,Loads* loads,Materials* materials, Parameters* parameters,
+			bool kflag=true,bool pflag=true,bool penalty_kflag=true,bool penalty_pflag=true);
 
 #endif  /* _SYSTEMMATRICESX_H */
-
Index: /issm/trunk/src/c/solvers/solver_adjoint_linear.cpp
===================================================================
--- /issm/trunk/src/c/solvers/solver_adjoint_linear.cpp	(revision 5692)
+++ /issm/trunk/src/c/solvers/solver_adjoint_linear.cpp	(revision 5693)
@@ -26,7 +26,5 @@
 	Vec pf=NULL;
 
-	kflag=1; pflag=1;
-	SystemMatricesx(&Kgg, &pg,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-	PenaltySystemMatricesx(Kgg, pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
+	SystemMatricesx(&Kgg, &pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
 
 	Reducematrixfromgtofx(&Kff,&Kfs,Kgg,femmodel->nodesets,femmodel->parameters); MatFree(&Kgg);
Index: /issm/trunk/src/c/solvers/solver_diagnostic_nonlinear.cpp
===================================================================
--- /issm/trunk/src/c/solvers/solver_diagnostic_nonlinear.cpp	(revision 5692)
+++ /issm/trunk/src/c/solvers/solver_diagnostic_nonlinear.cpp	(revision 5693)
@@ -34,9 +34,7 @@
 
 	/*parameters:*/
-	int kflag,pflag;
 	int verbose=0;
 
 	/*Recover parameters: */
-	kflag=1; pflag=1;
 	femmodel->parameters->FindParam(&verbose,VerboseEnum);
 	femmodel->parameters->FindParam(&min_mechanical_constraints,MinMechanicalConstraintsEnum);
@@ -67,6 +65,5 @@
 		VecFree(&old_uf);old_uf=uf;
 
-		SystemMatricesx(&Kgg, &pg,femmodel->elements,femmodel->nodes,femmodel->vertices,loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-		PenaltySystemMatricesx(Kgg, pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,loads,femmodel->materials,femmodel->parameters,kflag,pflag);
+		SystemMatricesx(&Kgg, &pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,loads,femmodel->materials,femmodel->parameters);
 		
 		Reducematrixfromgtofx(&Kff,&Kfs,Kgg,femmodel->nodesets,femmodel->parameters);
Index: /issm/trunk/src/c/solvers/solver_linear.cpp
===================================================================
--- /issm/trunk/src/c/solvers/solver_linear.cpp	(revision 5692)
+++ /issm/trunk/src/c/solvers/solver_linear.cpp	(revision 5693)
@@ -9,8 +9,4 @@
 
 void solver_linear(FemModel* femmodel){
-
-	/*parameters:*/
-	int kflag,pflag;
-	int verbose=0;
 
 	/*output: */
@@ -25,10 +21,5 @@
 	Vec pf=NULL;
 
-	/*Recover parameters: */
-	kflag=1; pflag=1;
-	femmodel->parameters->FindParam(&verbose,VerboseEnum);
-
-	SystemMatricesx(&Kgg, &pg,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-	PenaltySystemMatricesx(Kgg, pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
+	SystemMatricesx(&Kgg,&pg,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
 
 	Reducematrixfromgtofx(&Kff,&Kfs,Kgg,femmodel->nodesets,femmodel->parameters); MatFree(&Kgg);
Index: /issm/trunk/src/c/solvers/solver_thermal_nonlinear.cpp
===================================================================
--- /issm/trunk/src/c/solvers/solver_thermal_nonlinear.cpp	(revision 5692)
+++ /issm/trunk/src/c/solvers/solver_thermal_nonlinear.cpp	(revision 5693)
@@ -54,24 +54,12 @@
 		InputUpdateFromConstantx( femmodel->elements,femmodel->nodes, femmodel->vertices, femmodel->loads, femmodel->materials, femmodel->parameters,reset_penalties,ResetPenaltiesEnum);
 
-		//*Generate system matrices
-		if (!lowmem){
-
-			/*Compute Kgg_nopenalty and pg_nopenalty once for all: */
-			if (count==1){
-				SystemMatricesx(&Kgg_nopenalty, &pg_nopenalty,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-			}
-
-			/*Copy K_gg_nopenalty into Kgg, same for pg: */
-			MatDuplicate(Kgg_nopenalty,MAT_COPY_VALUES,&Kgg);
-			VecDuplicatePatch(&pg,pg_nopenalty);
-
-			//apply penalties each time
-			PenaltySystemMatricesx(Kgg, pg,&melting_offset,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-		}
-		else{
-			SystemMatricesx(&Kgg, &pg,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-			//apply penalties
-			PenaltySystemMatricesx(Kgg, pg,&melting_offset,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
-		}
+		/*===================== DEBUGING ====================*/
+		if (count==1) SystemMatricesx(&Kgg_nopenalty,&pg_nopenalty,NULL,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
+		MatDuplicate(Kgg_nopenalty,MAT_COPY_VALUES,&Kgg);
+		VecDuplicatePatch(&pg,pg_nopenalty);
+		PenaltySystemMatricesx(Kgg, pg,&melting_offset,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters,kflag,pflag);
+		/*===================== DEBUGING ====================*/
+		//SystemMatricesx(&Kgg,&pg,&melting_offset,femmodel->elements,femmodel->nodes,femmodel->vertices,femmodel->loads,femmodel->materials,femmodel->parameters);
+		/*===================== DEBUGING ====================*/
 
 		Reducematrixfromgtofx(&Kff,&Kfs,Kgg,femmodel->nodesets,femmodel->parameters);
Index: /issm/trunk/src/m/solvers/solver_adjoint_linear.m
===================================================================
--- /issm/trunk/src/m/solvers/solver_adjoint_linear.m	(revision 5692)
+++ /issm/trunk/src/m/solvers/solver_adjoint_linear.m	(revision 5693)
@@ -5,8 +5,5 @@
 %      femmodel =solver_adjoint_linear(femmodel)
 
-	femmodel.parameters.Kflag=1; femmodel.parameters.Pflag=1;
-
-	[K_gg, p_g]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
-	[K_gg, p_g,kmax]=PenaltySystemMatrices(K_gg,p_g,femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
+	[K_gg, p_g, kmax]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
 	
 	[K_ff, K_fs] = Reducematrixfromgtof( K_gg, femmodel.nodesets,femmodel.parameters); 
Index: /issm/trunk/src/m/solvers/solver_diagnostic_nonlinear.m
===================================================================
--- /issm/trunk/src/m/solvers/solver_diagnostic_nonlinear.m	(revision 5692)
+++ /issm/trunk/src/m/solvers/solver_diagnostic_nonlinear.m	(revision 5693)
@@ -4,10 +4,6 @@
 %   Usage:
 %      [femmodel]=solver_diagnostic_nonlinear(femmodel,conserve_loads)
-	
 
 	%Recover parameters
-	femmodel.parameters.Kflag=1;
-	femmodel.parameters.Pflag=1;
-	dim=femmodel.parameters.Dim;
 	min_mechanical_constraints=femmodel.parameters.MinMechanicalConstraints;
 
@@ -31,6 +27,5 @@
 		old_uf=uf;
 		
-		[K_gg_nopenalty , p_g_nopenalty]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,loads,femmodel.materials,femmodel.parameters);
-		[K_gg , p_g, kmax]=PenaltySystemMatrices(K_gg_nopenalty,p_g_nopenalty,femmodel.elements,femmodel.nodes,femmodel.vertices,loads,femmodel.materials,femmodel.parameters);
+		[K_gg, p_g, kmax]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,loads,femmodel.materials,femmodel.parameters);
 		
 		[K_ff, K_fs] = Reducematrixfromgtof( K_gg, femmodel.nodesets,femmodel.parameters); 
Index: /issm/trunk/src/m/solvers/solver_linear.m
===================================================================
--- /issm/trunk/src/m/solvers/solver_linear.m	(revision 5692)
+++ /issm/trunk/src/m/solvers/solver_linear.m	(revision 5693)
@@ -5,8 +5,5 @@
 %      femmodel =solver_linear(femmodel)
 
-	femmodel.parameters.Kflag=1; femmodel.parameters.Pflag=1;
-
-	[K_gg, p_g]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
-	[K_gg, p_g,kmax]=PenaltySystemMatrices(K_gg,p_g,femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
+	[K_gg, p_g,kmax]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
 	
 	[K_ff, K_fs] = Reducematrixfromgtof( K_gg,  femmodel.nodesets,femmodel.parameters); 
Index: /issm/trunk/src/m/solvers/solver_thermal_nonlinear.m
===================================================================
--- /issm/trunk/src/m/solvers/solver_thermal_nonlinear.m	(revision 5692)
+++ /issm/trunk/src/m/solvers/solver_thermal_nonlinear.m	(revision 5693)
@@ -19,11 +19,12 @@
 		[femmodel.elements femmodel.loads]=InputUpdateFromConstant(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters,reset_penalties,ResetPenaltiesEnum);
 
-		%system matrices 
-		if count==1
-			displaystring(femmodel.parameters.Verbose,'%s',['   system matrices']);
-			[K_gg_nopenalty, p_g_nopenalty]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
+		% ================= DEBUG FOR NOW ====================
+		if count==1 
+			[K_gg_nopenalty,p_g_nopenalty,melting_offset]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
 		end
-		displaystring(femmodel.parameters.Verbose,'%s',['   penalty system matrices']);
 		[K_gg , p_g, melting_offset]=PenaltySystemMatrices(K_gg_nopenalty,p_g_nopenalty,femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
+		% ================= DEBUG FOR NOW ====================
+		%[K_gg,p_g,melting_offset]=SystemMatrices(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters);
+		% ================= DEBUG FOR NOW ====================
 
 		[K_ff, K_fs] = Reducematrixfromgtof( K_gg, femmodel.nodesets,femmodel.parameters); 
@@ -35,5 +36,5 @@
 		t_g= Mergesolutionfromftog( t_f, femmodel.ys, femmodel.nodesets,femmodel.parameters); 
 		[femmodel.elements,femmodel.materials]=InputUpdateFromSolution(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads,femmodel.materials,femmodel.parameters,t_g);
-	
+
 		[femmodel.loads,constraints_converged,num_unstable_constraints] =PenaltyConstraints(femmodel.elements,femmodel.nodes,femmodel.vertices,femmodel.loads, femmodel.materials,femmodel.parameters);
 	
Index: /issm/trunk/src/mex/SystemMatrices/SystemMatrices.cpp
===================================================================
--- /issm/trunk/src/mex/SystemMatrices/SystemMatrices.cpp	(revision 5692)
+++ /issm/trunk/src/mex/SystemMatrices/SystemMatrices.cpp	(revision 5693)
@@ -14,9 +14,9 @@
 	Materials  *materials  = NULL;
 	Parameters *parameters = NULL;
-	int         kflag,pflag;
 	
 	/* output datasets: */
-	Mat Kgg=NULL;
-	Vec pg=NULL;
+	Mat    Kgg  = NULL;
+	Vec    pg   = NULL;
+	double kmax;
 
 	/*Boot module: */
@@ -34,8 +34,4 @@
 	FetchParams(&parameters,PARAMETERS);
 
-	/*parameters: */
-	parameters->FindParam(&kflag,KflagEnum);
-	parameters->FindParam(&pflag,PflagEnum);
-
 	/*configure: */
 	elements->  Configure(elements,loads, nodes,vertices, materials,parameters);
@@ -45,9 +41,10 @@
 
 	/*!Generate internal degree of freedom numbers: */
-	SystemMatricesx(&Kgg, &pg,elements,nodes,vertices,loads,materials,parameters,kflag,pflag);
+	SystemMatricesx(&Kgg,&pg,&kmax,elements,nodes,vertices,loads,materials,parameters);
 
 	/*write output datasets: */
 	WriteData(KGG,Kgg);
 	WriteData(PG,pg);
+	WriteData(KMAX,kmax);
 	
 	/*Free ressources: */
@@ -68,5 +65,5 @@
 {
 	_printf_("\n");
-	_printf_("   usage: [Kgg,pg] = %s(elements,nodes,vertices,loads,materials,parameters);\n",__FUNCT__);
+	_printf_("   usage: [Kgg,pg,kmax] = %s(elements,nodes,vertices,loads,materials,parameters);\n",__FUNCT__);
 	_printf_("\n");
 }
Index: /issm/trunk/src/mex/SystemMatrices/SystemMatrices.h
===================================================================
--- /issm/trunk/src/mex/SystemMatrices/SystemMatrices.h	(revision 5692)
+++ /issm/trunk/src/mex/SystemMatrices/SystemMatrices.h	(revision 5693)
@@ -18,18 +18,19 @@
 
 /* serial input macros: */
-#define ELEMENTS (mxArray*)prhs[0]
-#define NODES (mxArray*)prhs[1]
-#define VERTICES (mxArray*)prhs[2]
-#define LOADS (mxArray*)prhs[3]
-#define MATERIALS (mxArray*)prhs[4]
-#define PARAMETERS (mxArray*)prhs[5]
+#define ELEMENTS   (mxArray *)prhs[0]
+#define NODES      (mxArray *)prhs[1]
+#define VERTICES   (mxArray *)prhs[2]
+#define LOADS      (mxArray *)prhs[3]
+#define MATERIALS  (mxArray *)prhs[4]
+#define PARAMETERS (mxArray *)prhs[5]
 
 /* serial output macros: */
-#define KGG (mxArray**)&plhs[0]
-#define PG (mxArray**)&plhs[1]
+#define KGG  (mxArray**)&plhs[0]
+#define PG   (mxArray**)&plhs[1]
+#define KMAX (mxArray**)&plhs[2]
 
 /* serial arg counts: */
 #undef NLHS
-#define NLHS  2
+#define NLHS  3
 #undef NRHS
 #define NRHS  6