/**
* @file    FEMfunction.cpp
* @author  Sanbai Li
* @date    2014/9
* @brief   Finite element relevant functions including:
                   1. Produce element stiffness matrix 
                   2. Transform body force and traction into node force vector
* 
* @version
* FracTHM
*/

#include "FEMfunction.h"
#include "Constants.h"
#include "MatrixManuplt.h"

namespace FracTHM{

//Given Young's Modulus and Poisson Ratio, calculating elastic matrix D (Stiffness)
void calMtrD_Stiffness_Iso(const double E, const double Nu, double* D){
	double temp = E / (1 + Nu) / (1 - 2 * Nu);
	memset(D,0x0,36*sizeof(double));
	D[0 ] = D[7 ] = D[14] = temp*(1 - Nu);
	D[21] = D[28] = D[35] = temp*(0.5 - Nu);
	D[1 ] = D[2 ] = D[6 ] = D[8] = D[12] = D[13] = temp*Nu;
}

void calMtrD_Stiffness_Aniso(const double* ENuShearModu9, double* D){
	double AA[9];
	memset(D, 0x0, 36 * sizeof(double));
	AA[0] = 1 / ENuShearModu9[0];
	AA[1] = -ENuShearModu9[3] / ENuShearModu9[1];
	AA[2] = -ENuShearModu9[5] / ENuShearModu9[2];
	AA[3] = -ENuShearModu9[3] / ENuShearModu9[0];
	AA[4] = 1 / ENuShearModu9[1];
	AA[5] = -ENuShearModu9[4] / ENuShearModu9[2];
	AA[6] = -ENuShearModu9[5] / ENuShearModu9[0];
	AA[7] = -ENuShearModu9[4] / ENuShearModu9[1];
	AA[8] = 1 / ENuShearModu9[2];
	SVD_Inv3(AA);

	D[0] = AA[0];
	D[1] = AA[1];
	D[2] = AA[2];

	D[6] = AA[3];
	D[7] = AA[4];
	D[8] = AA[5];

	D[12] = AA[6];
	D[13] = AA[7];
	D[14] = AA[8];

	D[21] = ENuShearModu9[6];
	D[28] = ENuShearModu9[7];
	D[35] = ENuShearModu9[8];
}

void calNi(double* gp, double* Ni){
	
	Ni[0] = 0.125*(1 - gp[0])*(1 - gp[1])*(1 - gp[2]);
	Ni[1] = 0.125*(1 + gp[0])*(1 - gp[1])*(1 - gp[2]);
	Ni[2] = 0.125*(1 + gp[0])*(1 + gp[1])*(1 - gp[2]);
	Ni[3] = 0.125*(1 - gp[0])*(1 + gp[1])*(1 - gp[2]);
	Ni[4] = 0.125*(1 - gp[0])*(1 - gp[1])*(1 + gp[2]);
	Ni[5] = 0.125*(1 + gp[0])*(1 - gp[1])*(1 + gp[2]);
	Ni[6] = 0.125*(1 + gp[0])*(1 + gp[1])*(1 + gp[2]);
	Ni[7] = 0.125*(1 - gp[0])*(1 + gp[1])*(1 + gp[2]);

}

//Calculate derivatives of Shape function Ni with respect to local coodinates (n,s,t)
void cal_dNidnst(double* gp, double* dNidnst){
	dNidnst[0] = -0.125*(1 - gp[1])*(1 - gp[2]);
	dNidnst[1] =  0.125*(1 - gp[1])*(1 - gp[2]);
	dNidnst[2] =  0.125*(1 + gp[1])*(1 - gp[2]);
	dNidnst[3] = -0.125*(1 + gp[1])*(1 - gp[2]);
	dNidnst[4] = -0.125*(1 - gp[1])*(1 + gp[2]);
	dNidnst[5] =  0.125*(1 - gp[1])*(1 + gp[2]);
	dNidnst[6] =  0.125*(1 + gp[1])*(1 + gp[2]);
	dNidnst[7] = -0.125*(1 + gp[1])*(1 + gp[2]); //dNi_ds

	dNidnst[8 ] = -0.125*(1 - gp[0])*(1 - gp[2]);
	dNidnst[9 ] = -0.125*(1 + gp[0])*(1 - gp[2]);
	dNidnst[10] =  0.125*(1 + gp[0])*(1 - gp[2]);
	dNidnst[11] =  0.125*(1 - gp[0])*(1 - gp[2]);
	dNidnst[12] = -0.125*(1 - gp[0])*(1 + gp[2]);
	dNidnst[13] = -0.125*(1 + gp[0])*(1 + gp[2]);
	dNidnst[14] =  0.125*(1 + gp[0])*(1 + gp[2]);
	dNidnst[15] =  0.125*(1 - gp[0])*(1 + gp[2]);//dNi_dt

	dNidnst[16] = -0.125*(1 - gp[0])*(1 - gp[1]);
	dNidnst[17] = -0.125*(1 + gp[0])*(1 - gp[1]);
	dNidnst[18] = -0.125*(1 + gp[0])*(1 + gp[1]);
	dNidnst[19] = -0.125*(1 - gp[0])*(1 + gp[1]);
	dNidnst[20] =  0.125*(1 - gp[0])*(1 - gp[1]);
	dNidnst[21] =  0.125*(1 + gp[0])*(1 - gp[1]);
	dNidnst[22] =  0.125*(1 + gp[0])*(1 + gp[1]);
	dNidnst[23] =  0.125*(1 - gp[0])*(1 + gp[1]);//dNi_dn
}

void calEle_LT(double* _LT, Element* ele, DataBase* AllData){

	//8 points with the following coordinates in the local coordinates system
	double XX[8] = { -cntGP,  cntGP,  cntGP, -cntGP, -cntGP,  cntGP, cntGP, -cntGP };
	double YY[8] = { -cntGP, -cntGP,  cntGP,  cntGP, -cntGP, -cntGP, cntGP,  cntGP };
	double ZZ[8] = { -cntGP, -cntGP, -cntGP, -cntGP,  cntGP,  cntGP, cntGP,  cntGP };

	for (size_t i0 = 0; i0 < 8; ++i0)  //Eight points n
	{
		double _Ni[8] = { 0 };
		double dNidnst[24] = { 0 };
		double TranJAC[9] = { 0 };
		double gp[3] = { 0 };
		gp[0] = XX[i0]; gp[1] = YY[i0]; gp[2] = ZZ[i0];
		//setZero(3, 3, TranJAC);
		calNi(gp, _Ni);
		cal_dNidnst(gp, dNidnst);
		for (size_t i1 = 0; i1 < 8; ++i1) //Eight nodes
		{
			size_t nodeID = ele->eNd_num[i1] - 1;
			TranJAC[0] += AllData->NodeQUE[nodeID].Xcood * dNidnst[  i1  ];
			TranJAC[1] += AllData->NodeQUE[nodeID].Ycood * dNidnst[  i1  ];
			TranJAC[2] += AllData->NodeQUE[nodeID].Zcood * dNidnst[  i1  ];//dx/ds, dy/ds, and dz/ds

			TranJAC[3] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 8];
			TranJAC[4] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 8];
			TranJAC[5] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 8];//dx/dt, dy/dt, and dz/dt

