#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];
	u8 c[3];
} Triangle;

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

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

typedef struct {
	int x[4];
	int y[4];
	u8 c[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);
void fill_triangle(int* x, int* y, u8 col);
void dprintf(const char*, ...);
void draw_cube(u16 ax, u16 ay, u16 az, int dx, int dy, int dz);
void gouraud_fill_triangle(int* x_, int* y_, u8* col);
void gouraud_horiz_line(int x1, int x2, int y, u8 c1, u8 c2);

/* 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];

static  unsigned  char  gamma4levels[] = {  0,  10, 20, 31 };
//static  unsigned  char  gamma64levels[] = {  0, 10, 14, 17, 19, 21, 23, 24, 26, 27, 28, 29, 31, 32, 33, 34, 35, 36, 37, 37, 38, 39, 40, 41, 41, 42, 43, 44, 44, 45, 46, 46,  47, 48, 48, 49, 49, 50, 51, 51, 52, 52, 53, 53, 54, 54, 55, 55,  56, 56, 57, 57, 58, 58, 59, 59, 60, 60, 61, 61, 62, 62, 63, 63  };  
static unsigned char gamma64levels[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13,14,14,15,15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,30,31,31};

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

   u8 red, blue, green, idx;
   for (red = 0; red < 4; red++) {
	   for (green = 0; green < 4; green++) {
		   for (blue = 0; blue < 4; blue++) {
			   idx = (blue << 4) + (green << 2) + red;
			   //palette[idx] = RGB16(gamma4levels[red],
			   //    gamma4levels[green], gamma4levels[blue]);
		   }
	   }
   }

   for (red = 0; red < 64; red++)
	   palette[64 + red] = RGB16(gamma64levels[red], 0, 0);
   for (green = 0; green < 64; green++)
	   palette[128 + green] = RGB16(0, gamma64levels[green], 0);
   for (blue = 0; blue < 64; blue++)
	   palette[192 + blue] = RGB16(0, 0, gamma64levels[blue]);

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

	   angle += 30;
	   if (angle >= 2048)
	   	   angle = 0;

	   poly.x[0] = 40; poly.y[0] = 60; poly.c[0] = 128;
	   poly.x[1] = 80; poly.y[1] = 60; poly.c[1] = 140;
	   poly.x[2] = 80; poly.y[2] = 100; poly.c[2] = 170;
	   poly.x[3] = 40; poly.y[3] = 100; poly.c[3] = 190;
	   
	   for (i = 0; i < 4; i++) {
		   rotate_point(angle, poly.x[i], poly.y[i], 60, 80, &poly_.x[i], &poly_.y[i]);
		   //poly_.x[i] = poly.x[i];
		   //poly_.y[i] = poly.y[i];
		   poly_.c[i] = poly.c[i];
	   }
	   
	   //fill_polygon(&poly);

	   //gouraud_horiz_line(10, 100, 50, 128, 191);
	   draw_cube(angle, -angle, angle, 25, 25, 1000);
	   //draw_cube(-angle, angle, -angle, 85, 70, 1000);
	   //draw_cube(-angle - 100, -angle - 300, -angle - 200, 25, 90, 1000);
	   
	   wait_for_vblank();
	   flip_buffer();
   }
}

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

	gouraud_fill_triangle(poly->x, poly->y, poly->c);
	gouraud_fill_triangle(tri.x, tri.y, tri.c);
}

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

void xchg_col(u8 *a, u8 *b)
{
	u8 temp;
	temp = *a;
	*a = *b;
	*b = temp;
}

void gouraud_horiz_line(int x1, int x2, int y, u8 c1, u8 c2)
{
	u8 i;
	u8 dc;
	u8 col;

	if (x2 < x1) {
	       	xchg(&x1, &x2);
		xchg_col(&c1, &c2);
	}

	if (x1 < 0)
		x1 = 0;

	col = c1;
	dc = (c2 - c1) / (x2 - x1);
#if 0
	asm volatile(
	    "mov r0,%4\n" /* r0 = c1 */
	    "mov r1,%0\n" /* r1 = x1 */
	    "mov r2,#240\n"
	    "mul r3,r2,%2\n" /* r3 = y * 240 */
	    "mov r4,%5\n"
	    "add r4,r4,r3\n" /* r4 = 0x6000000 + y * 240 */
	    "add r4,r4,%0\n"
	    "mov r2,%1\n"
	    "loop: \n"
	    "add r3,r0,r0,asl #8\n" /* r3 = (col << 8) + col */
	    "strh r3,[r4],#2\n"
	    "add r0,r0,%3\n" /* r0 += dc */
	    "and r0,r0,#0xFF\n"
	    "add r1,r1,#1\n"
	    "cmp r1,r2\n"
	    "blt loop\n"
	    "end:"
	    :
	    /* No output */ :
	    "r" (x1), "r" (x2), "r" (y), "r" (dc), "r" (c1), "r" (screenbuffer):
	    "r0", "r1", "r2", "r3", "r4");
#else
	for (i = x1; i < x2; i++) {
		screenbuffer[i + y * SCREENWIDTH] = (col << 8) + col;
		col += dc;
	}
#endif
}


