#include <gccore.h>
#include <ogcsys.h>
#include <wiiuse/wpad.h>

#include "../pad.h"

u16 new_pad=0, old_pad=0, now_pad=0;
int irx=0, iry=0, wiimote_connected=0;
float irroll=0.0;

void internal_wiimote_check()
{
	u32 conn_dev;
        if (WPAD_Probe(WPAD_CHAN_0, &conn_dev) != WPAD_ERR_NONE)
                wiimote_connected = 0;
        else
        {
                // TODO: better way to do this? a callback or something
                if (!wiimote_connected)
                {
                        WPAD_SetVRes(WPAD_CHAN_0, 640, 480);
                        //WPAD_SetDataFormat(WPAD_CHAN_0, WPAD_FMT_CORE_ACC_IR);
                        WPAD_SetSleepTime(60); // thanks eke-eke for the confirmation that this is the timeout in seconds
                }

                wiimote_connected = 1;
        }
}
u16 pad_refresh()
{
	u16 b = 0;
	WPADData data;

	internal_wiimote_check();
	if (!wiimote_connected)
		return 0;

	WPAD_Read(WPAD_CHAN_0, &data);

	if (data.btns_d & WPAD_BUTTON_2)
		b |= GB_PAD_A;
	if (data.btns_d & WPAD_BUTTON_1)
		b |= GB_PAD_B;
	if (data.btns_d & WPAD_BUTTON_B)
		b |= GB_CHANGE_SIZE;
	if (data.btns_d & WPAD_BUTTON_A)
		b |= GB_PAD_START;
	if (data.btns_d & WPAD_BUTTON_MINUS)
		b |= GB_PAD_SELECT;
	if (data.btns_d & WPAD_BUTTON_PLUS)
		b |= GB_PAD_START;
	if (data.btns_d & WPAD_BUTTON_HOME)
		b |= GB_MENU;
	if (data.btns_d & WPAD_BUTTON_LEFT)
		b |= GB_PAD_DOWN;
	if (data.btns_d & WPAD_BUTTON_RIGHT)
		b |= GB_PAD_UP;
	if (data.btns_d & WPAD_BUTTON_DOWN)
		b |= GB_PAD_RIGHT;
	if (data.btns_d & WPAD_BUTTON_UP)
		b |= GB_PAD_LEFT;

	if (data.ir.num_dots>0)
	{
		//i've tried to steady the wiimote pointer
		int x = data.ir.dot[0].x;
		int y = data.ir.dot[0].y;
		x -= irx;
		y -= iry;
		x = (x<0)? -x : x;
		y = (y<0)? -y : y;
		if (x+y>2)
		{
			irx = data.ir.dot[0].x;
			iry = data.ir.dot[0].y;
			irroll = data.orient.roll/90;
		}
		
	}
	else
		irx = iry = -1;

	return b;
}
u16 pad_menu_buttons(void)
{
	static int n=0;
	now_pad = pad_refresh();
	new_pad = now_pad & ~old_pad;
	if(old_pad==now_pad){
		n++;
		if(n>=40){
			new_pad = now_pad;
			n = 30;
		}
	}else{
		n=0;
		old_pad = now_pad;
	}
	return new_pad;
}
u16 pad_game_buttons(void)
{
	return pad_refresh();
}



void pad_position(int *x, int *y, float *roll)
{
	pad_refresh();
	*x = irx;
	*y = iry;
	*roll = irroll;
}
void pad_set_mode(u8 mode)
{
	if (mode == PAD_MODE_MENU)
		WPAD_SetDataFormat(WPAD_CHAN_0, WPAD_FMT_CORE_ACC_IR);
	else if(mode == PAD_MODE_GAME)
		WPAD_SetDataFormat(WPAD_CHAN_0, WPAD_FMT_CORE);
}
void pad_init(void)
{
	WPAD_Init();
}

void pad_deinit(void)
{
	WPAD_Shutdown();
}
