/*!\file SystemMatricesx
 * \brief retrieve vector from inputs in elements
 */

#include "./SystemMatricesx.h"
#include "../../shared/shared.h"
#include "../../toolkits/toolkits.h"
#include "../AllocateSystemMatricesx/AllocateSystemMatricesx.h"

void SystemMatricesx(Matrix<IssmDouble>** pKff, Matrix<IssmDouble>** pKfs, Vector<IssmDouble>** ppf, Vector<IssmDouble>** pdf, IssmDouble* pkmax,FemModel* femmodel){

	/*intermediary: */
	int      i,M,N;
	int      configuration_type;
	Element *element = NULL;
	Load    *load    = NULL;

	/*output: */
	Matrix<IssmDouble> *Kff  = NULL;
	Matrix<IssmDouble> *Kfs  = NULL;
	Vector<IssmDouble> *pf   = NULL;
	Vector<IssmDouble> *df   = NULL;
	IssmDouble          kmax = 0;

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

	/*retrive parameters: */
	femmodel->parameters->FindParam(&configuration_type,ConfigurationTypeEnum);

	/*First, we might need to do a dry run to get kmax if penalties are employed*/
	if(femmodel->loads->IsPenalty(configuration_type)){

		/*Allocate Kff_temp*/
		Matrix<IssmDouble> *Kff_temp = NULL;
		AllocateSystemMatricesx(&Kff_temp,NULL,NULL,NULL,femmodel);

		/*Get complete stiffness matrix without penalties*/
		for (i=0;i<femmodel->elements->Size();i++){
			element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
			element->CreateKMatrix(Kff_temp,NULL);
		}

		for (i=0;i<femmodel->loads->Size();i++){
			load=dynamic_cast<Load*>(femmodel->loads->GetObjectByOffset(i));
			if(load->InAnalysis(configuration_type)) load->CreateKMatrix(Kff_temp,NULL);
		}
		Kff_temp->Assemble();

		/*Now, figure out maximum value of stiffness matrix, so that we can penalize it correctly: */
		kmax=Kff_temp->Norm(NORM_INF);
		delete Kff_temp;
	}

	/*Allocate stiffness matrices and load vector*/
	AllocateSystemMatricesx(&Kff,&Kfs,&df,&pf,femmodel);

	/*Display size*/
	if(VerboseModule()){
		Kff->GetSize(&M,&N);
		_printf0_(" (Kff stiffness matrix size: "<<M<<" x "<<N<<")\n");
	}

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

	for (i=0;i<femmodel->loads->Size();i++){
		load=dynamic_cast<Load*>(femmodel->loads->GetObjectByOffset(i));
		if(load->InAnalysis(configuration_type)) load->CreateKMatrix(Kff,Kfs);
	}

	/*Fill right hand side vector, from elements and loads */
	for (i=0;i<femmodel->elements->Size();i++){
		element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
		element->CreatePVector(pf);
	}
	for (i=0;i<femmodel->loads->Size();i++){
		load=dynamic_cast<Load*>(femmodel->loads->GetObjectByOffset(i));
		if(load->InAnalysis(configuration_type)) load->CreatePVector(pf);
	}

	/*Now deal with penalties (only in loads)*/
	if(femmodel->loads->IsPenalty(configuration_type)){
		for (i=0;i<femmodel->loads->Size();i++){
			load=dynamic_cast<Load*>(femmodel->loads->GetObjectByOffset(i));
			if(load->InAnalysis(configuration_type)) load->PenaltyCreateKMatrix(Kff,Kfs,kmax);
		}
		for (i=0;i<femmodel->loads->Size();i++){
			load=dynamic_cast<Load*>(femmodel->loads->GetObjectByOffset(i));
			if(load->InAnalysis(configuration_type)) load->PenaltyCreatePVector(pf,kmax);
		}
	}

	/*Create dof vector for stiffness matrix preconditioning*/
	for (i=0;i<femmodel->elements->Size();i++){
		element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
		element->CreateDVector(df);
	}

	/*Assemble matrices and vector*/
	Kff->Assemble();
	Kfs->Assemble();
	pf->Assemble();
	df->Assemble();
	//Kff->AllocationInfo();
	//Kfs->AllocationInfo();

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