#include "sys_interaction.h"
#include "system.h"
#include "sys_vr.h"
#include "sys_vr_passthru.h"
#include "sys_quadstore.h"
#include "sys_tilestore.h"
#include "sys_render.h"
#include "sys_log.h"
#include "scenefile.h"

#define PI (3.1415926535897932f)
#define clamp(l,h,v) (((h)>(v))?(((v)<(l))?(l):(v)):(h))

static int yrInteraction_init(void* loadtoken);
static int yrInteraction_tick(void);
static int yrInteraction_save(void* savetoken, void* saveevent, int* savefailed);
static int yrInteraction_canfreeze(void);
static int yrInteraction_freeze(void);
static int yrInteraction_unfreeze(void);
static int yrInteraction_shutdown(void);

void yrInteraction_reg(void)
{
	yrSystem_register(sysInteraction,
					  yrInteraction_init,
					  yrInteraction_tick,
					  yrInteraction_save,
					  yrInteraction_canfreeze,
					  yrInteraction_freeze,
					  yrInteraction_unfreeze,
					  yrInteraction_shutdown,
					  (1<<sysLog)|(1<<sysVR)|(1<<sysRender)|(1<<sysTileStore)|(1<<sysQuadStore)|(1<<sysUndo));
}

static void toolswitch_tick(void);
static void toolswitch_open(int toolidx, int quick);

/***********************
* Tool mode declarations
************************/
#define TOOLCOUNT 2

static func_tool_shutdown		tfuncs_shutdown[tm_end]		= {NULL};
static func_tool_settrackedid	tfuncs_settrackedid[tm_end]	= {NULL};
static func_tool_tick			tfuncs_tick[tm_end]			= {NULL};
static func_tool_canfreeze		tfuncs_canfreeze[tm_end]	= {NULL};
static func_tool_forceidle		tfuncs_forceidle[tm_end]	= {NULL};
static func_tool_getpokepos		tfuncs_getpokepos[tm_end]	= {NULL};
static func_tool_renderwindow	tfuncs_renderwindow[tm_end]	= {NULL};
static func_tool_poke			tfuncs_poke[tm_end]			= {NULL};

int tool_pen_init(func_tool_shutdown* st_sd,
				  func_tool_settrackedid* ft_sti,
				  func_tool_tick* ft_t,
				  func_tool_canfreeze* ft_cf,
				  func_tool_forceidle* ft_fi,
				  func_tool_getpokepos* ft_gpp,
				  func_tool_renderwindow* ft_rw,
				  func_tool_poke* ft_p);
int tool_create_init(func_tool_shutdown* st_sd,
					 func_tool_settrackedid* ft_sti,
					 func_tool_tick* ft_t,
					 func_tool_canfreeze* ft_cf,
					 func_tool_forceidle* ft_fi,
					 func_tool_getpokepos* ft_gpp,
					 func_tool_renderwindow* ft_rw,
					 func_tool_poke* ft_p);
int tool_teleport_init(func_tool_shutdown* st_sd,
					   func_tool_settrackedid* ft_sti,
					   func_tool_tick* ft_t,
					   func_tool_canfreeze* ft_cf,
					   func_tool_forceidle* ft_fi,
					   func_tool_getpokepos* ft_gpp,
					   func_tool_renderwindow* ft_rw,
					   func_tool_poke* ft_p);
int tool_move_init(func_tool_shutdown* st_sd,
				   func_tool_settrackedid* ft_sti,
				   func_tool_tick* ft_t,
				   func_tool_canfreeze* ft_cf,
				   func_tool_forceidle* ft_fi,
				   func_tool_getpokepos* ft_gpp,
				   func_tool_renderwindow* ft_rw,
				   func_tool_poke* ft_p);
int tool_delete_init(func_tool_shutdown* st_sd,
					 func_tool_settrackedid* ft_sti,
					 func_tool_tick* ft_t,
					 func_tool_canfreeze* ft_cf,
					 func_tool_forceidle* ft_fi,
					 func_tool_getpokepos* ft_gpp,
					 func_tool_renderwindow* ft_rw,
					 func_tool_poke* ft_p);
int tool_copy_init(func_tool_shutdown* st_sd,
				   func_tool_settrackedid* ft_sti,
				   func_tool_tick* ft_t,
				   func_tool_canfreeze* ft_cf,
				   func_tool_forceidle* ft_fi,
				   func_tool_getpokepos* ft_gpp,
				   func_tool_renderwindow* ft_rw,
				   func_tool_poke* ft_p);
int tool_resize_init(func_tool_shutdown* st_sd,
					 func_tool_settrackedid* ft_sti,
					 func_tool_tick* ft_t,
					 func_tool_canfreeze* ft_cf,
					 func_tool_forceidle* ft_fi,
					 func_tool_getpokepos* ft_gpp,
					 func_tool_renderwindow* ft_rw,
					 func_tool_poke* ft_p);
