#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 white_left, white_right;

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_white_pixel(unsigned int x, unsigned int y)
{
	xfb[y * 320 + (x >> 1)] = x % 2 ? white_left : white_right;
}

void draw_joystick(joystick_t js, unsigned int cx, unsigned int cy, unsigned int r)
{
	f32 ang = js.ang;
	f32 mag = js.mag;

	if (!isnan(ang))
	{
		int i;
		f32 a = js.ang * M_PI_4 / 45;
		for (i = 0; i < (int) (mag * r); i++)
			draw_white_pixel(cx + i * sin(a), cy - i * cos(a));
	}
}

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

	white_left = CvtRGB(255, 255, 255, 0, 0, 0);
	white_right = CvtRGB(0, 0, 0, 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;
						joystick_t js = nunchuk.js;
						if (!isnan(js.ang))
						{
							f32 a = js.ang * M_PI_4 / 45;
							f32 m = js.mag;
							x += m * sin(a);
							y -= m * cos(a);
							if (nunchuk.btns_held)
								draw_white_pixel((unsigned int) x,(unsigned int) y);
						}
						break;
					}
					case EXP_CLASSIC:
					{
						break;
					}
					case EXP_GUITAR_HERO_3:
					{
						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;
}