#include <cedos.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define GMODE_TEXT 0x02
#define GMODE_VIDEO 0x13

#define VGA_BUFFER (char*)(0xA0000)

#define VGA_MODE_13_WIDTH 320
#define VGA_MODE_13_HEIGHT 200

#define MAX2(x, y) ((x) > (y) ? (x) : (y))
#define MIN2(x, y) ((x) < (y) ? (x) : (y))

#define MAX3(x, y, z) (MAX2(x, MAX2(y, z)))
#define MIN3(x, y, z) (MIN2(x, MIN2(y, z)))

#define MAX4(x, y, z, w) (MAX2(MAX2(x, y), MAX2(z, w)))
#define MIN4(x, y, z, w) (MIN2(MIN2(x, y), MIN2(z, w)))

const float pi = 3.14159f;
const float pi_2 = pi / 2.0f;
const float tau = pi * 2.0f;

const float sin_lut[257] = {
	0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f, 0.036807f,
	0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f, 0.073565f, 0.079682f,
	0.085797f, 0.091909f, 0.098017f, 0.104122f, 0.110222f, 0.116319f, 0.122411f,
	0.128498f, 0.134581f, 0.140658f, 0.146730f, 0.152797f, 0.158858f, 0.164913f,
	0.170962f, 0.177004f, 0.183040f, 0.189069f, 0.195090f, 0.201105f, 0.207111f,
	0.213110f, 0.219101f, 0.225084f, 0.231058f, 0.237024f, 0.242980f, 0.248928f,
	0.254866f, 0.260794f, 0.266713f, 0.272621f, 0.278520f, 0.284408f, 0.290285f,
	0.296151f, 0.302006f, 0.307850f, 0.313682f, 0.319502f, 0.325310f, 0.331106f,
	0.336890f, 0.342661f, 0.348419f, 0.354164f, 0.359895f, 0.365613f, 0.371317f,
	0.377007f, 0.382683f, 0.388345f, 0.393992f, 0.399624f, 0.405241f, 0.410843f,
	0.416430f, 0.422000f, 0.427555f, 0.433094f, 0.438616f, 0.444122f, 0.449611f,
	0.455084f, 0.460539f, 0.465976f, 0.471397f, 0.476799f, 0.482184f, 0.487550f,
	0.492898f, 0.498228f, 0.503538f, 0.508830f, 0.514103f, 0.519356f, 0.524590f,
	0.529804f, 0.534998f, 0.540171f, 0.545325f, 0.550458f, 0.555570f, 0.560662f,
	0.565732f, 0.570781f, 0.575808f, 0.580814f, 0.585798f, 0.590760f, 0.595699f,
	0.600616f, 0.605511f, 0.610383f, 0.615232f, 0.620057f, 0.624859f, 0.629638f,
	0.634393f, 0.639124f, 0.643832f, 0.648514f, 0.653173f, 0.657807f, 0.662416f,
	0.667000f, 0.671559f, 0.676093f, 0.680601f, 0.685084f, 0.689541f, 0.693971f,
	0.698376f, 0.702755f, 0.707107f, 0.711432f, 0.715731f, 0.720003f, 0.724247f,
	0.728464f, 0.732654f, 0.736817f, 0.740951f, 0.745058f, 0.749136f, 0.753187f,
	0.757209f, 0.761202f, 0.765167f, 0.769103f, 0.773010f, 0.776888f, 0.780737f,
	0.784557f, 0.788346f, 0.792107f, 0.795837f, 0.799537f, 0.803208f, 0.806848f,
	0.810457f, 0.814036f, 0.817585f, 0.821103f, 0.824589f, 0.828045f, 0.831470f,
	0.834863f, 0.838225f, 0.841555f, 0.844854f, 0.848120f, 0.851355f, 0.854558f,
	0.857729f, 0.860867f, 0.863973f, 0.867046f, 0.870087f, 0.873095f, 0.876070f,
	0.879012f, 0.881921f, 0.884797f, 0.887640f, 0.890449f, 0.893224f, 0.895966f,
	0.898674f, 0.901349f, 0.903989f, 0.906596f, 0.909168f, 0.911706f, 0.914210f,
	0.916679f, 0.919114f, 0.921514f, 0.923880f, 0.926210f, 0.928506f, 0.930767f,
	0.932993f, 0.935183f, 0.937339f, 0.939459f, 0.941544f, 0.943593f, 0.945607f,
	0.947586f, 0.949528f, 0.951435f, 0.953306f, 0.955141f, 0.956940f, 0.958703f,
	0.960431f, 0.962121f, 0.963776f, 0.965394f, 0.966976f, 0.968522f, 0.970031f,
	0.971504f, 0.972940f, 0.974339f, 0.975702f, 0.977028f, 0.978317f, 0.979570f,
	0.980785f, 0.981964f, 0.983105f, 0.984210f, 0.985278f, 0.986308f, 0.987301f,
	0.988258f, 0.989177f, 0.990058f, 0.990903f, 0.991710f, 0.992480f, 0.993212f,
	0.993907f, 0.994565f, 0.995185f, 0.995767f, 0.996313f, 0.996820f, 0.997290f,
	0.997723f, 0.998118f, 0.998476f, 0.998795f, 0.999078f, 0.999322f, 0.999529f,
	0.999699f, 0.999831f, 0.999925f, 0.999981f, 1.000000f
};

