#include <stdio.h>
#include <string.h>
#include <stdarg.h>

// Gba.h has all the register definitions
#include "gba.h"     //from Eloist Wire cube Demo, defines the main GBA registers

#include "keypad.h"  //All the keypad definitions thanks to nokturn

///REG_DISPCNT Defines////
#define BG0_ENABLE		0x100    //these are just the flags for enabling backgrounds and sprites
#define BG1_ENABLE		0x200
#define BG2_ENABLE		0x400
#define BG3_ENABLE		0x800
#define OBJ_ENABLE		0x1000
#define WIN0_ENABLE		0x2000
#define WIN1_ENABLE		0x4000
#define WINOBJ_ENABLE	0x8000


#define OBJ_MAP_LINEAR  0x40     //I'll talk about this when i talk about hardware sprites
#define OBJ_MAP_2D      0x0
#define BACKBUFFER     0x10     //this is the flag that controls which buffer is being rendered
	

#define MODE0			0x0     //these are the modes
#define MODE1			0x1
#define MODE2			0x2
#define MODE3			0x3
#define MODE4			0x4
#define MODE5			0x5

#define RGB16(r,g,b)  ((r)+(g<<5)+(b<<10))  //this converts a color value to 15 bit BGR value used by GBA
								//you can have 0-31 levels of each red,green,blue component.			

#define SCREENWIDTH 120  //width of the screen in pixels
#define SCREENHEIGHT 160 //height
#define PALETTE_SIZE 256
#define PI 3.14159

#define draw_pixel(x, y, c) if ((x >= 0) && (y >= 0) && (x < SCREENWIDTH) && (y < SCREENHEIGHT)) screenbuffer[(x)+(y)*SCREENWIDTH] = (c)

extern int sintable[2048];

typedef s32 Fixed;
//#define int_to_fix(x) (Fixed)((x) << 16)
//#define fix_to_u8(x) (u8)((x) >> 16)
#define f2i(x) ((x) >> 8)
#define i2f(x) ((x) << 8)
#define r2i(x) ((x) >> 16)
#define fixmul(x, y) f2i((x)*(y))
#define fixdiv(x, y) (i2f(x) / (y))

#define fixsin(x) sintable[(x) & 2047]
#define fixcos(x) fixsin((x)+512)


typedef struct { // The vertices need to be given clockwise
	int x[3];
	int y[3];
} Triangle;

typedef struct {
        int x;
	int y;
	int z;
} Vertex;

typedef struct {
	int x[4];
	int y[4];
	int z[4];
	u8 col;
} Face;

typedef struct {
	int x[4];
	int y[4];
} Polygon;

/* Function Prototypes */
void flip_buffer();
void wait_for_vblank();
void rotate_point(u16 angle, int x, int y, int center_x, int center_y, int* x_, int* y_);
void draw_line(int x1, int y1, int x2, int y2);
void fill_polygon(Polygon* poly, u8 col);
void fill_triangle(int* x, int* y, u8 col);
void dprintf(const char*, ...);
void draw_cube(u16 ax, u16 ay, u16 az);

/* Global Variables */
u16* frontbuffer = (u16*)0x6000000;
u16* backbuffer = (u16*)0x600A000;
u16* screenbuffer;
u16* palette = (u16*)0x5000000;
u16* OAM = (u16*)0x7000000;
s32 SIN[360], COS[360];
//Fixed DIV1_X[2*SCREENHEIGHT];