int tool_color_init(func_tool_shutdown* st_sd,
					func_tool_settrackedid* ft_sti,
					func_tool_tick* ft_t,
					func_tool_canfreeze* ft_cf,
					func_tool_forceidle* ft_fi,
					func_tool_getpokepos* ft_gpp,
					func_tool_renderwindow* ft_rw,
					func_tool_poke* ft_p);
int tool_background_init(func_tool_shutdown* st_sd,
						 func_tool_settrackedid* ft_sti,
						 func_tool_tick* ft_t,
						 func_tool_canfreeze* ft_cf,
						 func_tool_forceidle* ft_fi,
						 func_tool_getpokepos* ft_gpp,
						 func_tool_renderwindow* ft_rw,
						 func_tool_poke* ft_p);
int tool_undo_init(func_tool_shutdown* st_sd,
				   func_tool_settrackedid* ft_sti,
				   func_tool_tick* ft_t,
				   func_tool_canfreeze* ft_cf,
				   func_tool_forceidle* ft_fi,
				   func_tool_getpokepos* ft_gpp,
				   func_tool_renderwindow* ft_rw,
				   func_tool_poke* ft_p);

static struct {
	enum tool_mode		mode;
	func_tool_tick		tick;
	func_tool_canfreeze	canfreeze;
	func_tool_forceidle	forceidle;
	uint32_t tracked_id;
}
tool[2] = {0};

yrColor color_pen = 0xFF052040;
yrColor color_bg = 0xFFFFFFFF;
unsigned char bg_image = 125;
yrColor color_saved[5] = {0xFF101010, 0xFF801010, 0xFF108010, 0xFF101080, 0xFF0080FF};


/************
* Static vars
*************/

static int frozen = 0;
static int toolswitch = 0;
static yrModel mdl_tool_body = NULL;
static yrModel mdl_tool_trig = NULL;
static yrModel mdl_tool_tpad = NULL;
static yrModel mdl_tool_tnub = NULL;
static yrModel mdl_tool_appm = NULL;
static yrTexture tex_tool_full = NULL;
static yrModel mdl_switchball = NULL;
static yrTexture tex_switchball[tm_end] = {NULL};

/*******************************
* System functions
* (init, tick, shutdown, freeze)
********************************/