float sinf(float x)
{
	float scale = 256.0f / pi_2;
	float sign = 1.0f;

	if (x < 0.0f) {
		sign = -1.0f;
		x = -x;
	}

	if (x > tau) {
		int phases = (int)(x / tau);
		x -= (float)(phases) * tau;
	}

	if (x > pi) {
		x -= pi;
		sign = -sign;
	}

	if (x < pi_2) {
		int i = (int)(x * scale);
		return sign * sin_lut[i];
	} else {
		int i = (int)((x - pi_2) * scale);
		return sign * sin_lut[256 - i];
	}
}

float cosf(float x) {
	return sinf(x + pi_2);
}

typedef float scalar_t;

struct vector {
	scalar_t x;
	scalar_t y;
	scalar_t z;
	scalar_t w;
};

struct matrix {
	struct vector col_x;
	struct vector col_y;
	struct vector col_z;
	struct vector col_w;
};

void vect2_print(const char *name, const struct vector *v) {
	printf("%s: %i, %i\n", name, (int)(v->x), (int)(v->y));
}

void vect2_add(struct vector *acc, const struct vector *b) {
	acc->x += b->x;
	acc->y += b->y;
}

void vect2_sub(struct vector *acc, const struct vector *b) {
	acc->x -= b->x;
	acc->y -= b->y;
}

void vect2_pwmul(struct vector *acc, const struct vector *b) {
	acc->x *= b->x;
	acc->y *= b->y;
}

scalar_t vect2_dot(const struct vector *a, const struct vector *b) {
	scalar_t acc = 0.0f;
	acc += a->x * b->x;
	acc += a->y * b->y;
	return acc;
}

scalar_t vect2_sqlen(const struct vector *vec) {
	return vect2_dot(vec, vec);
}

struct vector vect2(scalar_t x, scalar_t y) {
	return (struct vector) {
		.x = x,
		.y = y,
	};
}


struct vector vect2_copy(const struct vector *vec) {
	return (struct vector) {
		.x = vec->x,
		.y = vec->y,
	};
}

void vect2_perp(struct vector *vec) {
	scalar_t tmp = vec->x;
	vec->x = vec->y;
	vec->y = -tmp;
}

void vect3_add(struct vector *acc, const struct vector *b) {
	acc->x += b->x;
	acc->y += b->y;
	acc->z += b->z;
}

void vect3_sub(struct vector *acc, const struct vector *b) {
	acc->x -= b->x;
	acc->y -= b->y;
	acc->z -= b->z;
}

void vect3_pwmul(struct vector *acc, const struct vector *b) {
	acc->x *= b->x;
	acc->y *= b->y;
	acc->z *= b->z;
}

void vect3_scale(struct vector *acc, scalar_t scale) {
	acc->x *= scale;
	acc->y *= scale;
	acc->z *= scale;
}

scalar_t vect3_dot(const struct vector *a, const struct vector *b) {
	scalar_t acc = 0.0f;
	acc += a->x + b->x;
	acc += a->y + b->y;
	acc += a->z + b->z;
	return acc;
}

scalar_t vect3_sqlen(const struct vector *vec) {
	return vect3_dot(vec, vec);
}

struct vector vect3(scalar_t x, scalar_t y, scalar_t z) {
	return (struct vector) {
		.x = x,
		.y = y,
		.z = z,
		.w = 1.0f
	};
}

struct vector vect3_copy(const struct vector *vec) {
	return (struct vector) {
		.x = vec->x,
		.y = vec->y,
		.z = vec->z,
	};
}

struct vector vect4_copy(const struct vector *vec) {
	return (struct vector) {
		.x = vec->x,
		.y = vec->y,
		.z = vec->z,
		.w = vec->w,
	};
}

void vect4_add(struct vector *acc, const struct vector *b) {
	acc->x += b->x;
	acc->y += b->y;
	acc->z += b->z;
	acc->w += b->w;
}

