#include <math.h>
#include "shapeFuncInternals.h"

/* Macros for accessing other dimension indices in Cartesian coordinates */
#define INC3(i) ((i + 1) % 3)
#define DEC3(i) ((i + 2) % 3)

/* Macros for accessing other dimension indices in volume coordinates */
#define INC4(i) ((i + 1) % 4)
#define OPP4(i) ((i + 2) % 4)
#define DEC4(i) ((i + 3) % 4)

/* Macro to convert four false derivatives into three true derivatives for volume coordinates */
#define REDUCE_DRV4 {dN[nshp][0] = dN4[0] - dN4[3]; dN[nshp][1] = dN4[1] - dN4[3]; dN[nshp][2] = dN4[2] - dN4[3];}

/* n choose r for use in entity functions */
double nCr(int n, int r) {
	int i;
	double c = 1;

	for (i = n; i > 1; i--) {
		if (i > r && i > n - r)
			c *= i;
		else if (i <= r && i <= n - r)
			c /= i;
	}

	return c;
}

/* Entity function along an edge according to equation 33 */
double Phi(int p, double x) {
	int i;
	double scale;
	double sum = 0;

	for (i = 0; i <= p - 2; i++) {
		scale = nCr(p - 2, i) * nCr(p - 1, i) / (i + 1);
		if (i % 2)
			sum -= pow(x, i) * pow(1 - x, p - 2 - i) * scale;
		else
			sum += pow(x, i) * pow(1 - x, p - 2 - i) * scale;
	}

	return sum;
}

/* Entity function derivative along an edge */
double PhiDrv(int p, double x) {
	int i;
	double scale;
	double sum = 0;

	for (i = 0; i <= p - 2; i++) {
		scale = nCr(p - 2, i) * nCr(p - 1, i) / (i + 1);
		if (i % 2) {
			sum -= i * pow(x, i - 1) * pow(1 - x, p - 2 - i) * scale;
			sum += pow(x, i) * (p - 2 - i) * pow(1 - x, p - 3 - i) * scale;
		} else {
			sum += i * pow(x, i - 1) * pow(1 - x, p - 2 - i) * scale;
			sum -= pow(x, i) * (p - 2 - i) * pow(1 - x, p - 3 - i) * scale;
		}
	}

	return sum;
}

/* Map to appropriate coordinates and entity function */
double Map(int p, double x) {
	return Phi(p, (1.0 + x) / 2.0);
}

/* Map to appropriate coordinates and entity function derivative */
double MapDrv(int p, double x) {
	return PhiDrv(p, (1.0 + x) / 2.0);
}

/* Entity Functions **********************************************************/

/* Compute all modes of entity functions for edge parameterization 1 */
int ent_edge1(double x, int i, int p, double *Ent, double **EntDrv) {
	int a;
	int n = 0;

	/* Loop over every mode 2 to p*/
	for (a = 2; a <= p; a++) {

		/* Entity function for mode a */
		Ent[n] = Map(a, x);

		/* Entity function derivatives for mode a */
		EntDrv[n][i] = MapDrv(a, x);
		EntDrv[n][INC3(i)] = 0.0;
		EntDrv[n][DEC3(i)] = 0.0;

		n++;
	}

	/* Return number of modes */
	return n;
}

/* Compute all modes of entity functions for edge parameterization 2 */
int ent_edge2(double x0, double x1, int i, int j, int p, double *Ent, double **EntDrv) {
	int a, k;
	int n = 0;
	double drv[2];

	/* Loop over every mode 0 to p - 2 */
	for (a = 0; a <= p - 2; a++) {

		/* Entity function for mode a */
		Ent[n] = En(a, x0, x1);

		/* Entity function derivatives for mode a */
		EnDrv(a, x0, x1, drv);

		for (k = 0; k < 4; k++)
			EntDrv[n][k] = 0.0;
		EntDrv[n][i] = drv[0];
		EntDrv[n][j] = drv[1];

		n++;
	}

	/* Return number of modes */
	return n;
}