static int yrInteraction_init(void* loadtoken)
{
	frozen = 0;
	toolswitch = 0;

	//load tool model
	mdl_tool_body = yrModel_load("data/vr/tool_body.ymd");			if(!mdl_tool_body) return -1;
	mdl_tool_trig = yrModel_load("data/vr/tool_trig.ymd");			if(!mdl_tool_body) return -1;
	mdl_tool_tpad = yrModel_load("data/vr/tool_tpad.ymd");			if(!mdl_tool_body) return -1;
	mdl_tool_tnub = yrModel_load("data/vr/tool_tnub.ymd");			if(!mdl_tool_body) return -1;
	mdl_tool_appm = yrModel_load("data/vr/tool_appm.ymd");			if(!mdl_tool_body) return -1;
	tex_tool_full = yrTexture_loadfile("data/vr/tool_full.png");	if(!tex_tool_full) return -1;
	mdl_switchball = yrModel_load("data/vr/switchball.ymd");		if(!mdl_switchball) return -1;
	tex_switchball[tmPen]		= yrTexture_loadfile("data/vr/switch_pen.png");		if(!tex_switchball[tmPen]) return -1;
	tex_switchball[tmTeleport]	= yrTexture_loadfile("data/vr/switch_teleport.png");	if(!tex_switchball[tmTeleport]) return -1;
	tex_switchball[tmCreate]	= yrTexture_loadfile("data/vr/switch_create.png");		if(!tex_switchball[tmCreate]) return -1;
	tex_switchball[tmMove]		= yrTexture_loadfile("data/vr/switch_move.png");		if(!tex_switchball[tmMove]) return -1;
	tex_switchball[tmDelete]	= yrTexture_loadfile("data/vr/switch_delete.png");		if(!tex_switchball[tmDelete]) return -1;
	tex_switchball[tmCopy]		= yrTexture_loadfile("data/vr/switch_copy.png");		if(!tex_switchball[tmCopy]) return -1;
	tex_switchball[tmResize]	= yrTexture_loadfile("data/vr/switch_resize.png");		if(!tex_switchball[tmResize]) return -1;
	tex_switchball[tmColor]		= yrTexture_loadfile("data/vr/switch_color.png");		if(!tex_switchball[tmColor]) return -1;
	tex_switchball[tmBackground]= yrTexture_loadfile("data/vr/switch_background.png");	if(!tex_switchball[tmBackground]) return -1;
	tex_switchball[tmUndo]		= yrTexture_loadfile("data/vr/switch_undo.png");		if(!tex_switchball[tmUndo]) return -1;

	//init modes
	int err = 0;
	err = tool_pen_init(tfuncs_shutdown + tmPen,
						tfuncs_settrackedid + tmPen,
						tfuncs_tick + tmPen,
						tfuncs_canfreeze + tmPen,
						tfuncs_forceidle + tmPen,
						tfuncs_getpokepos + tmPen,
						tfuncs_renderwindow + tmPen,
						tfuncs_poke + tmPen);
	if(err) return err;
	err = tool_teleport_init(tfuncs_shutdown + tmTeleport,
							 tfuncs_settrackedid + tmTeleport,
							 tfuncs_tick + tmTeleport,
							 tfuncs_canfreeze + tmTeleport,
							 tfuncs_forceidle + tmTeleport,
							 tfuncs_getpokepos + tmTeleport,
							 tfuncs_renderwindow + tmTeleport,
							 tfuncs_poke + tmTeleport);
	if(err) return err;
	err = tool_create_init(tfuncs_shutdown + tmCreate,
						   tfuncs_settrackedid + tmCreate,
						   tfuncs_tick + tmCreate,
						   tfuncs_canfreeze + tmCreate,
						   tfuncs_forceidle + tmCreate,
						   tfuncs_getpokepos + tmCreate,
						   tfuncs_renderwindow + tmCreate,
						   tfuncs_poke + tmCreate);
	if(err) return err;
	err = tool_move_init(tfuncs_shutdown + tmMove,
						 tfuncs_settrackedid + tmMove,
						 tfuncs_tick + tmMove,
						 tfuncs_canfreeze + tmMove,
						 tfuncs_forceidle + tmMove,
						 tfuncs_getpokepos + tmMove,
						 tfuncs_renderwindow + tmMove,
						 tfuncs_poke + tmMove);
	if(err) return err;
	err = tool_delete_init(tfuncs_shutdown + tmDelete,
						   tfuncs_settrackedid + tmDelete,
						   tfuncs_tick + tmDelete,
						   tfuncs_canfreeze + tmDelete,
						   tfuncs_forceidle + tmDelete,
						   tfuncs_getpokepos + tmDelete,
						   tfuncs_renderwindow + tmDelete,
						   tfuncs_poke + tmDelete);
	if(err) return err;
	err = tool_copy_init(tfuncs_shutdown + tmCopy,
						 tfuncs_settrackedid + tmCopy,
						 tfuncs_tick + tmCopy,
						 tfuncs_canfreeze + tmCopy,
						 tfuncs_forceidle + tmCopy,
						 tfuncs_getpokepos + tmCopy,
						 tfuncs_renderwindow + tmCopy,
						 tfuncs_poke + tmCopy);
	if(err) return err;
	err = tool_resize_init(tfuncs_shutdown + tmResize,
						   tfuncs_settrackedid + tmResize,
						   tfuncs_tick + tmResize,
						   tfuncs_canfreeze + tmResize,
						   tfuncs_forceidle + tmResize,
						   tfuncs_getpokepos + tmResize,
						   tfuncs_renderwindow + tmResize,
						   tfuncs_poke + tmResize);
	if(err) return err;
	err = tool_color_init(tfuncs_shutdown + tmColor,
						  tfuncs_settrackedid + tmColor,
						  tfuncs_tick + tmColor,
						  tfuncs_canfreeze + tmColor,
						  tfuncs_forceidle + tmColor,
						  tfuncs_getpokepos + tmColor,
						  tfuncs_renderwindow + tmColor,
						  tfuncs_poke + tmColor);
	if(err) return err;
	err = tool_background_init(tfuncs_shutdown + tmBackground,
							   tfuncs_settrackedid + tmBackground,
							   tfuncs_tick + tmBackground,
							   tfuncs_canfreeze + tmBackground,
							   tfuncs_forceidle + tmBackground,
							   tfuncs_getpokepos + tmBackground,
							   tfuncs_renderwindow + tmBackground,
							   tfuncs_poke + tmBackground);
	if(err) return err;
	err = tool_undo_init(tfuncs_shutdown + tmUndo,
						 tfuncs_settrackedid + tmUndo,
						 tfuncs_tick + tmUndo,
						 tfuncs_canfreeze + tmUndo,
						 tfuncs_forceidle + tmUndo,
						 tfuncs_getpokepos + tmUndo,
						 tfuncs_renderwindow + tmUndo,
						 tfuncs_poke + tmUndo);
	if(err) return err;
	
	//set initial modes on controllers
	for(size_t t = 0; t < TOOLCOUNT; ++t)
	{
		tool[t].mode = t;
		tool[t].tracked_id = 0xfffffffful;
		tool[t].tick = tfuncs_tick[t];
		tool[t].canfreeze = tfuncs_canfreeze[t];
		tool[t].forceidle = tfuncs_forceidle[t];
	}

	//register with sysVR for controller notifications
	yrVR_ready_interaction(1);

	//load colors
	size_t rw;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, &color_pen);		if(rw != 4) color_pen = 0xFF101010;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, &color_bg);		if(rw != 4) color_pen = 0xFFFFFFFF;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, color_saved + 0);	if(rw != 4) color_pen = 0xFF101010;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, color_saved + 1);	if(rw != 4) color_pen = 0xFFC01010;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, color_saved + 2);	if(rw != 4) color_pen = 0xFF10C010;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, color_saved + 3);	if(rw != 4) color_pen = 0xFF1010C0;
	rw = yrSFRead_read((yrReadHandle*) loadtoken, 4, color_saved + 4);	if(rw != 4) color_pen = 0xFFC0C0C0;

	return 0;
}

