#include <complex.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#define TEST

#define CGLM_ALL_UNALIGNED
#include <cglm/vec3.h>
#include <cglm/vec4.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifndef CMPLX
#define CMPLX(a, b) (a + I * b)
#endif

#ifdef TEST
#include <assert.h>
#endif

typedef void (*function_t)(float *, int *, int);

struct parm
{
	function_t f;
	unsigned char m, n;
	unsigned int grid;
} parm;

int factorial(int n)
{
	if (n == 1)
		return 1;

	return n * factorial(n - 1);
}

int numero_caras(int n)
{
	if (n == 2)
		return 1;

	return (1 << (n - 3)) * factorial(n) / factorial(n - 2);
}

void riemman(float *d_surface, int *coords, int grid)
{
	complex double eq;
	float u = 2 * ((float)coords[0] / grid) - 1;
	float v = 2 * ((float)coords[1] / grid) - 1;

	eq = csqrt(CMPLX(u, v));

	d_surface[0] = u;
	d_surface[1] = v;
	d_surface[2] = creal(eq);
	d_surface[3] = cimag(eq);
}

void cube(float *d_surface, int *coord, int grid)
{
	for (int i = 0; i < parm.m; i++)
		d_surface[i] = ((float)coord[i] / grid) - 0.5;
}

void sphere(float *d_surface, int *coord, int grid)
{
	for (int i = 0; i < parm.m; i++)
		d_surface[i] = ((float)coord[i] / grid) - 0.5;

	float norm = 0;
	for(int i = 0; i < parm.m; i++)
		norm += d_surface[i] * d_surface[i];

	for (int i = 0; i < parm.m; i++)
		d_surface[i] = d_surface[i] / sqrt(norm);
}

void lens(float *d_surface, int *coord, int grid)
{
	int p = 7;
	int q = 1;

	float sphere[4];
	for (int i = 0; i < 4; i++)
		sphere[i] = ((float)coord[i] / grid) - 0.5;

	float norm = 0;
	for(int i = 0; i < 4; i++)
		norm += sphere[i] * sphere[i];

	for (int i = 0; i < 4; i++)
		sphere[i] = sphere[i] / sqrt(norm);
	
	d_surface[0] = (sphere[0] * cos(2 * M_PI / p)) - (sphere[1] * sin(2 * M_PI / p));
	d_surface[1] = (sphere[0] * sin(2 * M_PI / p)) + (sphere[1] * cos(2 * M_PI / p));
	d_surface[2] = (sphere[2] * cos(2 * M_PI * q / p)) - (sphere[3] * sin(2 * M_PI * q / p));
	d_surface[3] = (sphere[2] * sin(2 * M_PI * q / p)) + (sphere[3] * cos(2 * M_PI * q / p));
}

void mobius(float *d_surface, int *coord, int grid)
{
	const float width = 0.5;
	float u = (2 * M_PI) * ((float)coord[0] / grid);
	float v = (2 * width) * ((float)coord[1] / grid) - width;

	d_surface[0] = cos(u) + v * cos(u / 2) * cos(u);
	d_surface[1] = sin(u) + v * cos(u / 2) * sin(u);
	d_surface[2] = v * sin(u / 2);
}

void torus(float *d_surface, int *coord, int grid)
{
	float u = (2 * M_PI) * ((float)coord[0] / grid);
	float v = (2 * M_PI) * ((float)coord[1] / grid);

	d_surface[0] = (1 + 0.5 * cos(v)) * cos(u);
	d_surface[1] = (1 + 0.5 * cos(v)) * sin(u);
	d_surface[2] = 0.5 * sin(v);
}

void klein(float *d_surface, int *coord, int grid)
{
	float u = (2 * M_PI) * ((float)coord[0] / grid);
	float v = (2 * M_PI) * ((float)coord[1] / grid);

	d_surface[0] = (0.5 * cos(v) + 0.5) * cos(u);
	d_surface[1] = (0.5 * cos(v) + 0.5) * sin(u);
	d_surface[2] = sin(v) * cos(u / 2);
	d_surface[3] = sin(v) * sin(u / 2);
}