/* Compute all modes of entity functions for quad face */
int ent_quad(double x0, double x1, int i, int p, double *Ent, double **EntDrv) {
	int a, a0, a1;
	int n = 0;

	/* Loop over every mode a0 + a1 = 4 to p */
	for (a = 4; a <= p; a++) {
		for (a0 = 2; a0 <= a - 2; a0++) {
			for (a1 = 2; a1 <= a - 2; a1++) {
				if (a0 + a1 == a) {

					/* Entity function for mode a0,a1 */
					Ent[n] = Map(a0, x0) * Map(a1, x1);

					/* Entity function derivatives for mode a0,a1 */
					EntDrv[n][i] = 0.0;
					EntDrv[n][INC3(i)] = MapDrv(a0, x0) * Map(a1, x1);
					EntDrv[n][DEC3(i)] = Map(a0, x0) * MapDrv(a1, x1);

					n++;
				}
			}
		}
	}

	/* Return number of modes */
	return n;
}

/* Compute all modes of entity functions for triangle face */
int ent_tri(double x0, double x1, int i, int p, double *Ent, double **EntDrv) {
	int a, a0, a1;
	int n = 0;
	double drv[2];

	/* Loop over every mode a0 + a1 = 0 to p - 3 */
	for (a = 0; a <= p - 3; a++) {
		for (a0 = 0; a0 <= a; a0++) {
			for (a1 = 0; a1 <= a; a1++) {
				if (a0 + a1 == a) {

					/* Entity function for mode a0,a1 */
					Ent[n] = Fn(a0, a1, x0, x1);

					/* Entity function derivatives for mode a0,a1 */
					FnDrv(a0, a1, x0, x1, drv);

					EntDrv[n][i] = 0.0;
					EntDrv[n][INC4(i)] = drv[0];
					EntDrv[n][OPP4(i)] = drv[1];
					EntDrv[n][DEC4(i)] = -drv[0] - drv[1];

					n++;
				}
			}
		}
	}

	/* Return number of modes */
	return n;
}

/* Compute all modes of entity functions for hexahedron */
int ent_hex(double x0, double x1, double x2, int p, double *Ent, double **EntDrv) {
	int a, a0, a1, a2;
	int n = 0;

	/* Loop over every mode a0 + a1 + a2 = 6 to p */
	for (a = 6; a <= p; a++) {
		for (a0 = 2; a0 < a - 4; a0++) {
			for (a1 = 2; a1 < a - 4; a1++) {
				for (a2 = 2; a2 < a - 4; a2++) {
					if (a0 + a1 + a2 == a) {

						/* Entity function for mode a0,a1,a2 */
						Ent[n] = Map(a0, x0) * Map(a1, x1) * Map(a2, x2);

						/* Entity function derivatives for mode a0,a1,a2 */
						EntDrv[n][0] = MapDrv(a0, x0) * Map(a1, x1) * Map(a2, x2);
						EntDrv[n][1] = Map(a0, x0) * MapDrv(a1, x1) * Map(a2, x2);
						EntDrv[n][2] = Map(a0, x0) * Map(a1, x1) * MapDrv(a2, x2);

						n++;
					}
				}
			}
		}
	}

	/* Return number of modes */
	return n;
}

/* Compute all modes of entity functions for tetrahedron */
int ent_tet(double x0, double x1, double x2, int p, double *Ent, double **EntDrv) {
	int a, a0, a1, a2;
	int n = 0;
	double drv[3];

	/* Loop over every mode a0 + a1 + a2 = 0 to p - 4 */
	for (a = 0; a <= p - 4; a++) {
		for (a0 = 0; a0 <= a; a0++) {
			for (a1 = 0; a1 <= a; a1++) {
				for (a2 = 0; a2 <= a; a2++) {
					if (a0 + a1 + a2 == a) {

						/* Entity function for mode a0,a1,a2 */
						Ent[n] = Bn(a0, a1, a2, x0, x1, x2);

						/* Entity function derivatives for mode a0,a1,a2 */
						BnDrv(a0, a1, a2, x0, x1, x2, drv);

						EntDrv[n][0] = drv[0];
						EntDrv[n][1] = drv[1];
						EntDrv[n][2] = drv[2];
						EntDrv[n][3] = -drv[0] - drv[1] - drv[1];

						n++;
					}
				}
			}
		}
	}

	/* Return number of modes */
	return n;
}

/* Shape and Derivative Functions ********************************************/