void gouraud_fill_triangle(int* x_, int* y_, u8* col_) {
	int dx[3], dy[3], d[3], dc[3], c[3];
	u8 col[3], c1, c2;
	int x[3], y[3];
	int i;

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

	if (y[1] < y[0]) {
		xchg(&y[0], &y[1]);
		xchg(&x[0], &x[1]);
		xchg_col(&col[0], &col[1]);
	}
	if (y[2] < y[1]) {
		xchg(&y[1], &y[2]);
		xchg(&x[1], &x[2]);
		xchg_col(&col[1], &col[2]);
	}
	if (y[1] < y[0]) {
		xchg(&y[0], &y[1]);
		xchg(&x[0], &x[1]);
		xchg_col(&col[0], &col[1]);
	}

	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];
	dc[0] = col[1] - col[0];
	dc[1] = col[2] - col[1];
	dc[2] = col[0] - col[2];

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

	c1 = col[0];
	c2 = col[0];
	for (i = y[0]; i < y[1]; i++) {
		gouraud_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,
		    //f2i(i2f(col[0]) + fixmul(i2f(i - y[0]), c[0])), 
		    //f2i(i2f(col[0]) + fixmul(i2f(i - y[0]), c[2])));
		    c1, c2);
		c1 += c[2];
		c2 += c[0];
	}

	//c1 = col[0];
	c2 = col[2];
	for (i = y[1]; i < y[2]; i++) {
		gouraud_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,
		    //f2i(i2f(col[1]) + fixmul(i2f(i - y[1]), c[1])), 
		    //f2i(i2f(col[0]) + fixmul(i2f(i - y[0]), c[2])));
		    c1, c2);
		c1 += c[2];
		c2 += c[1];
	}
	
}

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_ = f2i(fixmul(i2f(x - center_x), fixcos(angle)) - (fixmul(i2f(y - center_y), fixsin(angle)) + i2f(center_x)));
	*y_ = f2i(fixmul(i2f(y - center_y), fixcos(angle)) + (fixmul(i2f(x - center_x), fixsin(angle))) + i2f(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 c0, u8 c1, u8 c2, u8 c3)
{
	Polygon poly;
	u8 color;

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

	fill_polygon(&poly);
}

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

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

	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.c[0] = p1->c[0];
	t.x[1] = p1->x[1]; t.y[1] = p1->y[1]; t.z[1] = p1->z[1]; t.c[1] = p1->c[1];
	t.x[2] = p1->x[2]; t.y[2] = p1->y[2]; t.z[2] = p1->z[2]; t.c[2] = p1->c[2];
	t.x[3] = p1->x[3]; t.y[3] = p1->y[3]; t.z[3] = p1->z[3]; t.c[3] = p1->c[3];

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

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

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 draw_cube(u16 ax, u16 ay, u16 az, int dx, int dy, int dz)
{
	int x[8], y[8], z[8];
	int xs[8], ys[8];
	int x_[8], y_[8], z_[8];
	u8 c[8], cs[8];
	int i, j;
        Face face[6];

	x[0] = -30; y[0] = -30; z[0] = -30; c[0] = 128;
	x[1] = 30; y[1] = -30; z[1] = -30; c[1] = 191;
	x[2] = 30; y[2] = 30; z[2] = -30; c[2] = 140;
	x[3] = -30; y[3] = 30; z[3] = -30; c[3] = 181;
	x[4] = 30; y[4] = -30; z[4] = 30; c[4] = 170;
	x[5] = -30; y[5] = -30; z[5] = 30; c[5] = 161;
	x[6] = -30; y[6] = 30; z[6] = 30; c[6] = 135;
	x[7] = 30; y[7] = 30; z[7] = 30; c[7] = 151;

	for (i = 0; i < 8; i++) {
		z[i] += dz; /* Translate on the z-axis (should be done elsewhere */
		/* Rotate each vertex */
		rotate3d(x[i], y[i], z[i], &x_[i], &y_[i], &z_[i], 0, 0, 1000, ax, ay, az);
	}

	/* Make the faces */
	face[0] = make_face(x_[0], y_[0], z_[0], c[0], x_[1], y_[1], z_[1], c[1], x_[2], y_[2], z_[2], c[2], x_[3], y_[3], z_[3], c[3]);
	face[1] = make_face(x_[1], y_[1], z_[1], c[1], x_[4], y_[4], z_[4], c[4],  x_[7], y_[7], z_[7], c[7], x_[2], y_[2], z_[2], c[2]);
	face[2] = make_face(x_[4], y_[4], z_[4], c[4],  x_[5], y_[5], z_[5], c[5], x_[6], y_[6], z_[6], c[6], x_[7], y_[7], z_[7], c[7]);
	face[3] = make_face(x_[5], y_[5], z_[5], c[5], x_[0], y_[0], z_[0], c[0], x_[3], y_[3], z_[3], c[3], x_[6], y_[6], z_[6], c[6]);
	face[4] = make_face(x_[5], y_[5], z_[5], c[5], x_[4], y_[4], z_[4], c[4], x_[1], y_[1], z_[1], c[1], x_[0], y_[0], z_[0], c[0]);
	face[5] = make_face(x_[3], y_[3], z_[3], c[3], x_[2], y_[2], z_[2], c[2], x_[7], y_[7], z_[7], c[7], x_[6], y_[6], z_[6], c[6]);

	z_sort_faces(face, 6);

	for (i = 0; i < 6; i++) {
		for (j = 0; j < 4; j++) {
			/* Project each vertex to screen */
			proj_screen(face[i].x[j], face[i].y[j], face[i].z[j], &xs[j], &ys[j]);
			xs[j] += dx; /* Translate */
			ys[j] += dy;
			cs[j] = face[i].c[j];
		}

		/* Draw the face */
		draw_face(xs[0], ys[0], xs[1], ys[1], xs[2], ys[2], xs[3], ys[3], cs[0], cs[1], cs[2], cs[3]);
	}
}