			TranJAC[6] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 16];
			TranJAC[7] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 16];
			TranJAC[8] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 16];//dx/dn, dy/dn, and dz/dn
		}
		double a, b, c, d, e, f, g, h, l;//9 entries of J star matrix 
		a = TranJAC[4] * TranJAC[8] - TranJAC[5] * TranJAC[7];
		b = TranJAC[2] * TranJAC[7] - TranJAC[1] * TranJAC[8];
		c = TranJAC[1] * TranJAC[5] - TranJAC[2] * TranJAC[4];
		d = TranJAC[5] * TranJAC[6] - TranJAC[3] * TranJAC[8];
		e = TranJAC[0] * TranJAC[8] - TranJAC[2] * TranJAC[6];
		f = TranJAC[2] * TranJAC[3] - TranJAC[0] * TranJAC[5];
		g = TranJAC[3] * TranJAC[7] - TranJAC[4] * TranJAC[6];
		h = TranJAC[1] * TranJAC[6] - TranJAC[0] * TranJAC[7];
		l = TranJAC[0] * TranJAC[4] - TranJAC[1] * TranJAC[3];

		//Calculate the determinant of transformation matrix det[J]
		double Jdet = a*TranJAC[0] + d*TranJAC[1] + g*TranJAC[2];

		for (size_t i = 0; i < 8; ++i){
			_LT[3 * i    ] += (a*dNidnst[i] + b*dNidnst[i + 8] + c*dNidnst[i + 16]);
			_LT[3 * i + 1] += (d*dNidnst[i] + e*dNidnst[i + 8] + f*dNidnst[i + 16]);
			_LT[3 * i + 2] += (g*dNidnst[i] + h*dNidnst[i + 8] + l*dNidnst[i + 16]);
		}
	}
}

