/*!\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(Matrix** pKff, Matrix** pKfs, Vector** ppf, Vector** pdf, IssmDouble* 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: */
	Matrix*    Kff  = NULL;
	Matrix*    Kfs  = NULL;
	Vector*    pf   = NULL;
	Vector*    df=NULL;
	IssmDouble kmax = 0;

	/*Display message*/
	if(VerboseModule()) _pprintLine_("   Generating matrices");

	/*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 {{{*/
	if(penalty_kflag)_assert_(kflag);
	if(penalty_pflag)_assert_(pflag);
	/*}}}*/

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

		Kff=new Matrix(fsize,fsize,connectivity,numberofdofspernode);
		Kfs=new Matrix(fsize,ssize,connectivity,numberofdofspernode);
		df=new Vector(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: */
		Kff->Assemble();
		Kfs->Assemble();
		df->Assemble();
	}
	
	if(pflag){

		pf=new Vector(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);
		}
		pf->Assemble();
	}

	/*Now, figure out maximum value of K_gg, so that we can penalize it correctly: */
	kmax=Kff->Norm(NORM_INF);

	/*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: */
		Kff->Assemble();
		Kfs->Assemble();
	}

	
	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);
		}

		pf->Assemble();
	}

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