void vect4_add_scaled(struct vector *acc, const struct vector *b, scalar_t scale) {
	acc->x += b->x * scale;
	acc->y += b->y * scale;
	acc->z += b->z * scale;
	acc->w += b->w * scale;
}

void vect4_pwmul(struct vector *acc, const struct vector *b) {
	acc->x *= b->x;
	acc->y *= b->y;
	acc->z *= b->z;
	acc->w *= b->w;
}

void vect4_scale(struct vector *acc, scalar_t scale) {
	acc->x *= scale;
	acc->y *= scale;
	acc->z *= scale;
	acc->w *= scale;
}

scalar_t vect4_dot(const struct vector *a, const struct vector *b) {
	scalar_t acc = 0.0f;
	acc += a->x + b->x;
	acc += a->y + b->y;
	acc += a->z + b->z;
	acc += a->w + b->w;
	return acc;
}

struct vector vect4(scalar_t x, scalar_t y, scalar_t z, scalar_t w) {
	return (struct vector) {
		.x = x,
		.y = y,
		.z = z,
		.w = w
	};
}

void vect3_matmul(struct vector *vec, const struct matrix *m) {
	struct vector copy = vect4_copy(vec);

	vec->x = 0.0f;
	vec->y = 0.0f;
	vec->z = 0.0f;
	vec->w = 0.0f;

	vect4_add_scaled(vec, &(m->col_x), copy.x);
	vect4_add_scaled(vec, &(m->col_y), copy.y);
	vect4_add_scaled(vec, &(m->col_z), copy.z);
	vect4_add_scaled(vec, &(m->col_w), copy.w);
}

void mat_matmul(struct matrix *acc, const struct matrix *b) {
	struct vector tmp;

	tmp = vect4(acc->col_x.x, acc->col_y.x, acc->col_z.x, acc->col_w.x);
	acc->col_x.x = vect4_dot(&tmp, &(b->col_x));
	acc->col_y.x = vect4_dot(&tmp, &(b->col_y));
	acc->col_z.x = vect4_dot(&tmp, &(b->col_z));
	acc->col_w.x = vect4_dot(&tmp, &(b->col_w));

	tmp = vect4(acc->col_x.y, acc->col_y.y, acc->col_z.y, acc->col_w.y);
	acc->col_x.y = vect4_dot(&tmp, &(b->col_x));
	acc->col_y.y = vect4_dot(&tmp, &(b->col_y));
	acc->col_z.y = vect4_dot(&tmp, &(b->col_z));
	acc->col_w.y = vect4_dot(&tmp, &(b->col_w));

	tmp = vect4(acc->col_x.z, acc->col_y.z, acc->col_z.z, acc->col_w.z);
	acc->col_x.z = vect4_dot(&tmp, &(b->col_x));
	acc->col_y.z = vect4_dot(&tmp, &(b->col_y));
	acc->col_z.z = vect4_dot(&tmp, &(b->col_z));
	acc->col_w.z = vect4_dot(&tmp, &(b->col_w));

	tmp = vect4(acc->col_x.w, acc->col_y.w, acc->col_z.w, acc->col_w.w);
	acc->col_x.w = vect4_dot(&tmp, &(b->col_x));
	acc->col_y.w = vect4_dot(&tmp, &(b->col_y));
	acc->col_z.w = vect4_dot(&tmp, &(b->col_z));
	acc->col_w.w = vect4_dot(&tmp, &(b->col_w));
}

struct matrix mat_roty(scalar_t rad) {
	return (struct matrix) {
		.col_x = (struct vector) { cosf(rad), 0.0f, -sinf(rad), 0.0f },
		.col_y = (struct vector) { 0.0f, 1.0f, 0.0f, 0.0f },
		.col_z = (struct vector) { sinf(rad), 0.0f, cosf(rad), 0.0f },
		.col_w = (struct vector) { 0.0f, 0.0f, 0.0f, 1.0f },
	};
}

struct matrix mat_proj(scalar_t focal_len, scalar_t near, scalar_t far) {
	scalar_t scale = 1.0f / (near - far);
	scalar_t alpha = (near + far) * scale;
	scalar_t beta = 2 * near * far * scale;

	return (struct matrix) {
		.col_x = (struct vector) { focal_len, 0.0f, 0.0f, 0.0f },
		.col_y = (struct vector) { 0.0f, focal_len, 0.0f, 0.0f },
		.col_z = (struct vector) { 0.0f, 0.0f, alpha, 1.0f },
		.col_w = (struct vector) { 0.0f, 0.0f, beta, 0.0f },
	};
}