double cal_1_M(const double phi0, const double Cf, const double Kdr, const double Ks) {
	return phi0*Cf + ((1 - Kdr / Ks) - phi0) / Ks;
}

void calEleFaceLoadVct(const int FaceNUM, const int Dir, const double trac, double* Faceforce,
	                    Element* ele, DataBase* AllData){

	//8 points with the following coordinates in the local coordinates system
	double XX[4], YY[4], ZZ[4];

	if (FaceNUM == 0){
		XX[0] = XX[3] = -cntGP; XX[1] = XX[2] = cntGP;
		YY[0] = YY[1] = -cntGP; YY[2] = YY[3] = cntGP;
		ZZ[0] = ZZ[1] = ZZ[2] = ZZ[3] = 1.0;
	}
	else if (FaceNUM == 1){
		XX[0] = XX[3] = -cntGP; XX[1] = XX[2] = cntGP;
		YY[0] = YY[1] = -cntGP; YY[2] = YY[3] = cntGP;
		ZZ[0] = ZZ[1] = ZZ[2] = ZZ[3] = -1.0;
	}
	else if (FaceNUM == 2){
		XX[0] = XX[3] = -cntGP; XX[1] = XX[2] = cntGP;
		ZZ[0] = ZZ[1] = -cntGP; ZZ[2] = ZZ[3] = cntGP;
		YY[0] = YY[1] = YY[2] = YY[3] = -1.0;
	}
	else if (FaceNUM == 3){
		XX[0] = XX[3] = -cntGP; XX[1] = XX[2] = cntGP;
		ZZ[0] = ZZ[1] = -cntGP; ZZ[2] = ZZ[3] = cntGP;
		YY[0] = YY[1] = YY[2] = YY[3] = 1.0;
	}
	else if (FaceNUM == 4){
		ZZ[0] = ZZ[3] = -cntGP; ZZ[1] = ZZ[2] = cntGP;
		YY[0] = YY[1] = -cntGP; YY[2] = YY[3] = cntGP;
		XX[0] = XX[1] = XX[2] = XX[3] = -1.0;
	}
	else if (FaceNUM == 5){
		ZZ[0] = ZZ[3] = -cntGP; ZZ[1] = ZZ[2] = cntGP;
		YY[0] = YY[1] = -cntGP; YY[2] = YY[3] = cntGP;
		XX[0] = XX[1] = XX[2] = XX[3] = 1.0;
	}
	else;

	for (size_t i0 = 0; i0 < 4; ++i0)  //Four points 
	{
		double gp[3] = { XX[i0], YY[i0], ZZ[i0] };
		double _Ni[8] = { 0 }, dNidnst[24] = { 0 };
		double TranJAC[9] = { 0 };
		//setZero(3, 3, TranJAC);
		calNi(gp, _Ni);
		cal_dNidnst(gp, dNidnst);

		for (size_t i1 = 0; i1 < 8; ++i1)  //Eight nodes
		{
			size_t nodeID = ele->eNd_num[i1] - 1;
			TranJAC[0] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1    ];
			TranJAC[1] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1    ];
			TranJAC[2] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1    ];//dx/ds, dy/ds, and dz/ds

			TranJAC[3] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 8];
			TranJAC[4] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 8];
			TranJAC[5] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 8];//dx/dt, dy/dt, and dz/dt

			TranJAC[6] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 16];
			TranJAC[7] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 16];
			TranJAC[8] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 16];//dx/dn, dy/dn, and dz/dn
		}

		//Transform the face load into equivalent node load vector
		double S1, S2, S12;
		if (Dir == 1){/*x direction*/
			S1  = TranJAC[3] * TranJAC[3] + TranJAC[4] * TranJAC[4] + TranJAC[5] * TranJAC[5];
			S2  = TranJAC[6] * TranJAC[6] + TranJAC[7] * TranJAC[7] + TranJAC[8] * TranJAC[8];
			S12 = TranJAC[3] * TranJAC[6] + TranJAC[4] * TranJAC[7] + TranJAC[5] * TranJAC[8];
		}
		else if (Dir == 2){/*y direction*/
			S1 = TranJAC[0] * TranJAC[0] + TranJAC[1] * TranJAC[1] + TranJAC[2] * TranJAC[2];
			S2 = TranJAC[6] * TranJAC[6] + TranJAC[7] * TranJAC[7] + TranJAC[8] * TranJAC[8];
			S12 = TranJAC[0] * TranJAC[6] + TranJAC[1] * TranJAC[7] + TranJAC[2] * TranJAC[8];
		}
		else if (Dir == 3){/*z direction*/
			S1  = TranJAC[0] * TranJAC[0] + TranJAC[1] * TranJAC[1] + TranJAC[2] * TranJAC[2];
			S2  = TranJAC[3] * TranJAC[3] + TranJAC[4] * TranJAC[4] + TranJAC[5] * TranJAC[5];
			S12 = TranJAC[0] * TranJAC[3] + TranJAC[1] * TranJAC[4] + TranJAC[2] * TranJAC[5];
		}
		else;
		double tempD = sqrt(S1*S2 - S12);
		//Calculate each node's load of element arised from face force
		for (size_t i = 0; i < 8; i++)
		{
			Faceforce[i] += _Ni[i] * trac * tempD;
		}
	}
}