int main()
{
   int x[4], y[4], x_[4], y_[4];
   s16 i, j;
   int angle;
   Polygon poly, poly_;

   Triangle poly2, poly2_;

   REG_DISPCNT = MODE4 | BG2_ENABLE;	
   screenbuffer = backbuffer;	// start drawing on the backbuffer

   for (i = 0; i < PALETTE_SIZE; i++) // Generate the palette. UGLY.
	   /*palette[i] = RGB16((int)(16 + 15 * cos(i * PI / 64 + (double)31337/74)), 
			   (int)(16 + 15 * sin(i * PI / 64 + (double)31337/63)),
			   (int)(16 - 15 * cos(i * PI / 64 + (double)31337/81)));*/
	   palette[i] = 0x101 * i; // 0xFFFF / 0xFF = 0x101

   //for (i = 0; i < 360; i++) {	// precalculate SIN and COS (degrees)
//	   COS[i] = (s32)(cos(i * PI / 180) * 65536);
   //   SIN[i] = (s32)(sin(i * PI / 180) * 65536);
   // }

   /*for (i = 0; i < 2 * SCREENHEIGHT; i++) {
	   DIV1_X[i] = ((int_to_fix(1) >> 16) / int_to_fix(i - SCREENHEIGHT));
   }*/

   poly.x[0] = 40; poly.y[0] = 61; // to avoid division by 0
   poly.x[1] = 80; poly.y[1] = 60;
   poly.x[2] = 80; poly.y[2] = 100;
   poly.x[3] = 40; poly.y[3] = 100;

   poly2.x[0] = 40; poly2.y[0] = 61;
   poly2.x[2] = 80; poly2.y[2] = 60;
   poly2.x[1] = 60; poly2.y[1] = 40;

   while (1) {

	   for(i = 0; i < SCREENWIDTH; i++) {
		   for(j = 0; j < SCREENHEIGHT; j++) {
			   draw_pixel(i, j, (0xFF << 8) + 0xFF);
		   }
	   }

	   
	   /*for (i = 0; i < 4; i++) {
		   rotate_point(angle, poly.x[i], poly.y[i], 60, 80, &poly_.x[i], &poly_.y[i]);
	   }
	   for (i = 0; i < 3; i++) {
		   rotate_point(angle, poly2.x[i], poly2.y[i], 60, 80, &poly2_.x[i], &poly2_.y[i]);
	   }

	   //fill_triangle(x_, y_, 0x00);
	   fill_polygon(&poly_, 0x00);
	   fill_triangle(poly2_.x, poly2_.y, 0xD8);
	   draw_pixel(poly_.x[0], poly_.y[0], 0xD8D8);
	   draw_pixel(poly_.x[1], poly_.y[1], 0xD8D8);
	   draw_pixel(poly_.x[2], poly_.y[2], 0xD8D8);
	   draw_pixel(poly_.x[3], poly_.y[3], 0xD8D8);
	   angle += 5;
	   if (angle >= 360)
		   angle = 0;*/
   

	   angle += 30;
	   if (angle >= 2048)
	   	   angle = 0;
	  
	   draw_cube(angle, -angle, angle);
	   wait_for_vblank();
	   flip_buffer();
   }
}

void fill_polygon(Polygon* poly, u8 col) {
	Triangle tri;
	
	tri.x[0] = poly->x[0]; tri.y[0] = poly->y[0];
	tri.x[1] = poly->x[2]; tri.y[1] = poly->y[2];
	tri.x[2] = poly->x[3]; tri.y[2] = poly->y[3];

	fill_triangle(poly->x, poly->y, col);
	fill_triangle(tri.x, tri.y, col);
	//dprintf("0(%d, %d) 1(%d, %d) 2(%d, %d) 3(%d, %d)\n", poly->x[0], poly->y[0], poly->x[1], poly->y[1], poly->x[2], poly->y[2], tri.x[3], poly->y[3]);
	
}

void horiz_line(int x1, int x2, int y, u8 col)
{
	int i;

	if (x2 < x1) {
		i = x1;
		x1 = x2;
		x2 = i;
	}

	for (i = x1; i < x2; i++)
		screenbuffer[i + y * SCREENWIDTH] = (col << 8) + col;
}

void xchg(int* a, int* b)
{
	int temp;
	temp = *a;
	*a = *b;
	*b = temp;
}
	