static int yrInteraction_shutdown(void)
{
	//disable controller notifications
	yrVR_ready_interaction(0);

	//disconnect tools
	for(size_t t = 0; t < TOOLCOUNT; ++t) {
		yrInteraction_notifycontroller(tool[t].tracked_id, 0);
	}

	//shutdown modes
	for(size_t i = 0; i < tm_end; ++i) {
		if(tfuncs_shutdown[i]) {
			tfuncs_shutdown[i]();
			tfuncs_shutdown[i] = NULL;
		}
	}

	//cleanup vars
	for(unsigned i = 0; i < tm_end; ++i)
		yrTexture_unload(tex_switchball[i]);
	yrModel_unload(mdl_switchball);
	yrTexture_unload(tex_tool_full);
	yrModel_unload(mdl_tool_body);
	yrModel_unload(mdl_tool_trig);
	yrModel_unload(mdl_tool_tpad);
	yrModel_unload(mdl_tool_tnub);
	yrModel_unload(mdl_tool_appm);
	for(unsigned i = 0; i < tm_end; ++i)
		tex_switchball[i] = NULL;
	mdl_switchball = NULL;
	tex_tool_full = NULL;
	mdl_tool_body = NULL;
	mdl_tool_trig = NULL;
	mdl_tool_tpad = NULL;
	mdl_tool_tnub = NULL;
	mdl_tool_appm = NULL;

	return 0;
}