//void calElementStfness(bool& isFrc, const double E, const double Nu, const double rhoBulk,
//					   double* Bodyforce, Element* ele, double* Ke, DataBase* AllData){
//
//	double D[36];
//	if (!isFrc)
//		calMtrD(E, Nu, D);
//	else;
//		//FailureCheck(ele->Ele_num, D, isFrc, ele->dipAng, ele->azimuthAng);
//
//	//8 points with the following coordinates in the local coordinates system
//	double XX[8] = { -cntGP,  cntGP,  cntGP, -cntGP, -cntGP,  cntGP, cntGP, -cntGP };
//	double YY[8] = { -cntGP, -cntGP,  cntGP,  cntGP, -cntGP, -cntGP, cntGP,  cntGP };
//	double ZZ[8] = { -cntGP, -cntGP, -cntGP, -cntGP,  cntGP,  cntGP, cntGP,  cntGP };
//
//	setZero(24, 24, Ke);
//	for (size_t i0 = 0; i0 < 8; ++i0)  //Eight gauss points for integration of (BTDB)
//	{
//		double gp[3] = { XX[i0], YY[i0], ZZ[i0] };
//		double _Ni[8], dNidnst[24];
//		double TranJAC[9], Bi[18];
//		double  D_B[144], BTDB[576],B[144];
//		//double x = 0.0, y = 0.0, z = 0.0;
//		setZero(3, 3, TranJAC);
//		setZero(6, 3, Bi);
//		setZero(6, 24, B);
//		calNi(gp, _Ni);
//		cal_dNidnst(gp, dNidnst);
//		for (size_t i1 = 0; i1 < 8; ++i1)
//		{
//			size_t nodeID = ele->eNd_num[i1] - 1;
//			TranJAC[0] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1    ];
//			TranJAC[1] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1    ];
//			TranJAC[2] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1    ];//dx/ds, dy/ds, and dz/ds
//
//			TranJAC[3] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 8];
//			TranJAC[4] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 8];
//			TranJAC[5] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 8];//dx/dt, dy/dt, and dz/dt
//
//			TranJAC[6] += AllData->NodeQUE[nodeID].Xcood * dNidnst[i1 + 16];
//			TranJAC[7] += AllData->NodeQUE[nodeID].Ycood * dNidnst[i1 + 16];
//			TranJAC[8] += AllData->NodeQUE[nodeID].Zcood * dNidnst[i1 + 16];//dx/dn, dy/dn, and dz/dn
//		}
//
//
//		double a, b, c, d, e, f, g, h, l;//9 entries of J star matrix 
//		a = TranJAC[4] * TranJAC[8] - TranJAC[5] * TranJAC[7];
//		b = TranJAC[2] * TranJAC[7] - TranJAC[1] * TranJAC[8];
//		c = TranJAC[1] * TranJAC[5] - TranJAC[2] * TranJAC[4];
//		d = TranJAC[5] * TranJAC[6] - TranJAC[3] * TranJAC[8];
//		e = TranJAC[0] * TranJAC[8] - TranJAC[2] * TranJAC[6];
//		f = TranJAC[2] * TranJAC[3] - TranJAC[0] * TranJAC[5];
//		g = TranJAC[3] * TranJAC[7] - TranJAC[4] * TranJAC[6];
//		h = TranJAC[1] * TranJAC[6] - TranJAC[0] * TranJAC[7];
//		l = TranJAC[0] * TranJAC[4] - TranJAC[1] * TranJAC[3];
//
//		//Calculate the determinant of transformation matrix det[J]
//		double Jdet = a*TranJAC[0] + d*TranJAC[1] + g*TranJAC[2];
//
//		//Calculate each node's load of element arised from body force
//		for (size_t i = 0; i < 8; i++)
//		{
//			Bodyforce[i] -= _Ni[i] * rhoBulk * cntg * Jdet;
//		}
//
//		//Calculate Matrix B
//		for (size_t i = 0; i < 8; ++i){
//			Bi[0 ] =  a*dNidnst[i] + b*dNidnst[i + 8] + c*dNidnst[i + 16];
//			Bi[4 ] =  d*dNidnst[i] + e*dNidnst[i + 8] + f*dNidnst[i + 16];
//			Bi[8 ] =  g*dNidnst[i] + h*dNidnst[i + 8] + l*dNidnst[i + 16];
//			Bi[1 ] = Bi[2 ] = Bi[3 ] = 0.0;
//			Bi[5 ] = Bi[6 ] = Bi[7 ] = 0.0;
//			Bi[11] = Bi[12] = Bi[16] = 0.0;
//			Bi[17] = Bi[10] = Bi[0 ];
//			Bi[14] = Bi[9 ] = Bi[4 ];
//			Bi[15] = Bi[13] = Bi[8 ];
//
//			for (size_t j = 0; j < 6; ++j)
//			{
//				for (size_t k = 0; k < 3; ++k)
//				{
//					B[i * 3 + j * 24 + k] = Bi[k + j * 3] / Jdet;
//					//B=[B1,B2,...,B8]/detJ
//				}
//			}
//		}
//
//		//Calculate element stifness matrix Ke
//		MatrA_MatrB<double>(6, 6, 24, D, B, D_B);
//		MatrA_MatrB<double>(24, 6, 24, TransM(6, 24, B), D_B, BTDB);
//		for (size_t i = 0; i < 576; ++i)
//			Ke[i] += BTDB[i] * Jdet;
//	}
//}

