#include <stdio.h>
#include <math.h>
#include <debug.h>
#include <gccore.h>
#include "wiiuse.h"
#include "ir.h"
/*
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <ogcsys.h>
#include <sdcard.h>
#include <ogc/isfs.h>
#include <ogc/usb.h>
*/

/*
#include "nand.h"
#include "sdio.h"

#include "tmd_bin.h"
#include "etik_bin.h"
#include "certs_bin.h"
*/

#define SWAP16(x)					((((x)&0xff)<<8)|(((x)>>8)&0xff))

static u32 *xfb = NULL;
static GXRModeObj *rmode = NULL;
/*
static void (*loader)() = (void(*)())0x80001800;
static void (*loader)() = (void(*)())0x90000020;
*/

#if 0
char ascii(char s)
{
	if(s < 0x20) return '.';
	if(s > 0x7E) return '.';
	return s;
}

void hexdump(void *d, int len)
{
	u8 *data;
	int off=0;
	int i;
	data = (u8*)d;
	while(off<len) {
		printf("%08x  ", off);
		for(i=0; i<8; i++) {
			if((i+off)>=len) {
				printf("   ");
			} else {
				printf("%02x ", data[off+i]);
			}
		}
		printf(" ");
		for(i=0; i<8; i++) {
			if((i+off)>=len) {
				printf(" ");
			} else {
				printf("%c", ascii(data[off+i]));
			}
		}
		printf("\n");
		off +=8;
	}
}
#endif
static u32 red, green, blue, white;

u32
CvtRGB (u8 r1, u8 g1, u8 b1, u8 r2, u8 g2, u8 b2)
{
  int y1, cb1, cr1, y2, cb2, cr2, cb, cr;
 
  y1 = (299 * r1 + 587 * g1 + 114 * b1) / 1000;
  cb1 = (-16874 * r1 - 33126 * g1 + 50000 * b1 + 12800000) / 100000;
  cr1 = (50000 * r1 - 41869 * g1 - 8131 * b1 + 12800000) / 100000;
 
  y2 = (299 * r2 + 587 * g2 + 114 * b2) / 1000;
  cb2 = (-16874 * r2 - 33126 * g2 + 50000 * b2 + 12800000) / 100000;
  cr2 = (50000 * r2 - 41869 * g2 - 8131 * b2 + 12800000) / 100000;
 
  cb = (cb1 + cb2) >> 1;
  cr = (cr1 + cr2) >> 1;
 
  return (y1 << 24) | (cb << 16) | (y2 << 8) | cr;
}

short quit = 0;
static f32 x = 320, y = 240;

static void __resetCB()
{
	x = 320;
	y = 240;
//	loader();
}

void __exception_closeall();

void draw_with_js(joystick_t js, u32 buttons, u32 color)
{
	if (!isnan(js.ang))
	{
		int ix, iy;
		f32 a = js.ang * M_PI_4 / 45;
		f32 m = js.mag;
		x += m * sin(a);
		y -= m * cos(a);
		ix = (unsigned int) x;
		iy = (unsigned int) y;
		if (buttons)
			xfb[iy * 320 + (ix >> 1)] = color;
	}
}