void fill_triangle(int* x_, int* y_, u8 col) {
#if 0
	int A, B, C;
	int x_, y_, start_x, end_x, mid_y_vertex, end_y_vertex;

	// Select the starting vertex
	if ((y[0] < y[1]) && (y[0] < y[2])) {
		A = 0; B = 1; C = 2;
	} else if ((y[1] < y[2]) && (y[1] < y[0])) {
		A = 1; B = 2; C = 0;
	} else {
		A = 2; B = 0; C = 1;
	}

	//dprintf("0(%d, %d) 1(%d, %d) 2(%d, %D) A: %d B: %d C: %d\n", x[0], y[0], x[1], y[1], x[2], y[2], A, B, C);

	if (y[B] < y[C]) {
		mid_y_vertex = B; end_y_vertex = C;
	} else {
		mid_y_vertex = C; end_y_vertex = B;
	}

	
	/*
	 * On (AB): y = m * x + b, x = (y - b) / m.
	 * b = (xA yB - xB yA) / (xA - xB)
	 * m = (yA - yB) / (xA - xB)
	 * After some algebra, x = (y * (xA - xB) - (xA yB - xB yA)) / (yA - yB)
	 */

	for (y_ = y[A]; y_ < y[mid_y_vertex]; y_++) { // top triangle
		if (y[A] == y[C])
			start_x = x[A];
		else
			start_x = (y_ * (x[A] - x[C]) - (x[A] * y[C] - x[C] * y[A])) / (y[A] - y[C]);
		 if (y[A] == y[B])
			 end_x = x[A];
		 else
			 end_x = (y_ * (x[A] - x[B]) - (x[A] * y[B] - x[B] * y[A])) / (y[A] - y[B]);

		 //for (x_ = start_x; x_ < end_x; x_++) {
		 //	screenbuffer[x_ + y_ * SCREENWIDTH] = (col << 8) + col;
		 //}
		 horiz_line(start_x, end_x, y_, col);
	}

	for (y_ = y[mid_y_vertex]; y_ < y[end_y_vertex]; y_++) { // bottom triangle
		if (mid_y_vertex == C) { // C is above B
			if (y[C] == y[B])
				start_x = x[B];
			else
				start_x = (y_ * (x[C] - x[B]) - (x[C] * y[B] - x[B] * y[C])) / (y[C] - y[B]);
			if (y[A] == y[B])
				end_x = x[A];
			else
				end_x = (y_ * (x[A] - x[B]) - (x[A] * y[B] - x[B] * y[A])) / (y[A] - y[B]);
		} else { // B is above C
			if (y[B] == y[C])
				end_x = x[B];
			else
				end_x = (y_ * (x[B] - x[C]) - (x[B] * y[C] - x[C] * y[B])) / (y[B] - y[C]); // on BC
			if (y[A] == y[C])
				start_x = x[A];
			else
				start_x = (y_ * (x[A] - x[C]) - (x[A] * y[C] - x[C] * y[A])) / (y[A] - y[C]);
		}

		for (x_ = start_x; x_ < end_x; x_++) {
			screenbuffer[x_ + y_ * SCREENWIDTH] = (col << 8) + col;
		}
	}
#else
	int dx[3], dy[3], d[3];
	int x[3], y[3];
	int i, sx, sy, ex, ey;
	int A = 0, B = 1, C = 2;

	for (i = 0; i < 3; i++) {
		x[i] = x_[i];
		y[i] = y_[i];
	}

	/*if ((y[0] <= y[1]) && (y[1] <= y[2])) {
		y[0] = y[0];
	} else if ((y[0] <= y[1]) && (y[2] <= y[1]) && (y[0] <= y[2])) {
		y[0] = y[0];
		y[1] = y_[2]; x[1] = x_[2];
		y[2] = y_[1]; x[2] = y_[1];
	} else if ((y[1] <= y[0]) && (y[0] <= y[2])) {
		y[0] = y_[1]; x[0] = x_[1];
		y[1] = y_[0]; x[1] = x_[0];
		y[2] = y_[2]; x[2] = x_[2];
	} else if ((y[1] <= y[0]) && (y[2] <= y[0]) && (y[1] <= y[2])) {
		y[0] = y_[1]; x[0] = x_[1];
		y[1] = y_[2]; x[1] = x_[2];
		y[2] = y_[0]; x[2] = x_[0];
	} else if ((y[2] <= y[0]) && (y[0] <= y[1])) {
		y[0] = y_[2]; x[0] = x_[2];
		y[1] = y_[0]; x[1] = x_[0];
		y[2] = y_[1]; x[2] = x_[1];
	} else if ((y[2] <= y[0]) && (y[1] <= y[0]) && (y[2] <= y[1])) {
		y[0] = y_[2]; x[0] = x_[2];
		y[1] = y_[1]; x[1] = x_[1];
		y[2] = y_[0]; x[2] = x_[0];
		}*/
	

	if (y[1] < y[0]) {
		xchg(&y[0], &y[1]);
		xchg(&x[0], &x[1]);
	}
	if (y[2] < y[1]) {
		xchg(&y[1], &y[2]);
		xchg(&x[1], &x[2]);
	}
	if (y[1] < y[0]) {
		xchg(&y[0], &y[1]);
		xchg(&x[0], &x[1]);
	}
#if 0
	if (y[1] - y[0] > 0)
		dx[0] = fixdiv(i2f(x[1] - x[0]), i2f(y[1] - y[0]));
	else
		dx[0] = 0;

	if (y[2] - y[0] > 0)
		dx[1] = fixdiv(i2f(x[2] - x[0]), i2f(y[2] - y[0]));
	else
		dx[i] = 0;

	if (y[2] - y[1] > 0)
		dx[2] = fixdiv(i2f(x[2] - x[1]), i2f(y[2] - y[1]));
	else
		dx[2] = 0;

	sx = ex = i2f(x[0]);
	sy = ey = y[0];

	if (dx[0] > dx[1]) {
		for (; sy <= y[1]; sy++, ey++, sx += dx[1], ex += dx[0])
			horiz_line(f2i(sx), f2i(ex), sy, col);
		ex = i2f(x[1]);
		for (; sy <= y[2]; sy++, ey++, sx += dx[1], ex += dx[2])
			horiz_line(f2i(sx), f2i(ex), sy, col);
	} else {
		for (; sy <= y[1]; sy++, ey++, sx += dx[0], ex += dx[1])
			horiz_line(f2i(sx), f2i(ex), sy, col);
		sx = i2f(x[1]);
		for (; sy <= y[2]; sy++, ey++, sx += dx[2], ex += dx[1])
			horiz_line(f2i(sx), f2i(ex), sy, col);	
	}
	
#else
	dx[0] = x[1] - x[0];
	dy[0] = y[1] - y[0];
	dx[1] = x[2] - x[1];
	dy[1] = y[2] - y[1];
	dx[2] = x[0] - x[2];
	dy[2] = y[0] - y[2];

	for (i = 0; i < 3; i++)
		if (dy[i] != 0)
			d[i] = fixdiv(i2f(dx[i]), i2f(dy[i]));
		else
			d[i] = 0;

	for (i = y[0]; i < y[1]; i++)
		horiz_line(f2i(i2f(x[0]) + fixmul(i2f(i - y[0]), d[0])),
			   f2i(i2f(x[0]) + fixmul(i2f(i - y[0]), d[2])), i, col);

	for (i = y[1]; i < y[2]; i++)
		horiz_line(f2i(i2f(x[1]) + fixmul(i2f(i - y[1]), d[1])),
			   f2i(i2f(x[0]) + fixmul(i2f(i - y[0]), d[2])), i, col);
#endif
#endif
}