static int yrInteraction_tick(void)
{
	//draw shared tool models
	mat4f identity = {{
		{1.0f,0.0f,0.0f,0.0f},
		{0.0f,1.0f,0.0f,0.0f},
		{0.0f,0.0f,1.0f,0.0f},
		{0.0f,0.0f,0.0f,1.0f},
	}};
	vec4i zero = {0,0,0,0};
	//handle controllers
	for(size_t t = 0; t < TOOLCOUNT; ++t)
	{
		if(tool[t].tracked_id == 0xFFFFFFFFul) continue;
		//get button state
		VRControllerState_t cbuttons = {0};
		ovrSystem_GetControllerState(tool[t].tracked_id, &cbuttons);

		//draw body
		yrModel_render(mdl_tool_body, tex_tool_full, identity, zero, 0xFFFFFFFFul, tool[t].tracked_id);
		//touchpad
		int touchd = 0!=(cbuttons.ulButtonPressed & (1ull << EVRButtonId_k_EButton_SteamVR_Touchpad));
		float touch_x = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Touchpad - EVRButtonId_k_EButton_Axis0].x;
		float touch_y = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Touchpad - EVRButtonId_k_EButton_Axis0].y;
		float tpad_a = touch_x;
		float tpad_b = touch_y;
		float tpad_offy = -0.003f;
		float tpad_offz = 0.049f;
		if(touchd) {
			tpad_offy -= 0.0006f;
		} else {
			tpad_a = 0.0f;
			tpad_b = 0.0f;
		}

		tpad_a = tpad_a * -0.05f;
		tpad_b = tpad_b * -0.05f + 0.11f;
		float tpad_sa = sinf(tpad_a);
		float tpad_ca = cosf(tpad_a);
		float tpad_sb = sinf(tpad_b);
		float tpad_cb = cosf(tpad_b);
		mat4f touchmat = {{
			{tpad_ca,				tpad_sa,			0.0,		0.0f},
			{-tpad_sa * tpad_cb,	tpad_ca * tpad_cb,	tpad_sb,	0.0f},
			{tpad_sa * tpad_sb,		-tpad_ca * tpad_sb,	tpad_cb,	0.0f},
			{0.0f,					tpad_offy,			tpad_offz,		1.0f},
		}};
		yrModel_render(mdl_tool_tpad, tex_tool_full, touchmat, zero, 0xFFFFFFFFul, tool[t].tracked_id);
		//touchnub
		if(touch_x != 0.0f || touch_y != 0.0f)
		{
			float nubx = clamp(-0.018f, 0.018f, 0.02f * touch_x);
			float nubz = -clamp(-0.018f, 0.018f, 0.02f * touch_y) + 0.049f;
			float nuby = clamp(-0.0027f, 0.0027f, 0.003f * touch_y) + 0.005f;
			mat4f nubmat = {{
				{1.0f,0.0f,0.0f,0.0f},
				{0.0f,1.0f,0.0f,0.0f},
				{0.0f,0.0f,1.0f,0.0f},
				{nubx,nuby,nubz,1.0f},
			}};
			yrModel_render(mdl_tool_tnub, tex_tool_full, nubmat, zero, 0xFFFFFFFFul, tool[t].tracked_id);
		}
		//trigger
		float trig_angle = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Trigger - EVRButtonId_k_EButton_Axis0].x;
		trig_angle = -trig_angle * 0.3f;
		float trig_sin = sinf(trig_angle);
		float trig_cos = cosf(trig_angle);
		mat4f trigmat = {{
			{1.0f,0.0f,0.0f,0.0f},
			{0.0f,trig_cos,trig_sin,0.0f},
			{0.0f,-trig_sin,trig_cos,0.0f},
			{0.0f,-0.016f,0.0355f,1.0f},
		}};
		yrModel_render(mdl_tool_trig, tex_tool_full, trigmat, zero, 0xFFFFFFFFul, tool[t].tracked_id);
		//appmenu button
		int appdown = cbuttons.ulButtonPressed & (1ull << EVRButtonId_k_EButton_ApplicationMenu);
		float appm_off = appdown?-0.0006f:0.0f;
		mat4f appmmat = {{
			{1.0f,0.0f,0.0f,0.0f},
			{0.0f,1.0f,0.0f,0.0f},
			{0.0f,0.0f,1.0f,0.0f},
			{0.0f,appm_off,0.0f,1.0f},
		}};
		yrModel_render(mdl_tool_appm, tex_tool_full, appmmat, zero, 0xFFFFFFFFul, tool[t].tracked_id);

		if(toolswitch) {
			//toolswitch tick
			toolswitch_tick();
		}
		else if(!frozen) {
			//detect toolswitch open
			if(appdown) {
				toolswitch_open((int)t,0);
			}
			//tool tick
			else {
				tool[t].tick();
			}
		}
	}
	//handle vr "windows"
	if(!toolswitch) {
		for(size_t i = 0; i < tm_end; ++i) {
			tfuncs_poke[i]();
			tfuncs_renderwindow[i]();
		}
	}
	return 0;
}

static int yrInteraction_canfreeze(void)
{
	if(frozen) return 1;
	int tfreeze = 1;
	for(size_t t = 0; t < TOOLCOUNT; ++t) {
		tfreeze = tfreeze && (tool[t].tracked_id == 0xfffffffful || tool[t].canfreeze());
	}
	return toolswitch || tfreeze;
}
static int yrInteraction_freeze(void) {
	frozen = 1; return 0;
}
static int yrInteraction_unfreeze(void) {
	frozen = 0; return 0;
}

/****************
* Extra functions
*****************/

void yrInteraction_notifycontroller(uint32_t tracked_id, int on_off)
{
	if(on_off) {
		for(size_t t = 0; t < TOOLCOUNT; ++t) {
			if(tool[t].tracked_id == 0xfffffffful) {
				tool[t].tracked_id = tracked_id;
				if(tfuncs_settrackedid[tool[t].mode])
					tfuncs_settrackedid[tool[t].mode](tracked_id, (unsigned) t);
				yrRender_set_trackedshadow((unsigned)t, tracked_id);
				yrRenderTrackedOverlay_set((unsigned)t, tracked_id);
				break;
			}
		}
	}
	else {
		for(size_t t = 0; t < TOOLCOUNT; ++t) {
			if(tool[t].tracked_id == tracked_id) {
				tool[t].tracked_id = 0xfffffffful;
				if(tfuncs_settrackedid[tool[t].mode])
					tfuncs_settrackedid[tool[t].mode](0xfffffffful, (unsigned) t);
				yrRender_set_trackedshadow((unsigned)t, 0xfffffffful);
				yrRenderTrackedOverlay_set((unsigned)t, tracked_id);
				break;
			}
		}
	}
}

void yrInteraction_changemode(unsigned toolidx, enum tool_mode newmode)
{
	tfuncs_settrackedid[tool[toolidx].mode](0xfffffffful, 0);
	tool[toolidx].mode = newmode;
	tool[toolidx].tick = tfuncs_tick[newmode];
	tool[toolidx].canfreeze = tfuncs_canfreeze[newmode];
	tool[toolidx].forceidle = tfuncs_forceidle[newmode];
	tfuncs_settrackedid[newmode](tool[toolidx].tracked_id, toolidx);
}

