/*!\file PenaltySystemMatricesx
 * \brief: add penalties to system matrices
 */

#include "./PenaltySystemMatricesx.h"

#undef __FUNCT__ 
#define __FUNCT__ "PenaltySystemMatricesx"

#include "../shared/shared.h"
#include "../include/macros.h"
#include "../toolkits/toolkits.h"
#include "../EnumDefinitions/EnumDefinitions.h"

void PenaltySystemMatricesx(Mat Kgg, Vec pg,DataSet* elements,DataSet* nodes,DataSet* loads,DataSet* materials,
		int kflag,int pflag,ParameterInputs* inputs,int analysis_type){
	
	int i;

	extern int num_procs;
	extern int my_rank;
	double kmax;
	
	/*First, get elements and loads configured: */
	elements->Configure(elements,loads, nodes, materials);
	loads->Configure(elements, loads, nodes, materials);

	/*Now, figure out maximum value of K_gg, so that we can penalize it correctly: */
	MatNorm(Kgg,NORM_INFINITY,&kmax);
	#ifdef _DEBUG_
	 _printf_("   K_gg infinity norm: %g\n",kmax);
	#endif

	/*Add penalties to stiffnesses, from loads: */
	if(kflag)loads->PenaltyCreateKMatrix(Kgg,inputs,kmax,analysis_type);
	if(pflag)loads->PenaltyCreatePVector(pg,inputs,kmax,analysis_type);
	
	/*Assemble matrices: */
	if(kflag){
		MatAssemblyBegin(Kgg,MAT_FINAL_ASSEMBLY);
		MatAssemblyEnd(Kgg,MAT_FINAL_ASSEMBLY);
		MatCompress(Kgg);
	}
	if(pflag){
		VecAssemblyBegin(pg);
		VecAssemblyEnd(pg);
	}

	MatNorm(Kgg,NORM_INFINITY,&kmax);
	#ifdef _DEBUG_
	 _printf_("   K_gg infinity norm after penalties: %g\n",kmax);
	#endif



}