void dprint(const char* s)
{
	asm volatile("mov r2, %0\nldr r0, =0xc0ded00d\nmov r1, #0\nand r0, r0, r0\n" :
		     /* No output */ :
		     "r" (s) :
		     "r0", "r1", "r2");
}

void dprintf(const char* s, ...)
{
	char temp[1024];
	va_list ap;
	va_start(ap,s);
	vsprintf(temp, s, ap);
	dprint(temp);
}


void rotate_point(u16 angle, int x, int y, int center_x, int center_y, int* x_, int* y_) {
	*x_ = ((x - center_x) * COS[angle] >> 16) - ((y - center_y) * SIN[angle] >> 16) + center_x;
	*y_ = ((y - center_y) * COS[angle] >> 16) + ((x - center_x) * SIN[angle] >> 16) + center_y;
}

void draw_line(int x1, int y1, int x2, int y2) {
	s8 dx, dy;
	s8 dist, i, j;

	dist = (int)sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); // sqrt :)
	dx = x2 - x1;
	dy = y2 - y1;

	if (dist == 0) {
		draw_pixel(x1, y1, (0x00 << 8) + 0x00);
		return;
	}

	for (i = 0; i < dist; i++) {
		draw_pixel((x1+i*dx/dist), (y1+i*dy/dist), (0x00 << 8) + 0x00);
	}
	draw_pixel(x2, y2, (0x00 << 8) + 0x00);
}

void wait_for_vblank() {
	while (REG_VCOUNT < 160);
}