/* Compute all shape functions and derivatives for hexahedron */
int HexShapeAndDrv(int p, double par[3], double N[], double dN[][3]) {
	int i, j, k, m, n, modes;
	int nshp = 0;
	double Blend, BlendDrv[3], parsq[3];
	double *Ent;
	double *EntDrv[3];

	Ent = new double[100];
	for (i = 0; i < 3; i++)
		EntDrv[i] = new double[100];

	/* Convenient variables for computing the blending functions */
	for (i = 0; i < 3; i++)
		parsq[i] = par[i] * par[i] - 1;

	/* Nodes */
	for (i = -1; i < 2; i += 2) {
		for (j = -1; j < 2; j += 2) {
			for (k = -1; k < 2; k += 2) {
				/* Node at coordinates i,j,k */

				/* Shape function */
				N[nshp] = 0.125 * (1 + i * par[0]) * (1 + j * par[1]) * (1 + k * par[2]);

				/* Shape function derivatives */
				dN[nshp][0] = 0.125 * i * (1 + j * par[1]) * (1 + k * par[2]);
				dN[nshp][1] = 0.125 * j * (1 + k * par[2]) * (1 + i * par[0]);
				dN[nshp][2] = 0.125 * k * (1 + i * par[0]) * (1 + j * par[1]);

				nshp++;
			}
		}
	}

	/* Edges */
	if (p >= 2) {
		for (i = 0; i < 3; i++) {
			for (j = -1; j < 2; j += 2) {
				for (k = -1; k < 2; k += 2) {
					/* Edge parallel to axis i at coordinates j,k */

					/* Blending function */
					Blend = 0.125 * parsq[i] * (1 + j * par[INC3(i)]) * (1 + k * par[DEC3(i)]);

					/* Blending function derivatives */
					BlendDrv[i] = 0.25 * par[i] * (1 + j * par[INC3(i)]) * (1 + k * par[DEC3(i)]);
					BlendDrv[INC3(i)] = 0.125 * j * parsq[i] * (1 + k * par[DEC3(i)]);
					BlendDrv[DEC3(i)] = 0.125 * k * parsq[i] * (1 + j * par[INC3(i)]);

					/* Entity functions and derivatives for order p */
					modes = ent_edge1(par[i], i, p, Ent, EntDrv);
					for (m = 0; m < modes; m++) {

						/* Shape function */
						N[nshp] = Blend * Ent[m];

						/* Shape function derivatives */
						for (n = 0; n < 3; n++)
							dN[nshp][n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];

						nshp++;
					}
				}
			}
		}
	}

	/* Faces */
	if (p >= 4) {
		for (i = 0; i < 3; i++) {
			for (j = -1; j < 2; j += 2) {
				/* Face normal to axis i at coordinate j */

				/* Blending function */
				Blend = 0.125 * (1 + j * par[i]) * parsq[INC3(i)] * parsq[DEC3(i)];

				/* Blending function derivative */
				BlendDrv[i] = 0.125 * j * parsq[INC3(i)] * parsq[DEC3(i)];
				BlendDrv[INC3(i)] = 0.25 * par[INC3(i)] * (1 + j * par[i]) * parsq[DEC3(i)];
				BlendDrv[DEC3(i)] = 0.25 * par[DEC3(i)] * (1 + j * par[i]) * parsq[INC3(i)];

				/* Entity functions and derivatives for order p */
				modes = ent_quad(par[INC3(i)], par[DEC3(i)], i, p, Ent, EntDrv);
				for (m = 0; m < modes; m++) {

					/* Shape function */
					N[nshp] = Blend * Ent[m];

					/* Shape function derivatives */
					for (n = 0; n < 3; n++)
						dN[nshp][n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];

					nshp++;
				}
			}
		}
	}

	/* Hex */
	if (p >= 6) {

		/* Blending function */
		Blend = 0.125 * parsq[0] * parsq[1] * parsq[2];

		/* Blending function derivatives */
		BlendDrv[0] = 0.25 * par[0] * parsq[1] * parsq[2];
		BlendDrv[1] = 0.25 * par[1] * parsq[2] * parsq[0];
		BlendDrv[2] = 0.25 * par[2] * parsq[0] * parsq[1];

		/* Entity functions and derivatives for order p */
		modes = ent_hex(par[0], par[1], par[2], p, Ent, EntDrv);
		for (m = 0; m < modes; m++) {

			/* Shape function */
			N[nshp] = Blend * Ent[m];

			/* Shape function derivatives */
			for (n = 0; n < 3; n++)
				dN[nshp][n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];

			nshp++;
		}
	}

	/* Return number of shape functions */
	return nshp;
}