int main()
{
	u32 i;
	u32 foundmotes;
	u32 connmotes;
	wiimote** wiimotes = NULL;

	red = CvtRGB(255, 0, 0, 255, 0, 0);
	green = CvtRGB(0, 255, 0, 0, 255, 0);
	blue = CvtRGB(0, 0, 255, 0, 0, 255);
	white = CvtRGB(255, 255, 255, 255, 255, 255);
	VIDEO_Init();
	PAD_Init();

	/* Configure for use with USB on EXI channel 1 (memcard slot B) */
	/* Other option: GDBSTUB_DEVICE_TCP. Note: second parameter acts as port for this type of device */
	//DEBUG_Init(GDBSTUB_DEVICE_USB, 1);
	
	switch(VIDEO_GetCurrentTvMode())
	{
		case VI_NTSC:
			rmode = &TVNtsc480IntDf;
			break;
		case VI_PAL:
			rmode = &TVPal528IntDf;
			break;
		case VI_MPAL:
			rmode = &TVMpal480IntDf;
			break;
		default:
			rmode = &TVNtsc480IntDf;
			break;
	}

	xfb = MEM_K0_TO_K1(SYS_AllocateFramebuffer(rmode));
	console_init(xfb, 20, 20, rmode->fbWidth, rmode->xfbHeight, rmode->fbWidth*VI_DISPLAY_PIX_SZ);
		
	VIDEO_Configure(rmode);
	VIDEO_SetNextFramebuffer(xfb);
	VIDEO_SetBlack(FALSE);
	VIDEO_Flush();
	VIDEO_WaitVSync();
	if(rmode->viTVMode&VI_NON_INTERLACE) VIDEO_WaitVSync();

	/* This function call enters the debug stub for the first time */
	/* It's needed to call this if one wants to start debugging. */
	//_break();

	printf("\nHold down 1 and 2 until wiimote is connected\n");
	SYS_SetResetCallback(__resetCB);

	wiiuse_init();

	connmotes = 0;
	foundmotes = wiiuse_find((struct wiimote_t**)&wiimotes, 2);
	if(foundmotes>0) connmotes = wiiuse_connect(wiimotes, foundmotes);

//	printf("C:%u/M:%u\n", connmotes, foundmotes);

	if(connmotes==1)
	{
//		printf("Setting LED\n");
		wiiuse_set_leds(wiimotes[0], WIIMOTE_LED_1);
		/*
		wiiuse_set_leds(wiimotes[1], WIIMOTE_LED_2);
		wiiuse_set_leds(wiimotes[2], WIIMOTE_LED_3);
		wiiuse_set_leds(wiimotes[3], WIIMOTE_LED_4);
		*/
//		printf("Setting Streaming Input\n");
		wiiuse_motion_sensing(wiimotes[0], 1);
//		printf("Turning on IR\n");
//		wiiuse_set_ir(wiimotes[0], 1);
	}
//	__exception_closeall();
	for (i = 0; i < 28; i++) printf("\n");

	while(1)
	{
		/*
		PAD_ScanPads();
		while(!(PAD_ButtonsDown(0)&PAD_BUTTON_A) && !(PAD_ButtonsDown(0)&PAD_BUTTON_B)
			&& !(PAD_ButtonsDown(0)&PAD_BUTTON_X) && !(PAD_ButtonsDown(0)&PAD_BUTTON_Y)) {
			VIDEO_WaitVSync();
			PAD_ScanPads();
		}
		*/

		if(connmotes>0)
		{
//			s32 x, y;
			for(i=0;i<connmotes;i++)
			{
				switch(wiimotes[i]->event)
				{
					int j;
					case WIIUSE_NONE:
						break;
					case WIIUSE_EVENT:
//						printf("wiimote %u: event\n");
						break;
					case WIIUSE_STATUS:
//						printf("wiimote %u: status\n", i + 1);
						break;
					case WIIUSE_DISCONNECT:
						for (j = 0; j < 30; j++) printf("\n");
						printf("wiimote %u: disconnect\n", i + 1);
						break;
					case WIIUSE_NUNCHUK_INSERTED:
						printf("wiimote %u: nunchuk connected\n", i + 1);
//						for (j = 0; j < 29; j++) printf("\n");
						break;
					case WIIUSE_NUNCHUK_REMOVED:
						printf("wiimote %u: nunchuk discconnected\n", i + 1);
//						for (j = 0; j < 29; j++) printf("\n");
						break;
					case WIIUSE_CLASSIC_CTRL_INSERTED:
						for (j = 0; j < 30; j++) printf("\n");
//						printf("wiimote %u: classic controller connected\n", i + 1);
						break;
					case WIIUSE_CLASSIC_CTRL_REMOVED:
						for (j = 0; j < 30; j++) printf("\n");
//						printf("wiimote %u: classic controller disconnected\n", i + 1);
						break;
					case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
						for (j = 0; j < 30; j++) printf("\n");
//						printf("wiimote %u: guitar hero 3 controller connected\n", i + 1);
						break;
					case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
						for (j = 0; j < 30; j++) printf("\n");
//						printf("wiimote %u: guitar hero 3 controller disconnected\n", i + 1);
						break;
//					default:
//						printf("wii use: %u\n", wiimotes[i]->event);
				}
				switch (wiimotes[i]->exp.type)
				{
					case EXP_NONE:
						break;
					case EXP_NUNCHUK:
					{
						nunchuk_t nunchuk = wiimotes[i]->exp.nunchuk;
						draw_with_js(nunchuk.js, nunchuk.btns_held, red);
						break;
					}
					case EXP_CLASSIC:
					{
						classic_ctrl_t classic = wiimotes[i]->exp.classic;
						draw_with_js(classic.ljs, classic.btns_held, green);
						draw_with_js(classic.rjs, classic.btns_held, blue);
						break;
					}
					case EXP_GUITAR_HERO_3:
					{
						guitar_hero_3_t gh3 = wiimotes[i]->exp.gh3;
						draw_with_js(gh3.js, gh3.btns_held, white);
						break;
					}
				}
			}
			/*
			x = wiimotes[i]->ir.x;
			y = wiimotes[i]->ir.y;
			printf("%x %x % 3i % 3i\n", x, y, x, y);
			*/
//			xfb[(x >> 1) + y * 320] = 0xffffffff;
		}

		VIDEO_WaitVSync();
//		VIDEO_Flush();
	}

	return 0;
}