void flip_buffer() {
	if (REG_DISPCNT & BACKBUFFER) {
		REG_DISPCNT &= ~BACKBUFFER;
		screenbuffer = backbuffer;
	} else {
		REG_DISPCNT |= BACKBUFFER;
		screenbuffer = frontbuffer;
	}
}

void proj_screen(int x, int y, int z, int *xs, int *ys)
{
	int dist = 512;

	*xs = x * dist / z;
	*ys = y * dist / z;
}

void rotate3d(int x, int y, int z,
	      int *x_, int *y_, int *z_, 
	      int cx, int cy, int cz,
	      u16 ax, u16 ay, u16 az)
{
	int sinx, siny, sinz, cosx, cosy, cosz;

	cosx = fixcos(ax);
	cosy = fixcos(ay);
	cosz = fixcos(az);
	sinx = fixsin(ax);
	siny = fixsin(ay);
	sinz = fixsin(az);
		
	*x_ = fixmul(x - cx, fixmul(cosz, cosy))
		- fixmul(y - cy, fixmul(cosy, sinz))
		- fixmul(z - cz, siny)
		+ cx;

	*y_ = fixmul(x - cx, fixmul(cosx, sinz) - fixmul(sinx, fixmul(siny, cosz)))
		+ fixmul(y - cy, fixmul(sinx, fixmul(siny, sinz)) + fixmul(cosx, cosz))
		- fixmul(z - cz, fixmul(sinx, cosy))
		+ cy;

	*z_ = fixmul(x - cx, fixmul(cosx, fixmul(siny, cosz)) + fixmul(sinx, sinz))
		+ fixmul(y - cy, fixmul(sinx, cosz) - fixmul(cosx, fixmul(siny, sinz)))
		+ fixmul(z - cz, fixmul(cosx, cosy))
		+ cz;
}

void draw_face(int x0, int y0, int x1, int y1, int x2, int y2, int x3, int y3, u8 col)
{
	Polygon poly;
	u8 color;

	//draw_line(x0, y0, x1, y1);
	//draw_line(x1, y1, x2, y2);
	//draw_line(x2, y2, x3, y3);
	//draw_line(x3, y3, x0, y0);

	poly.x[0] = x0; poly.y[0] = y0;
	poly.x[1] = x1; poly.y[1] = y1;
	poly.x[2] = x2; poly.y[2] = y2;
	poly.x[3] = x3; poly.y[3] = y3;

	fill_polygon(&poly, col);
}

Face make_face(int x0, int y0, int z0, int x1, int y1, int z1, int x2, int y2, int z2, int x3, int y3, int z3, u8 col)
{
	Face poly;

	poly.x[0] = x0; poly.y[0] = y0; poly.z[0] = z0;
	poly.x[1] = x1; poly.y[1] = y1; poly.z[1] = z1;
	poly.x[2] = x2; poly.y[2] = y2; poly.z[2] = z2;
	poly.x[3] = x3; poly.y[3] = y3; poly.z[3] = z3;

	poly.col = col;

	return poly;
}

void xchg_poly(Face* p1, Face* p2)
{
	Face t;

	t.x[0] = p1->x[0]; t.y[0] = p1->y[0]; t.z[0] = p1->z[0];
	t.x[1] = p1->x[1]; t.y[1] = p1->y[1]; t.z[1] = p1->z[1];
	t.x[2] = p1->x[2]; t.y[2] = p1->y[2]; t.z[2] = p1->z[2];
	t.x[3] = p1->x[3]; t.y[3] = p1->y[3]; t.z[3] = p1->z[3];
	t.col = p1->col;

	p1->x[0] = p2->x[0]; p1->y[0] = p2->y[0]; p1->z[0] = p2->z[0];
	p1->x[1] = p2->x[1]; p1->y[1] = p2->y[1]; p1->z[1] = p2->z[1];
	p1->x[2] = p2->x[2]; p1->y[2] = p2->y[2]; p1->z[2] = p2->z[2];
	p1->x[3] = p2->x[3]; p1->y[3] = p2->y[3]; p1->z[3] = p2->z[3];
	p1->col = p2->col;

	p2->x[0] = t.x[0]; p2->y[0] = t.y[0]; p2->z[0] = t.z[0];
	p2->x[1] = t.x[1]; p2->y[1] = t.y[1]; p2->z[1] = t.z[1];
	p2->x[2] = t.x[2]; p2->y[2] = t.y[2]; p2->z[2] = t.z[2];
	p2->x[3] = t.x[3]; p2->y[3] = t.y[3]; p2->z[3] = t.z[3];
	p2->col = t.col;
}