float *generate_data_surface(unsigned char *s)
{
	unsigned int i, j, k, o, p, n;
	long size, q = 0;
	float *d_surface;

	int *cara;

	parm.f = lens;
	parm.m = 4;
	parm.n = 4;
	parm.grid = 5;

#ifdef TEST
	assert(numero_caras(2) == 1);
	assert(numero_caras(3) == 6);
	assert(numero_caras(4) == 24);
#endif

	cara = malloc(parm.m * sizeof(int));
	*s = parm.n;

	size = parm.grid * parm.grid * 6 * (parm.n) * numero_caras(parm.m);
	d_surface = malloc((size + 1) * sizeof(float));
	d_surface[0] = size;

	for (o = 0; o < parm.m; o++)
	{
		for (p = 0; p < o; p++)
		{
			for (k = 0; k < (1 << (parm.m - 2)); k++)
			{
				unsigned char skip = 0;
				for (n = 0; n < parm.m; n++)
				{
					if (n == o || n == p)
						skip++;

					cara[n] = (k & (1 << (n - skip))) ? parm.grid : 0;
				}

				for (i = 0; i < parm.grid; i++)
				{
					for (j = 0; j < parm.grid; j++)
					{
						cara[o] = i;
						cara[p] = j;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;

						cara[o] = i + 1;
						cara[p] = j;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;

						cara[o] = i + 1;
						cara[p] = j + 1;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;

						cara[o] = i;
						cara[p] = j;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;

						cara[o] = i;
						cara[p] = j + 1;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;

						cara[o] = i + 1;
						cara [p] = j + 1;
						parm.f(&d_surface[q + 1], cara, parm.grid);
						q += parm.n;
					}
				}
			}
		}
	}
#ifdef TEST
	assert(q == size);
#endif
	return d_surface;
}

static void __calculate_normal(
    float *p1, float *p2, float *p3, float *normal, unsigned char n)
{
    unsigned char i;
    float alpha;
    float *v1, *v2, *v3;
    float *u1, *u2, *u3;

    v1 = malloc(n * sizeof(float));
    v2 = malloc(n * sizeof(float));
    v3 = malloc(n * sizeof(float));
    u1 = malloc(n * sizeof(float));
    u2 = malloc(n * sizeof(float));
    u3 = malloc(n * sizeof(float));

    /* 
    	Calculate a normal vector of a plain using Gram-Schmidt process
    */
    {
        for (i = 0; i < n; ++i) {
            v1[i] = p2[i] - p1[i];  
            v2[i] = p3[i] - p1[i]; 
            v3[i] = p1[i];          
        }

        for (i = 0; i < n; ++i) {
            u1[i] = v1[i]; 
        }

        {
            float proj[n];
            float dot_v2_u1 = 0.0f, dot_u1_u1 = 0.0f;
            for (i = 0; i < n; ++i) {
                dot_v2_u1 += v2[i] * u1[i];
                dot_u1_u1 += u1[i] * u1[i];
            }
            alpha = dot_v2_u1 / dot_u1_u1;

            for (i = 0; i < n; ++i) {
                proj[i] = u1[i] * alpha;
                u2[i] = v2[i] - proj[i]; 
            }
        }

        {
            float proj1[n], proj2[n];
            float dot_v3_u1 = 0.0f, dot_u1_u1 = 0.0f;
            float dot_v3_u2 = 0.0f, dot_u2_u2 = 0.0f;

            for (i = 0; i < n; ++i) {
                dot_v3_u1 += v3[i] * u1[i];
                dot_u1_u1 += u1[i] * u1[i];
            }
            for (i = 0; i < n; ++i) {
                proj1[i] = u1[i] * (dot_v3_u1 / dot_u1_u1);
            }

            for (i = 0; i < n; ++i) {
                dot_v3_u2 += v3[i] * u2[i];
                dot_u2_u2 += u2[i] * u2[i];
            }
            for (i = 0; i < n; ++i) {
                proj2[i] = u2[i] * (dot_v3_u2 / dot_u2_u2);
                u3[i] = v3[i] - proj1[i] - proj2[i]; 
            }
        }

        float magnitude = 0.0f;
        for (i = 0; i < n; ++i) {
            magnitude += u3[i] * u3[i];
        }
        magnitude = sqrtf(magnitude);

        for (i = 0; i < n; ++i) {
            normal[i] = u3[i] / magnitude;  
        }

        free(v1);
        free(v2);
        free(v3);
        free(u1);
        free(u2);
        free(u3);
        return;
    }
}


float *generate_normals_surface(float *d, unsigned char m)
{
	float *n;
	n = malloc((*d + 1) * sizeof(float));
	*n = *d;

	float * norm_vec;
	norm_vec=malloc(m*sizeof(float));

	for (int i = 0; i < *d; i += 3 * m)
	{


		__calculate_normal(
			(d + 1) + i, (d + 1) + i + m, (d + 1) + i + 2 * m, norm_vec, m);
		glm_vec3_copy(norm_vec, (n + 1) + i);
		glm_vec3_copy(norm_vec, (n + 1) + i + m);
		glm_vec3_copy(norm_vec, (n + 1) + i + 2 * m);
	}

	free(norm_vec);
	return n;
}