/* Compute all shape functions and derivatives for tetrahedron */
int TetShapeAndDrv(int p, double par[3], double N[], double dN[][3]) {
	int i, j, k, m, n, modes;
	int nshp = 0;
	double Blend, BlendDrv[4], par4[4], dN4[4];
	double *Ent;
	double *EntDrv[4];

	Ent = new double[100];
	for (i = 0; i < 4; i++)
		EntDrv[i] = new double[100];

	/* Create false coordinates */
	for (i = 0; i < 3; i++)
		par4[i] = par[i];
	par4[3] = 1 - par[0] - par[1] - par[2];

	/* Nodes */
	for (i = 0; i < 4; i++) {

		/* Shape function */
		N[nshp] = par4[i];

		/* Shape function derivatives */
		for (k = 0; k < 4; k++)
			dN4[k] = 0;
		dN4[i] = 1;
		REDUCE_DRV4

		nshp++;
	}

	/* Edges */
	if (p >= 2) {
		for (i = 0; i < 3; i++) {
			for (j = i + 1; j < 4; j++) {
				/* Edge between i and j */

				/* Blending function */
				Blend = -2 * par4[i] * par4[j];

				/* Blending function derivatives */
				for (k = 0; k < 4; k++)
					BlendDrv[k] = 0;
				BlendDrv[i] = -2 * par4[j];
				BlendDrv[j] = -2 * par4[i];

				/* Entity functions and derivatives for order p */
				modes = ent_edge2(par4[i], par4[j], i, j, p, Ent, EntDrv);
				for (m = 0; m < modes; m++) {

					/* Shape function */
					N[nshp] = Blend * Ent[m];

					/* Shape function derivatives */
					for (n = 0; n < 4; n++)
						dN4[n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];
					REDUCE_DRV4

					nshp++;
				}
			}
		}
	}

	/* Faces */
	if (p >= 3) {
		for (i = 0; i < 4; i++) {
			/* Face normal to i */

			/* Blending function */
			Blend = par4[INC4(i)] * par4[OPP4(i)] * par4[DEC4(i)];

			/* Blending function derivatives */
			BlendDrv[i] = 0;
			BlendDrv[INC4(i)] = par4[OPP4(i)] * par4[DEC4(i)];
			BlendDrv[OPP4(i)] = par4[INC4(i)] * par4[DEC4(i)];
			BlendDrv[DEC4(i)] = par4[INC4(i)] * par4[OPP4(i)];

			/* Entity functions and derivatives for order p */
			modes = ent_tri(par4[INC4(i)], par4[OPP4(i)], i, p, Ent, EntDrv);
			for (m = 0; m < modes; m++) {

				/* Shape function */
				N[nshp] = Blend * Ent[m];

				/* Shape function derivatives */
				for (n = 0; n < 4; n++)
					dN4[n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];
				REDUCE_DRV4

				nshp++;
			}
		}
	}

	/* Tet */
	if (p >= 4) {

		/* Blending function */
		Blend = par4[0] * par4[1] * par4[2] * par4[3];

		/* Blending function derivatives */
		BlendDrv[0] = par4[1] * par4[2] * par4[3];
		BlendDrv[1] = par4[0] * par4[2] * par4[3];
		BlendDrv[2] = par4[0] * par4[1] * par4[3];
		BlendDrv[3] = par4[0] * par4[1] * par4[2];

		/* Entity functions and derivatives for order p */
		modes = ent_tet(par4[0], par4[1], par4[2], p, Ent, EntDrv);
		for (m = 0; m < modes; m++) {

			/* Shape function */
			N[nshp] = Blend * Ent[m];

			/* Shape function derivatives */
			for (n = 0; n < 4; n++)
				dN4[n] = Blend * EntDrv[n][m] + BlendDrv[n] * Ent[m];
			REDUCE_DRV4

			nshp++;
		}
	}

	/* Return number of shape functions */
	return nshp;
}