yrColor yrInteraction_getpencolor(void) { return color_pen; }
void yrInteraction_setpencolor(yrColor c) { color_pen = c; }
yrColor yrInteraction_getbgcolor(void) { return color_bg; }
void yrInteraction_setbgcolor(yrColor c) { color_bg = c; }
unsigned char yrInteraction_getbgimage(void) { return bg_image; }
void yrInteraction_setbgimage(int image) {
	YR_ASSERT(image <= 125);
	YR_ASSERT(image >= 0);
	bg_image = (unsigned char) image;
}
yrColor yrInteraction_getsavedcolor(size_t index)
{
	YR_ASSERT(index < 5);
	return color_saved[index];
}
void yrInteraction_setsavedcolor(size_t index, yrColor c)
{
	YR_ASSERT(index < 5);
	color_saved[index] = c;
}

static int trace_quad(tracehit* out, float mindist, float maxdist, yrQuad* q, vec4i rayorigin, vec4f dir)
{
	//transform to quad space
	vec4f foo = vec4f_mul(1.0f / YR_ACCURACY,
						  vec4f_from_vec4i(vec4i_sub(rayorigin, q->v[0])));
	vec4f QO = mat4f_apply(q->ext->invtransform, foo);
	vec4f QD = mat4f_apply(q->ext->invtransform, dir);

	//calculate distance 0 = oz + dist*dz
	float dist = -QO.z / QD.z;
	if(dist < mindist) return 0;
	if(dist >= maxdist) return 0;

	//boundary check
	vec4f Qproj = vec4f_add(QO, vec4f_mul(dist, QD));
	Qproj.x /= q->width;
	Qproj.y /= q->height;
	if(Qproj.x < 0.0f) return 0;
	if(Qproj.y < 0.0f) return 0;
	if(Qproj.x >= 1.0f) return 0;
	if(Qproj.y >= 1.0f) return 0;

	//it's a hit
	vec4f proj = vec4f_add(foo, vec4f_mul(dist, dir));
	out->pos = vec4i_add(q->v[0], vec4i_from_vec4f(vec4f_mul(YR_ACCURACY,proj)));
	out->dist = dist;
	out->q = q;
	out->hitcoord[0] = Qproj.x;
	out->hitcoord[1] = Qproj.y;
	return 1;
}

static int erasetrace_quad(vec4i* corner, vec4f dir, float depth, float radius, yrQuad* q)
{
	int hit = 0;
	for(size_t i = 0; !hit && i < 4; ++i)
	{
		//transform to quad space
		vec4f foo = vec4f_mul(1.0f / YR_ACCURACY,
							  vec4f_from_vec4i(vec4i_sub(corner[i], q->v[0])));
		vec4f QO = mat4f_apply(q->ext->invtransform, foo);
		vec4f QD = mat4f_apply(q->ext->invtransform, dir);

		//calculate distance 0 = oz + dist*dz
		float dist = -QO.z / QD.z;

		//boundary check
		vec4f Qproj = vec4f_add(QO, vec4f_mul(dist, QD));
		Qproj.x /= q->width;
		Qproj.y /= q->height;
		if(Qproj.x < -radius)	continue;
		if(Qproj.y < -radius)	continue;
		if(Qproj.x >= 1.0f + radius)	continue;
		if(Qproj.y >= 1.0f + radius)	continue;

		//distance check
		if(dist < 0.0f)		continue;
		if(dist >= depth)	continue;
		hit = 1;
	}
	return hit;
}

tracehit yrInteraction_trace(vec4i origin, vec4f dir, float min_dist, float max_dist)
{
	float fdevnull;
	yrQuad* q;
	tracehit out;
	out.q = NULL;
	out.dist = max_dist;
	out.pos = vec4i_add(origin, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY * max_dist, dir)));

	yrQuad_iter i = yrQuad_iter_top();
	while(q = yrQuad_iter_next(&i, &fdevnull)) {
		trace_quad(&out, min_dist, out.dist, q, origin, dir);
	}
	return out;
}

yrQuad* yrInteraction_erasetrace(vec4i* corners, vec4f dir, float erase_depth, float erase_radius)
{
	float bestm = -1.0f;
	float metric;
	yrQuad* bestq = NULL;
	yrQuad* q;
	yrQuad_iter i = yrQuad_iter_top();
	while(q = yrQuad_iter_next(&i, &metric)) {
		if(erasetrace_quad(corners, dir, erase_depth, erase_radius, q)) {
			if(metric > bestm) {
				bestq = q;
				bestm = metric;
			}
		}
	}
	return bestq;
}