int partition_faces(Face *face, int left, int right)
{
	int sum, a, b;

	sum = face[left].z[0] + face[left].z[1] + face[left].z[2] + face[left].z[3];

	a = left - 1;
	b = right + 1;

	while (1) {
		do {
			b--;
		} while (face[b].z[0] + face[b].z[1] + face[b].z[2] + face[b].z[3] < sum);
			
		do {
			a++;
		}      while (face[a].z[0] + face[a].z[1] + face[a].z[2] + face[a].z[3] > sum);
		if (a < b)
			xchg_poly(&face[a], &face[b]);
		else
			return b;
	}
}
	
void quicksort_faces(Face *face, int left, int right)
{
	int i;
	if (left < right) {
		i = partition_faces(face, left, right);
		quicksort_faces(face, left, i);
		quicksort_faces(face, i + 1, right);
	}
}

void z_sort_faces(Face *face, int num)
{
	quicksort_faces(face, 0, num-1);
}

void dotp(Face *face, int *x, int *y, int *z)
{
	*x = (face->y);
}

void draw_cube(u16 ax, u16 ay, u16 az)
{
	int x[8], y[8], z[8];
	int xs[8], ys[8];
	int x_[8], y_[8], z_[8];
	int i, j;
        Face face[6];

	x[0] = -30; y[0] = -30; z[0] = -30;
	x[1] = 30; y[1] = -30; z[1] = -30;
	x[2] = 30; y[2] = 30; z[2] = -30;
	x[3] = -30; y[3] = 30; z[3] = -30;
	x[4] = 30; y[4] = -30; z[4] = 30;
	x[5] = -30; y[5] = -30; z[5] = 30;
	x[6] = -30; y[6] = 30; z[6] = 30;
	x[7] = 30; y[7] = 30; z[7] = 30;

	for (i = 0; i < 8; i++) {
		z[i] += 1000;
		rotate3d(x[i], y[i], z[i], &x_[i], &y_[i], &z_[i], 0, 0, 1000, ax, ay, az);
	}
	
	face[0] = make_face(x_[0], y_[0], z_[0], x_[1], y_[1], z_[1], x_[2], y_[2], z_[2], x_[3], y_[3], z_[3], 0xcc);
	face[1] = make_face(x_[1], y_[1], z_[1], x_[4], y_[4], z_[4], x_[7], y_[7], z_[7], x_[2], y_[2], z_[2], 0x22);
	face[2] = make_face(x_[4], y_[4], z_[4], x_[5], y_[5], z_[5], x_[6], y_[6], z_[6], x_[7], y_[7], z_[7], 0x44);
	face[3] = make_face(x_[5], y_[5], z_[5], x_[0], y_[0], z_[0], x_[3], y_[3], z_[3], x_[6], y_[6], z_[6], 0x66);
	face[4] = make_face(x_[5], y_[5], z_[5], x_[4], y_[4], z_[4], x_[1], y_[1], z_[1], x_[0], y_[0], z_[0], 0x88);
	face[5] = make_face(x_[3], y_[3], z_[3], x_[2], y_[2], z_[2], x_[7], y_[7], z_[7], x_[6], y_[6], z_[6], 0xaa);

	z_sort_faces(face, 6);

	for (i = 0; i < 6; i++) {
		for (j = 0; j < 4; j++) {
			//if (face[i].z[j] < 0)
			//	continue;
			proj_screen(face[i].x[j], face[i].y[j], face[i].z[j], &xs[j], &ys[j]);
			xs[j] += 50;
			ys[j] += 50;
			//draw_pixel(xs[j], ys[j], 0x00);
		}

		u8 col;

		col = 10*ni

		draw_face(xs[0], ys[0], xs[1], ys[1], xs[2], ys[2], xs[3], ys[3], col);
		//draw_pixel(xs[0], ys[0], 0x00);
	}
}