/*!\file SystemMatricesx
 * \brief: create system matrices (stiffness matrix, loads vector)
 */

#include "./SystemMatricesx.h"
#include "../../shared/shared.h"
#include "../../include/include.h"
#include "../../io/io.h"
#include "../../toolkits/toolkits.h"
#include "../../EnumDefinitions/EnumDefinitions.h"

void SystemMatricesx(Mat* pKff, Mat* pKfs, Vec* ppf, Vec* pdf, 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      i,j;
	int      fsize,ssize;
	int      connectivity, numberofdofspernode;
	int      analysis_type, configuration_type;
	Element *element = NULL;
	Load    *load    = NULL;
	
	/*output: */
	Mat    Kff  = NULL;
	Mat    Kfs  = NULL;
	Vec    pf   = NULL;
	Vec    df=NULL;
	double kmax = 0;

	/*Display message*/
	_printf_(VerboseModule(),"   Generating matrices\n");

	/*retrive parameters: */
	parameters->FindParam(&analysis_type,AnalysisTypeEnum);
	parameters->FindParam(&configuration_type,ConfigurationTypeEnum);
	parameters->FindParam(&connectivity,MeshAverageVertexConnectivityEnum);

	/*Get size of matrices: */
	fsize=nodes->NumberOfDofs(configuration_type,FsetEnum);
	ssize=nodes->NumberOfDofs(configuration_type,SsetEnum);

	numberofdofspernode=nodes->MaxNumDofs(configuration_type,GsetEnum);

	/*Checks in debugging mode {{{1*/
	if(penalty_kflag)_assert_(kflag);
	if(penalty_pflag)_assert_(pflag);
	/*}}}*/

	/*Compute penalty free mstiffness matrix and load vector*/
	if(kflag){

		Kff=NewMat(fsize,fsize,connectivity,numberofdofspernode);
		Kfs=NewMat(fsize,ssize,connectivity,numberofdofspernode);
		df=NewVec(fsize);

		/*Fill stiffness matrix from elements: */
		for (i=0;i<elements->Size();i++){
			element=(Element*)elements->GetObjectByOffset(i);
			element->CreateKMatrix(Kff,Kfs,df);
		}

		/*Fill stiffness matrix from loads if loads have the current configuration_type: */
		for (i=0;i<loads->Size();i++){
			load=(Load*)loads->GetObjectByOffset(i);
			if (load->InAnalysis(configuration_type)) load->CreateKMatrix(Kff,Kfs);
		}

		/*Assemble matrix and doftypes and compress matrix to save memory: */
		MatAssemblyBegin(Kff,MAT_FINAL_ASSEMBLY);
		MatAssemblyEnd(Kff,MAT_FINAL_ASSEMBLY);
		#if _PETSC_MAJOR_ == 2 
		MatCompress(Kff);
		#endif

		MatAssemblyBegin(Kfs,MAT_FINAL_ASSEMBLY);
		MatAssemblyEnd(Kfs,MAT_FINAL_ASSEMBLY);
		#if _PETSC_MAJOR_ == 2 
		MatCompress(Kfs);
		#endif
		VecAssemblyBegin(df);
		VecAssemblyEnd(df);
		
	}
	
	if(pflag){

		pf=NewVec(fsize);

		/*Fill right hand side vector, from elements: */
		for (i=0;i<elements->Size();i++){
			element=(Element*)elements->GetObjectByOffset(i);
			element->CreatePVector(pf);
		}

		/*Fill right hand side from loads if loads have the current configuration_type: */
		for (i=0;i<loads->Size();i++){
			load=(Load*)loads->GetObjectByOffset(i);
			if (load->InAnalysis(configuration_type)) load->CreatePVector(pf);
		}

		VecAssemblyBegin(pf);
		VecAssemblyEnd(pf);
	}

	/*Now, figure out maximum value of K_gg, so that we can penalize it correctly: */
	MatNorm(Kff,NORM_INFINITY,&kmax);

	/*Now, deal with penalties*/
	if(penalty_kflag){

		/*Fill stiffness matrix from loads: */
		for (i=0;i<loads->Size();i++){
			load=(Load*)loads->GetObjectByOffset(i);
			if (load->InAnalysis(configuration_type)) load->PenaltyCreateKMatrix(Kff,Kfs,kmax);
		}

		/*Assemble matrix and compress matrix to save memory: */
		MatAssemblyBegin(Kff,MAT_FINAL_ASSEMBLY);
		MatAssemblyEnd(Kff,MAT_FINAL_ASSEMBLY);
		#if _PETSC_MAJOR_ == 2 
		MatCompress(Kff);
		#endif

		MatAssemblyBegin(Kfs,MAT_FINAL_ASSEMBLY);
		MatAssemblyEnd(Kfs,MAT_FINAL_ASSEMBLY);
		#if _PETSC_MAJOR_ == 2 
		MatCompress(Kfs);
		#endif
	}

	
	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(pf,kmax);
		}

		VecAssemblyBegin(pf);
		VecAssemblyEnd(pf);
	}

	/*Assign output pointers: */
	if(pKff) *pKff=Kff;
	else      MatFree(&Kff);
	if(pKfs) *pKfs=Kfs;
	else      MatFree(&Kfs);
	if(ppf)  *ppf=pf;
	else      VecFree(&pf);
	if(pdf)  *pdf=df;
	else      VecFree(&df);
	if(pkmax) *pkmax=kmax;
}