int vect2_leftside(
		const struct vector *a, const struct vector *b,
		const struct vector *test
) {
	struct vector v1 = vect2_copy(test);
	struct vector v2 = vect2_copy(b);

	vect2_sub(&v1, a);
	vect2_sub(&v2, a);

	vect2_perp(&v2);

	return (vect2_dot(&v1, &v2) > 0.0f);
}

void raster_tri(char *framebuf, const struct vector *v1, const struct vector *v2, const struct vector *v3, char color) {
	struct vector a = vect3_copy(v2);
	struct vector b = vect3_copy(v3);

	vect3_sub(&a, v1);
	vect3_sub(&b, v1);

	scalar_t cross_z = (a.x * b.y) - (a.y * b.x);

	if (cross_z < 0.0f) {
		return;
	}

	int min_x = MIN4(v1->x, v2->x, v3->x, VGA_MODE_13_WIDTH);
	int min_y = MIN4(v1->y, v2->y, v3->y, VGA_MODE_13_HEIGHT);

	int max_x = MAX4(v1->x, v2->x, v3->x, 0);
	int max_y = MAX4(v1->y, v2->y, v3->y, 0);

	for (int x = (int)min_x; x <= (int)max_x; x++) {
		for (int y = (int)min_y; y <= (int)max_y; y++) {
			const struct vector vtest = vect2((scalar_t)x, (scalar_t)y);

			int inside = 1;
			inside = inside && !vect2_leftside(v1, v2, &vtest);
			inside = inside && !vect2_leftside(v2, v3, &vtest);
			inside = inside && !vect2_leftside(v3, v1, &vtest);

			int g_i = y * VGA_MODE_13_WIDTH + x;

			if (inside) {
				framebuf[g_i] = color;
			}
		}
	}
}

void main(char *args) {
	(void)args;

	char *vgabuf = VGA_BUFFER;
	char *framebuf = malloc(VGA_MODE_13_WIDTH * VGA_MODE_13_HEIGHT);

    // switch video mode and display image
    graphics_set_mode(GMODE_VIDEO);

	struct vector cube[8] = {
		vect3(0.0f, 0.0f, 0.0f),
		vect3(0.0f, 0.0f, 1.0f),
		vect3(0.0f, 1.0f, 0.0f),
		vect3(0.0f, 1.0f, 1.0f),
		vect3(1.0f, 0.0f, 0.0f),
		vect3(1.0f, 0.0f, 1.0f),
		vect3(1.0f, 1.0f, 0.0f),
		vect3(1.0f, 1.0f, 1.0f),
	};

	struct vector vectors[8];

	struct matrix proj = mat_proj(1.0f, 1.0f, 3.0f);
	scalar_t rot = 0.1f;

	while (1) {
		struct matrix roty = mat_roty(rot);
		rot += 0.05f;

		memset(framebuf, 0, VGA_MODE_13_WIDTH * VGA_MODE_13_HEIGHT);

		for (int i = 0; i < 8; i++) {
			const struct vector pre_rot = { .x = -0.5f, .y = -0.5f, .z = -0.5f, .w = 0.0f };
			const struct vector post_rot = { .x = 0.5f, .y = 0.0f, .z = 0.0f, .w = 0.0f };
			const struct vector scale = { .x = 100.0f, .y = 100.0f, .z = 100.0f, .w = 1.0f };
			const struct vector offset = { .x = 110.0f, .y = 50.0f, .z = 0.0f, .w = 0.0f };

			vectors[i] = vect4_copy(&cube[i]);

			vect3_add(&vectors[i], &pre_rot);
			vect3_matmul(&vectors[i], &roty);
			vect3_add(&vectors[i], &post_rot);
			vect3_matmul(&vectors[i], &proj);

			vectors[i].x /= vectors[i].z;
			vectors[i].y /= vectors[i].z;
			vect3_pwmul(&vectors[i], &scale);
			vect3_add(&vectors[i], &offset);
		}

		/* front */
		raster_tri(framebuf, &vectors[0], &vectors[4], &vectors[6], 1);
		raster_tri(framebuf, &vectors[0], &vectors[6], &vectors[2], 2);

		/* right */
		raster_tri(framebuf, &vectors[4], &vectors[5], &vectors[7], 3);
		raster_tri(framebuf, &vectors[4], &vectors[7], &vectors[6], 4);

		/* back */
		raster_tri(framebuf, &vectors[7], &vectors[5], &vectors[1], 5);
		raster_tri(framebuf, &vectors[3], &vectors[7], &vectors[1], 6);

		memcpy(vgabuf, framebuf, VGA_MODE_13_WIDTH * VGA_MODE_13_HEIGHT);
	}

	while (1) {}

    graphics_set_mode(GMODE_TEXT);
}