tracehit yrInteraction_intersect(yrQuad* q, vec4i origin, vec4f dir)
{
	tracehit out;
	trace_quad(&out, -INFINITY, INFINITY, q, origin, dir);
	return out;
}

size_t yrInteraction_getpokepos(vec4f* pickpos, vec4f* pickdir, float* picklen, size_t start)
{
	for(size_t t = start; t < TOOLCOUNT; ++t) {
		if(tool[t].tracked_id != 0xFFFFFFFFul) {
			if(tfuncs_getpokepos[tool[t].mode](pickpos,pickdir,picklen)) {
				return t + 1;
			}
		}
	}
	*picklen = -1.0f;
	return TOOLCOUNT;
}

/***********
* Toolswitch
************/

static float ts_scale;
static vec4f ts_ballpos[tm_end];
static vec4f ts_balldir[tm_end];
static mat4f ts_initmat;
static int ts_phase;
static uint64_t ts_phaseend;
static int ts_appreleased;
static int ts_trigreleased;
#define ts_phaselen_open 500000
#define ts_phaselen_close 500000
#define ts_movspeed (0.12f)
#define ts_rotspeed (0.12f)

static vec4f switchball_position(size_t idx, mat4f hmdmat)
{
	float ang = 2 * PI / tm_end;
	float ypos = cosf(idx * ang);
	float xpos = -sinf(idx * ang);
	vec4f out = vec4f_add(vec4f_add(
		vec4f_mul(0.1f * xpos,	hmdmat.col[0]),
		vec4f_mul(0.1f * ypos,	hmdmat.col[1])),
		hmdmat.col[3]);
	return out;
}

static enum tool_mode detect_mode(size_t tidx)
{
	YR_ASSERT(tool[tidx].tracked_id != 0xFFFFFFFFul);
	YR_ASSERT(tool[tidx].mode != tm_end);
	mat4f ctrlmat = yrVR_world_from_tracked(tool[tidx].tracked_id);
	vec4f ctrlpos = vec4f_add(ctrlmat.col[3], vec4f_mul(-0.02f, ctrlmat.col[2]));
	enum tool_mode bestmode = tool[tidx].mode;
	float bestdist = 0.1f;
	for(size_t m = 0; m < tm_end; ++m) {
		if(tool[tidx].mode == m) continue;
		
		float dist = vec3f_length(vec4f_sub(ctrlpos, ts_ballpos[m]));
		if(dist < bestdist) {
			bestdist = dist;
			bestmode = m;
		}
	}
	return bestmode;
}

static void toolswitch_open(int toolidx, int quick)
{
	//open toolswitch
	toolswitch = 1;
	ts_appreleased = 0;
	ts_trigreleased = 0;
	ts_phase = 0;
	ts_phaseend = yr_get_microtime() + ts_phaselen_open;

	//force all tools to idle
	for(size_t t = 0; t < TOOLCOUNT; ++t) {
		if(tool[t].tracked_id != 0xfffffffful) {
			tool[t].forceidle();
		}
	}

	//init tool ball positions
	ts_initmat = yrVR_world_from_tracked(0);
	ts_initmat.col[3] = yrVR_world_from_tracked(tool[toolidx].tracked_id).col[3];
	vec4f initdir = vec4f_neg(ts_initmat.col[2]);
	for(size_t i = 0; i < tm_end; ++i) {
		ts_balldir[i] = initdir;
		ts_ballpos[i] = switchball_position(i, ts_initmat);
	}
	ts_scale = 0.0001f;
}