void FindIJBlcfromKe(double* kk9, int i, int j, double *Ke) {
	kk9[0] = Ke[72 * i + 3 * j     ];
	kk9[1] = Ke[72 * i + 3 * j + 1 ];
	kk9[2] = Ke[72 * i + 3 * j + 2 ];
	kk9[3] = Ke[72 * i + 3 * j + 24];
	kk9[4] = Ke[72 * i + 3 * j + 25];
	kk9[5] = Ke[72 * i + 3 * j + 26];
	kk9[6] = Ke[72 * i + 3 * j + 48];
	kk9[7] = Ke[72 * i + 3 * j + 49];
	kk9[8] = Ke[72 * i + 3 * j + 50];
}

/*	void calGlobalStfness(double* KK, DataBase* AllData){
		size_t _NdAmtoElm = AllData->NdAmtoElm;
		size_t _NodeAmt = AllData->NodeAmt;
		double _Ke[576];
		for (auto& _Element : AllData->Elmlist){
			double YM = AllData->YoungsModulus;
			double PR = AllData->PoissonRatio;
			calElementStfness(YM, PR, &_Element, _Ke, AllData);
			for (size_t i = 0; i < _NdAmtoElm * 3; ++i)
			{
				//The row node number of each entry block of the globle stiffness matrix
				size_t Ndnum_I = _Element.eNd_num[i / 3] - 1;
				for (size_t j = 0; j < _NdAmtoElm * 3; ++j)
				{
					//The column node number of each entry block of the globle stiffness matrix
					size_t Ndnum_J = _Element.eNd_num[j / 3] - 1;
					//Search the location of entries of the globle stiffness matrix
					size_t EntryKij = (9 * Ndnum_I + 3 * (i % 3))*_NodeAmt + 3 * Ndnum_J + j % 3;
					KK[EntryKij] += _Ke[i*_NdAmtoElm + j];
				}
			}
		}
		//double aa=0.0;
		//for (int i = 0; i < 24; ++i) aa += KK[i];
		//std::cout << aa << std::endl;
		//std::cout << "Assemble stiffness matrix finished." << std::endl;
	}
*/	

}//namespace FracTHM