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

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

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

void mobius(float *d_surface, int i, int j, int grid_size)
{
	const float width = 0.5;
	float u = (2 * M_PI) * ((float)i / grid_size);
	float v = (2 * width) * ((float)j / grid_size) - 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 i, int j, int grid_size)
{
	float u = (2 * M_PI) * ((float)i / grid_size);
	float v = (2 * M_PI) * ((float)j / grid_size);

	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 i, int j, int grid_size)
{
	float u = (2 * M_PI) * ((float)i / grid_size);
	float v = (2 * M_PI) * ((float)j / grid_size);

	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);
}

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

float *generate_data_surface(int grid_size, unsigned char *m)
{
	unsigned int i, j, k = 0;
	long size;
	function_t f;
	float *d_surface;

	f = klein;
	*m = 4;
	size = grid_size * grid_size * 6 * (*m);
	d_surface = malloc((size + 1) * sizeof(float));
	d_surface[0] = size;

	for (i = 0; i < grid_size; i++)
	{
		for (j = 0; j < grid_size; j++)
		{
			// triangle 1, Front
			f(&d_surface[k + 1], i, j, grid_size);
			k += *m;
			f(&d_surface[k + 1], i + 1, j, grid_size);
			k += *m;
			f(&d_surface[k + 1], i + 1, j + 1, grid_size);
			k += *m;

			// triangle 2, Back
			f(&d_surface[k + 1], i, j, grid_size);
			k += *m;
			f(&d_surface[k + 1], i, j + 1, grid_size);
			k += *m;
			f(&d_surface[k + 1], i + 1, j + 1, grid_size);
			k += *m;
		}
	}

	return d_surface;
}

/* pa' rearmar la funcion _calc_normal te entendi que creara las funciones de cglm artesanalmente, entonces ps eso hago xd */

void subtract(const float *v1, const float *v2, float *result, unsigned char n) 
{
    for (unsigned char i = 0; i < n; i++) {
        result[i] = v1[i] - v2[i];
    }
}

float dot_product(const float *a, const float *b, unsigned char n) 
{
    float result = 0.0f;
    for (unsigned char i = 0; i < n; i++) {
        result += a[i] * b[i];
    }
    return result;
}

void escalar_product(float a, const float *v1, float *result, unsigned char n) 
{
    for (unsigned char i = 0; i < n; i++) {
        result[i] = a * v1[i];
    }
}

void norm(const float *v1, float *result, unsigned char n) 
{
    float lenght = sqrtf(dot_product(v1, v1, n));
    float inv_lenght = 1.0f / lenght;
    escalar_product(inv_lenght, v1, result, n);
}

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));

	switch (n)
	{
	case 3:
		glm_vec3_sub(p2, p1, v1);
		glm_vec3_sub(p3, p1, v2);

		glm_vec3_cross(v1, v2, normal);
		glm_vec3_normalize(normal);
		return;
		/*
			In Grant-Shmidth we need 3 linearly independian vector that forms a
		   basis, so we can have a ortonormal version of that basis, since, we
		   must have v1 = p3 - p1 v2 = p2 - p1 Then v3 = p1, will most certantly
		   be linerly independiant to v1 and v2.
		*/
    default:
    	for( i=0; i<n; ++i )
    	{
    		v1[i]=p2[i]-p1[i]; //cglm_vec4_sub( p2, p1, v1 );
    		v2[i]=p3[i]-p1[i]; //cglm_vec4_sub( p3, p1, v2 );
    		v3[i]=p1[i];   //cglm_vec4_copy( p1, v3 );
        }

    	for( i=0; i<n; ++i )
    		u1[i]=v1[i];   //cglm_vec4_copy( v1, u1 );

        {
            vec4 proj;
            alpha = glm_vec4_dot(v2, u1) / glm_vec4_dot(u1, u1);
            glm_vec4_scale(u1, alpha, proj);
            glm_vec4_sub(v2, proj, u2);
        }
		
        {
            vec4 proj1, proj2;
            alpha = glm_vec4_dot(v3, u1) / glm_vec4_dot(u1, u1);
            glm_vec4_scale(u1, alpha, proj1);

            alpha = glm_vec4_dot(v3, u2) / glm_vec4_dot(u2, u2);
            glm_vec4_scale(u2, alpha, proj2);

            glm_vec4_sub(v3, proj1, u3);
            glm_vec4_sub(u3, proj2, u3);
        }

        glm_vec4_copy(u3, normal);
        glm_vec4_normalize(normal);

	free(v1);
	free(v2);
	free(v3);
	free(u1);
	free(u2);
	free(u3);
        return;
#if 0
	default:
		u = malloc((n - 1) * sizeof(float *));
		for (unsigned char i = 0; i < n - 1; i++)
		{
			u[i] = malloc(n * sizeof(float));
		}

		for (unsigned char i = 0; i < n - 1; i++)
		{
			float *vi = malloc(n * sizeof(float));
			for (unsigned char j = 0; j < n; j++)
			{
				vi[j] = p2[j] - p1[j];
			}

			for (unsigned char j = 0; j < i; j++)
			{
				float dot_vu = 0.0f, dot_uu = 0.0f;

				for (unsigned char k = 0; k < n; k++)
				{
					dot_vu += vi[k] * u[j][k];
					dot_uu += u[j][k] * u[j][k];
				}

				for (unsigned char k = 0; k < n; k++)
				{
					vi[k] -= (dot_vu / dot_uu) * u[j][k];
				}
			}

			memcpy(u[i], vi, n * sizeof(float));
			free(vi);
		}

		memcpy(normal, u[n - 2], n * sizeof(float));

		float norm = 0.0f;
		for (unsigned char i = 0; i < n; i++)
		{
			norm += normal[i] * normal[i];
		}
		norm = sqrtf(norm);
		for (unsigned char i = 0; i < n; i++)
		{
			normal[i] /= norm;
		}

		for (unsigned char i = 0; i < n - 1; i++)
		{
			free(u[i]);
		}
		free(u);
		return;
#endif
	}
}

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;
}