static void toolswitch_tick(void)
{
	switch(ts_phase)
	{
	//open animation
	case 0: {
		uint64_t now = yr_get_microtime();
		if(now < ts_phaseend) {
			uint64_t timeleft = ts_phaseend - yr_get_microtime();
			ts_scale = 0.5f - 0.5f*((float)timeleft)/ts_phaselen_open;
		}
		else {
			ts_scale = 0.5f;
			ts_phase = 1;
		}
		break;
	}
	//close animation
	case 2: {
		uint64_t now = yr_get_microtime();
		if(now < ts_phaseend) {
			uint64_t timeleft = ts_phaseend - yr_get_microtime();
			ts_scale = 0.5f*((float)timeleft)/ts_phaselen_close;
		}
		else {
			ts_scale = 0.0001f;
			toolswitch = 0;
		}
		break;
	}
	//interaction with switchballs
	case 1: {
		int appmenu_allclear = 1;
		int trig_allclear = 1;
		for(size_t t = 0; t < TOOLCOUNT; ++t) {
			if(tool[t].tracked_id == 0xFFFFFFFFul) continue;
			VRControllerState_t cbuttons = {0};
			ovrSystem_GetControllerState(tool[t].tracked_id, &cbuttons);
			//on trigger swap balls
			float trig = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Trigger - EVRButtonId_k_EButton_Axis0].x;
			if(!ts_trigreleased) {
				if(trig > 0.5f) trig_allclear = 0;
			}
			if(ts_trigreleased && trig == 1.0) {
				enum tool_mode newmode = detect_mode(t);
				enum tool_mode oldmode = tool[t].mode;
				if(newmode != tool[t].mode) {
					yrInteraction_changemode((int)t, newmode);
					for(size_t othertool = 0; othertool < TOOLCOUNT; ++othertool)
					{
						if(othertool == t) continue;
						if(tool[othertool].mode == newmode) {
							yrInteraction_changemode((int)othertool, oldmode);
							tfuncs_settrackedid[tool[t].mode](tool[t].tracked_id, (unsigned) t);
							break;
						}
					}					
				}
				ts_trigreleased = 0;
				trig_allclear = 0;
			}
			//on appmenu close menu
			int appdown = cbuttons.ulButtonPressed & (1ull << EVRButtonId_k_EButton_ApplicationMenu);
			if(appdown) appmenu_allclear = 0;
			if(ts_appreleased && appdown) {
				ts_phase = 2;
				ts_phaseend = yr_get_microtime() + ts_phaselen_close;
			}
		}
		if(appmenu_allclear) ts_appreleased = 1;
		if(trig_allclear) ts_trigreleased = 1;

		//position balls
		mat4f hmdmat = yrVR_world_from_tracked(0);
		for(size_t i = 0; i < tm_end; ++i)
		{
			int set = 0;
			vec4f dstpos;
			vec4f dstdir;
			for(size_t t = 0; !set && t < TOOLCOUNT; ++t) {
				if(tool[t].tracked_id == 0xFFFFFFFFul) continue;
				if(tool[t].mode == i) {
					//position on controller
					mat4f ctrlmat = yrVR_world_from_tracked(tool[t].tracked_id);
					dstpos = vec4f_add(ctrlmat.col[3], vec4f_mul(-0.02f, ctrlmat.col[2]));
					dstdir = vec4f_neg(ctrlmat.col[1]);
					set = 1;
				}
			}
			if(!set) {
				//position in circle				
				dstpos = switchball_position(i, ts_initmat);
				//face user
				dstdir = vec3f_normalized(vec4f_sub(dstpos, hmdmat.col[3]));
			}

			ts_ballpos[i] = vec4f_add(vec4f_mul(1.0f - ts_movspeed, ts_ballpos[i]), vec4f_mul(ts_movspeed, dstpos));
			ts_balldir[i] = vec4f_add(vec4f_mul(1.0f - ts_rotspeed, ts_balldir[i]), vec4f_mul(ts_rotspeed, dstdir));
			ts_balldir[i] = vec3f_normalized(ts_balldir[i]);
		}
		break;
	}
	}
	//draw all balls
	const yrColor ballclr[tm_end] = {
		0xff2288ff, //tmPen
		0xff88ff22, //tmTeleport
		0xffff2288, //tmCreate
		0xffffff22, //tmMove
		0xffff22ff, //tmDelete
		0xff22ffff, //tmCopy
		0xff2222ff, //tmResize
		0xff22ff22, //tmColor
		0xffff2222, //tmBackground
		0xff888888, //tmUndo
	};
	for(size_t i = 0; i < tm_end; ++i)
	{
		vec4f up = {0.0f,0.0f,1.0f,0.0f};
		vec4i off = yrVR_get_coord_offset();
		mat4f mat;
		mat.col[3] = ts_ballpos[i];
		mat.col[2] = ts_balldir[i];
		mat.col[0] = vec3f_normalized(vec3f_cross(up, mat.col[2]));
		mat.col[1] = vec3f_cross(mat.col[2], mat.col[0]);
		mat.col[2] = vec4f_mul(ts_scale, mat.col[2]);
		mat.col[1] = vec4f_mul(ts_scale, mat.col[1]);
		mat.col[0] = vec4f_mul(ts_scale, mat.col[0]);
		yrModel_render(mdl_switchball, tex_switchball[i], mat, off, ballclr[i], 0xFFFFFFFF);
	}
}

//doesn't seem worth it threading for this
static int yrInteraction_save(void* savetoken, void* saveevent, int* savefailed)
{
	size_t rw;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, &color_pen);		if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, &color_bg);			if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, color_saved + 0);	if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, color_saved + 1);	if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, color_saved + 2);	if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, color_saved + 3);	if(rw != 4) goto onerror;
	rw = yrSFWrite_write((yrWriteHandle*) savetoken, 4, color_saved + 4);	if(rw != 4) goto onerror;
	yrEvent_set(saveevent, 1);
	return 0;
onerror:
	*savefailed = 1;
	yrEvent_set(saveevent, 1);
	return 0;
};