/*!\file ElementMatrix.cpp
 * \brief: implementation of the ElementMatrix object, used to plug values from element into global stiffness matrix
 */

/*Headers:*/
/*{{{1*/
#ifdef HAVE_CONFIG_H
	#include <config.h>
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include <stdio.h>
#include <string.h>
#include "../objects.h"
#include "../../shared/shared.h"
#include "../../Container/Container.h"
#include "../../include/include.h"
/*}}}*/

/*ElementMatrix constructors and destructor*/
/*FUNCTION ElementMatrix::ElementMatrix(){{{1*/
ElementMatrix::ElementMatrix(){

	this->nrows=0;
	this->ncols=0;
	this->values=NULL;
	this->dofsymmetrical=false;

	this->row_fsize=0;
	this->row_flocaldoflist=NULL;
	this->row_fglobaldoflist=NULL;
	this->row_ssize=0;
	this->row_slocaldoflist=NULL;
	this->row_sglobaldoflist=NULL;

	this->col_fsize=0;
	this->col_flocaldoflist=NULL;
	this->col_fglobaldoflist=NULL;
	this->col_ssize=0;
	this->col_slocaldoflist=NULL;
	this->col_sglobaldoflist=NULL;

}
/*}}}*/
/*FUNCTION ElementMatrix::ElementMatrix(ElementMatrix* Ke){{{1*/
ElementMatrix::ElementMatrix(ElementMatrix* Ke){

	if(!Ke) _error_("Input Element Matrix is a NULL pointer");
	this->Init(Ke);
	return;
}
/*}}}*/
/*FUNCTION ElementMatrix::ElementMatrix(ElementMatrix* Ke1, ElementMatrix* Ke2){{{1*/
ElementMatrix::ElementMatrix(ElementMatrix* Ke1, ElementMatrix* Ke2){

	/*intermediaries*/
	int i,j,counter;
	int gsize,fsize,ssize;
	int* P=NULL;
	bool found;

	/*If one of the two matrix is NULL, we copy the other one*/
	if(!Ke1 && !Ke2){
		_error_("Two input element matrices are NULL");
	}
	else if(!Ke1){
		this->Init(Ke2);
		return;
	}
	else if(!Ke2){
		this->Init(Ke1);
		return;
	}

	/*General Case: Ke1 and Ke2 are not empty*/
	if(!Ke1->dofsymmetrical || !Ke2->dofsymmetrical) _error_("merging 2 non dofsymmetrical matrices not implemented yet");

	/*Initialize itransformation matrix Ke[P[i]] = Ke2[i]*/
	P=(int*)xmalloc(Ke2->nrows*sizeof(int));

	/*1: Get the new numbering of Ke2 and get size of the new matrix*/
	gsize=Ke1->nrows;
	for(i=0;i<Ke2->nrows;i++){
		found=false;
		for(j=0;j<Ke1->nrows;j++){
			if(Ke2->gglobaldoflist[i]==Ke1->gglobaldoflist[j]){
				found=true; P[i]=j; break;
			}
		}
		if(!found){
			P[i]=gsize; gsize++;
		}
	}

	/*2: Initialize static fields*/
	this->nrows=gsize;
	this->ncols=gsize;
	this->dofsymmetrical=true;

	/*Gset and values*/
	this->gglobaldoflist=(int*)xmalloc(this->nrows*sizeof(int));
	this->values=(double*)xcalloc(this->nrows*this->ncols,sizeof(double));
	for(i=0;i<Ke1->nrows;i++){
		for(j=0;j<Ke1->ncols;j++){
			this->values[i*this->ncols+j] += Ke1->values[i*Ke1->ncols+j];
		}
		this->gglobaldoflist[i]=Ke1->gglobaldoflist[i];
	}
	for(i=0;i<Ke2->nrows;i++){
		for(j=0;j<Ke2->ncols;j++){
			this->values[P[i]*this->ncols+P[j]] += Ke2->values[i*Ke2->ncols+j];
		}
		this->gglobaldoflist[P[i]]=Ke2->gglobaldoflist[i];
	}

	/*Fset*/
	fsize=Ke1->row_fsize;
	for(i=0;i<Ke2->row_fsize;i++){
		if(P[Ke2->row_flocaldoflist[i]] >= Ke1->nrows) fsize++;
	}
	this->row_fsize=fsize;
	if(fsize){
		this->row_flocaldoflist =(int*)xmalloc(fsize*sizeof(int));
		this->row_fglobaldoflist=(int*)xmalloc(fsize*sizeof(int));
		for(i=0;i<Ke1->row_fsize;i++){
			this->row_flocaldoflist[i] =Ke1->row_flocaldoflist[i];
			this->row_fglobaldoflist[i]=Ke1->row_fglobaldoflist[i];
		}
		counter=Ke1->row_fsize;
		for(i=0;i<Ke2->row_fsize;i++){
			if(P[Ke2->row_flocaldoflist[i]] >= Ke1->nrows){
				this->row_flocaldoflist[counter] =P[Ke2->row_flocaldoflist[i]];
				this->row_fglobaldoflist[counter]=Ke2->row_fglobaldoflist[i];
				counter++;
			}
		}
	}
	else{
		this->row_flocaldoflist=NULL;
		this->row_fglobaldoflist=NULL;
	}

	/*Sset*/
	ssize=Ke1->row_ssize;
	for(i=0;i<Ke2->row_ssize;i++){
		if(P[Ke2->row_slocaldoflist[i]] >= Ke1->nrows) ssize++;
	}
	this->row_ssize=ssize;
	if(ssize){
		this->row_slocaldoflist =(int*)xmalloc(ssize*sizeof(int));
		this->row_sglobaldoflist=(int*)xmalloc(ssize*sizeof(int));
		for(i=0;i<Ke1->row_ssize;i++){
			this->row_slocaldoflist[i] =Ke1->row_slocaldoflist[i];
			this->row_sglobaldoflist[i]=Ke1->row_sglobaldoflist[i];
		}
		counter=Ke1->row_ssize;
		for(i=0;i<Ke2->row_ssize;i++){
			if(P[Ke2->row_slocaldoflist[i]] >= Ke1->nrows){
				this->row_slocaldoflist[counter] =P[Ke2->row_slocaldoflist[i]];
				this->row_sglobaldoflist[counter]=Ke2->row_sglobaldoflist[i];
				counter++;
			}
		}
	}
	else{
		this->row_slocaldoflist=NULL;
		this->row_sglobaldoflist=NULL;
	}

	/*don't do cols, we can pick them up from the rows: */
	this->col_fsize=0;
	this->col_flocaldoflist=NULL;
	this->col_fglobaldoflist=NULL;
	this->col_ssize=0;
	this->col_slocaldoflist=NULL;
	this->col_sglobaldoflist=NULL;

	/*clean-up*/
	xfree((void**)&P);
}
/*}}}*/
/*FUNCTION ElementMatrix::ElementMatrix(ElementMatrix* Ke1, ElementMatrix* Ke2,ElementMatrix* Ke3){{{1*/
ElementMatrix::ElementMatrix(ElementMatrix* Ke1, ElementMatrix* Ke2,ElementMatrix* Ke3){

	/*Concatenate all matrices*/
	ElementMatrix* Ke12 =new ElementMatrix(Ke1,Ke2);
	ElementMatrix* Ke123=new ElementMatrix(Ke12,Ke3);

	/*Initialize current object with this matrix*/
	this->Init(Ke123);

	/*clean-up*/
	delete Ke12;
	delete Ke123;
}
/*}}}*/
/*FUNCTION ElementMatrix::ElementMatrix(Node** nodes,int numnodes,Parameters* parameters,int approximation){{{1*/
ElementMatrix::ElementMatrix(Node** nodes,int numnodes,Parameters* parameters,int approximation){

	/*get Matrix size and properties*/
	this->dofsymmetrical=true;
	this->nrows=GetNumberOfDofs(nodes,numnodes,GsetEnum,approximation);
	this->ncols=this->nrows;

	/*fill values with 0: */
	this->values=(double*)xcalloc(this->nrows*this->ncols,sizeof(double));

	/*g list*/
	this->gglobaldoflist=GetGlobalDofList(nodes,numnodes,GsetEnum,approximation);

	/*get dof lists for f and s set: */
	this->row_fsize=GetNumberOfDofs(nodes,numnodes,FsetEnum,approximation);
	this->row_flocaldoflist =GetLocalDofList( nodes,numnodes,FsetEnum,approximation);
	this->row_fglobaldoflist=GetGlobalDofList(nodes,numnodes,FsetEnum,approximation);
	this->row_ssize=GetNumberOfDofs(nodes,numnodes,SsetEnum,approximation);
	this->row_slocaldoflist =GetLocalDofList( nodes,numnodes,SsetEnum,approximation);
	this->row_sglobaldoflist=GetGlobalDofList(nodes,numnodes,SsetEnum,approximation);

	/*Because this matrix is "dofsymmetrical" don't do cols, we can pick them up from the rows: */
	this->col_fsize=0;
	this->col_flocaldoflist=NULL;
	this->col_fglobaldoflist=NULL;
	this->col_ssize=0;
	this->col_slocaldoflist=NULL;
	this->col_sglobaldoflist=NULL;

}
/*}}}*/
/*FUNCTION ElementMatrix::~ElementMatrix(){{{1*/
ElementMatrix::~ElementMatrix(){
	
	xfree((void**)&this->values);
	xfree((void**)&this->gglobaldoflist);
	xfree((void**)&this->row_flocaldoflist);
	xfree((void**)&this->row_fglobaldoflist);
	xfree((void**)&this->row_slocaldoflist);
	xfree((void**)&this->row_sglobaldoflist);
	xfree((void**)&this->col_flocaldoflist);
	xfree((void**)&this->col_fglobaldoflist);
	xfree((void**)&this->col_slocaldoflist);
	xfree((void**)&this->col_sglobaldoflist);
}
/*}}}*/

/*ElementMatrix specific routines: */
/*FUNCTION ElementMatrix::AddToGlobal(Mat Kff, Mat Kfs){{{1*/
void ElementMatrix::AddToGlobal(Mat Kff, Mat Kfs){

	int i,j;
	double* localvalues=NULL;

	/*If Kfs is not provided, call the other function*/
	if(!Kfs){
		this->AddToGlobal(Kff);
		return;
	}

	/*In debugging mode, check consistency (no NaN, and values not too big)*/
	this->CheckConsistency();

	if(this->dofsymmetrical){
		/*only use row dofs to add values into global matrices: */
		
		if(this->row_fsize){
			/*first, retrieve values that are in the f-set from the g-set values matrix: */
			localvalues=(double*)xmalloc(this->row_fsize*this->row_fsize*sizeof(double));
			for(i=0;i<this->row_fsize;i++){
				for(j=0;j<this->row_fsize;j++){
					*(localvalues+this->row_fsize*i+j)=*(this->values+this->ncols*this->row_flocaldoflist[i]+this->row_flocaldoflist[j]);
				}
			}
			/*add local values into global  matrix, using the fglobaldoflist: */
			MatSetValues(Kff,this->row_fsize,this->row_fglobaldoflist,this->row_fsize,this->row_fglobaldoflist,(const double*)localvalues,ADD_VALUES);

			/*Free ressources:*/
			xfree((void**)&localvalues);
		}


		if((this->row_ssize!=0) && (this->row_fsize!=0)){
			/*first, retrieve values that are in the f and s-set from the g-set values matrix: */
			localvalues=(double*)xmalloc(this->row_fsize*this->row_ssize*sizeof(double));
			for(i=0;i<this->row_fsize;i++){
				for(j=0;j<this->row_ssize;j++){
					*(localvalues+this->row_ssize*i+j)=*(this->values+this->ncols*this->row_flocaldoflist[i]+this->row_slocaldoflist[j]);
				}
			}
			/*add local values into global  matrix, using the fglobaldoflist: */
			MatSetValues(Kfs,this->row_fsize,this->row_fglobaldoflist,this->row_ssize,this->row_sglobaldoflist,(const double*)localvalues,ADD_VALUES);

			/*Free ressources:*/
			xfree((void**)&localvalues);
		}
	}
	else{
		_error_(" non dofsymmetrical matrix AddToGlobal routine not support yet!");
	}

}
/*}}}*/
/*FUNCTION ElementMatrix::AddToGlobal(Mat Jff){{{1*/
void ElementMatrix::AddToGlobal(Mat Jff){

	int i,j;
	double* localvalues=NULL;

	/*Check that Jff is not NULL*/
	_assert_(Jff); 

	/*In debugging mode, check consistency (no NaN, and values not too big)*/
	this->CheckConsistency();

	if(this->dofsymmetrical){
		/*only use row dofs to add values into global matrices: */

		if(this->row_fsize){
			/*first, retrieve values that are in the f-set from the g-set values matrix: */
			localvalues=(double*)xmalloc(this->row_fsize*this->row_fsize*sizeof(double));
			for(i=0;i<this->row_fsize;i++){
				for(j=0;j<this->row_fsize;j++){
					*(localvalues+this->row_fsize*i+j)=*(this->values+this->ncols*this->row_flocaldoflist[i]+this->row_flocaldoflist[j]);
				}
			}
			/*add local values into global  matrix, using the fglobaldoflist: */
			MatSetValues(Jff,this->row_fsize,this->row_fglobaldoflist,this->row_fsize,this->row_fglobaldoflist,(const double*)localvalues,ADD_VALUES);

			/*Free ressources:*/
			xfree((void**)&localvalues);
		}

	}
	else{
		_error_(" non dofsymmetrical matrix AddToGlobal routine not support yet!");
	}

}
/*}}}*/
/*FUNCTION ElementMatrix::CheckConsistency{{{1*/
void ElementMatrix::CheckConsistency(void){
	/*Check element matrix values, only in debugging mode*/
	#ifdef _ISSM_DEBUG_ 
	for (int i=0;i<this->nrows;i++){
		for(int j=0;j<this->ncols;j++){
			if (isnan(this->values[i*this->ncols+j])) _error_("NaN found in Element Matrix");
			if (fabs(this->values[i*this->ncols+j])>1.e+50) _error_("Element Matrix values exceeds 1.e+50");
		}
	}
	#endif
}
/*}}}*/
/*FUNCTION ElementMatrix::Transpose{{{1*/
void ElementMatrix::Transpose(void){

	/*Intermediaries*/
	ElementMatrix* Ke_copy=new ElementMatrix(this);

	/*Update sizes*/
	this->nrows=Ke_copy->ncols;
	this->ncols=Ke_copy->nrows;

	/*Transpose values*/
	for (int i=0;i<this->nrows;i++) for(int j=0;j<this->ncols;j++) this->values[i*this->ncols+j]=Ke_copy->values[j*Ke_copy->ncols+i];

	/*Transpose indices*/
	if(!dofsymmetrical){
		_error_("not supported yet");
	}

	/*Clean up and return*/
	delete Ke_copy;
	return;
}
/*}}}*/
/*FUNCTION ElementMatrix::Echo{{{1*/
void ElementMatrix::Echo(void){

	int i,j;
	printf("Element Matrix echo: \n");
	printf("   nrows: %i\n",nrows);
	printf("   ncols: %i\n",ncols);
	printf("   dofsymmetrical: %s\n",dofsymmetrical?"true":"false");

	printf("   values: \n");
	for(i=0;i<nrows;i++){
		printf("      %i: ",i);
		for(j=0;j<ncols;j++) printf("%10g ",*(values+ncols*i+j));
		printf("\n");
	}

	printf("   gglobaldoflist (%p): ",gglobaldoflist);
	if(gglobaldoflist) for(i=0;i<nrows;i++)printf("%i ",gglobaldoflist[i]); printf("\n");

	printf("   row_fsize: %i\n",row_fsize);
	printf("   row_flocaldoflist (%p): ",row_flocaldoflist);
	if(row_flocaldoflist) for(i=0;i<row_fsize;i++)printf("%i ",row_flocaldoflist[i]); printf("\n");
	printf("   row_fglobaldoflist (%p): ",row_fglobaldoflist);
	if(row_fglobaldoflist)for(i=0;i<row_fsize;i++)printf("%i ",row_fglobaldoflist[i]); printf("\n");

	printf("   row_ssize: %i\n",row_ssize);
	printf("   row_slocaldoflist (%p): ",row_slocaldoflist);
	if(row_slocaldoflist)for(i=0;i<row_ssize;i++)printf("%i ",row_slocaldoflist[i]); printf("\n");
	printf("   row_sglobaldoflist (%p): ",row_sglobaldoflist);
	if(row_sglobaldoflist)for(i=0;i<row_ssize;i++)printf("%i ",row_sglobaldoflist[i]); printf("\n");

	if(!dofsymmetrical){
		printf("   col_fsize: %i\n",col_fsize);
		printf("   col_flocaldoflist (%p): ",col_flocaldoflist);
		if(col_flocaldoflist)for(i=0;i<col_fsize;i++)printf("%i ",col_flocaldoflist[i]); printf("\n");
		printf("   col_fglobaldoflist (%p): ",col_fglobaldoflist);
		if(col_fglobaldoflist)for(i=0;i<col_fsize;i++)printf("%i ",col_fglobaldoflist[i]); printf("\n");

		printf("   col_ssize: %i\n",col_ssize);
		printf("   col_slocaldoflist (%p): ",col_slocaldoflist);
		if(col_slocaldoflist)for(i=0;i<col_ssize;i++)printf("%i ",col_slocaldoflist[i]); printf("\n");
		printf("   col_sglobaldoflist (%p): ",col_sglobaldoflist);
		if(col_sglobaldoflist)for(i=0;i<col_ssize;i++)printf("%i ",col_sglobaldoflist[i]); printf("\n");
	}
}
/*}}}*/
/*FUNCTION ElementMatrix::Init{{{1*/
void ElementMatrix::Init(ElementMatrix* Ke){

	_assert_(Ke);

	this->nrows =Ke->nrows;
	this->ncols =Ke->ncols;
	this->dofsymmetrical=Ke->dofsymmetrical;

	this->values=(double*)xmalloc(this->nrows*this->ncols*sizeof(double));
	memcpy(this->values,Ke->values,this->nrows*this->ncols*sizeof(double));

	this->gglobaldoflist=(int*)xmalloc(this->nrows*sizeof(int));
	memcpy(this->gglobaldoflist,Ke->gglobaldoflist,this->nrows*sizeof(int));

	this->row_fsize=Ke->row_fsize;
	if(this->row_fsize){
		this->row_flocaldoflist=(int*)xmalloc(this->row_fsize*sizeof(int));
		memcpy(this->row_flocaldoflist,Ke->row_flocaldoflist,this->row_fsize*sizeof(int));
		this->row_fglobaldoflist=(int*)xmalloc(this->row_fsize*sizeof(int));
		memcpy(this->row_fglobaldoflist,Ke->row_fglobaldoflist,this->row_fsize*sizeof(int));
	}
	else{
		this->row_flocaldoflist=NULL;
		this->row_fglobaldoflist=NULL;
	}

	this->row_ssize=Ke->row_ssize;
	if(this->row_ssize){
		this->row_slocaldoflist=(int*)xmalloc(this->row_ssize*sizeof(int));
		memcpy(this->row_slocaldoflist,Ke->row_slocaldoflist,this->row_ssize*sizeof(int));
		this->row_sglobaldoflist=(int*)xmalloc(this->row_ssize*sizeof(int));
		memcpy(this->row_sglobaldoflist,Ke->row_sglobaldoflist,this->row_ssize*sizeof(int));
	}
	else{
		this->row_slocaldoflist=NULL;
		this->row_sglobaldoflist=NULL;
	}

	this->col_fsize=Ke->col_fsize;
	if(this->col_fsize){
		this->col_flocaldoflist=(int*)xmalloc(this->col_fsize*sizeof(int));
		memcpy(this->col_flocaldoflist,Ke->col_flocaldoflist,this->col_fsize*sizeof(int));
		this->col_fglobaldoflist=(int*)xmalloc(this->col_fsize*sizeof(int));
		memcpy(this->col_fglobaldoflist,Ke->col_fglobaldoflist,this->col_fsize*sizeof(int));
	}
	else{
		this->col_flocaldoflist=NULL;
		this->col_fglobaldoflist=NULL;
	}

	this->col_ssize=Ke->col_ssize;
	if(this->col_ssize){
		this->col_slocaldoflist=(int*)xmalloc(this->col_ssize*sizeof(int));
		memcpy(this->col_slocaldoflist,Ke->col_slocaldoflist,this->col_ssize*sizeof(int));
		this->col_sglobaldoflist=(int*)xmalloc(this->col_ssize*sizeof(int));
		memcpy(this->col_sglobaldoflist,Ke->col_sglobaldoflist,this->col_ssize*sizeof(int));
	}
	else{
		this->col_slocaldoflist=NULL;
		this->col_sglobaldoflist=NULL;
	}
}
/*}}}*/
/*FUNCTION ElementMatrix::SetDiag{{{1*/
void ElementMatrix::SetDiag(double scalar){

	int i;

	if(this->nrows!=this->ncols)_error_("need square matrix in input!");

	for(i=0;i<this->nrows;i++){
		this->values[this->ncols*i+i]=scalar;
	}
}
/*}}}*/
