#include "sys_interaction.h"
#include "system.h"
#include "sys_vr.h"
#include "sys_vr_passthru.h"
#include "sys_render.h"
#include "sys_tilestore.h"

#define PICK_LEN (0.03f)
#define PICK_OFF {0.0,PICK_LEN * -0.7071067811865476f, PICK_LEN * -0.7071067811865476f,1.0f}
#define PICK_DIR {0.0,-0.7071067811865476f,-0.7071067811865476f,0.0f}
#define PICK_NOTDIR {0.0,0.7071067811865476f,-0.7071067811865476f,0.0f}

#define LUM_R 0.299f
#define LUM_G 0.587f
#define LUM_B 0.114f

//TODO: haptic poke feedback

static uint32_t tracked_id = 0xfffffffful;
static unsigned toolidx = 0;
static int touchdown;
static yrModel mdl_tip = NULL;
static yrTexture tex_fill = NULL;
static yrModel mdl_indic = NULL;
static yrTexture tpadbut_colors = NULL;
static yrTexture tpadbut_colors_glow[4] = {NULL};
static mat4f saved_mat;
static int poslocked;
static int trigdown;
static size_t active_slot;
static float picked_rg;
static float picked_yb;
static float picked_lum;

static void color_shutdown(void);
static void color_settrackedid(uint32_t, unsigned);
static void color_tick(void);
static int  color_canfreeze(void);
static void color_forceidle(void);
static int	color_getpokepos(vec4f* pos, vec4f* dir, float* len);
static void color_renderwindow(void);
static void color_poke(void);

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)
{
	*st_sd	= color_shutdown;
	*ft_sti	= color_settrackedid;
	*ft_t	= color_tick;
	*ft_cf	= color_canfreeze;
	*ft_fi	= color_forceidle;
	*ft_gpp = color_getpokepos;	
	*ft_rw	= color_renderwindow;
	*ft_p	= color_poke;

	tracked_id = 0xfffffffful;
	toolidx = 0;
	touchdown = 0;
	poslocked = 0;
	trigdown = 0;
	tex_fill = yrTexture_loadfile("data/vr/empty.png"); if(!tex_fill) return -1;
	mdl_indic = yrModel_load("data/vr/switchball.ymd"); if(!tex_fill) return -1;
	mdl_tip = yrModel_load("data/vr/pen_tip.ymd"); if(!tex_fill) return -1;
	tpadbut_colors			= yrTexture_loadfile("data/vr/tpadbut_colors.png");		if(!tpadbut_colors) return -1;
	tpadbut_colors_glow[0]	= yrTexture_loadfile("data/vr/tpadbut_colors_right.png");	if(!tpadbut_colors_glow[0]) return -1;
	tpadbut_colors_glow[1]	= yrTexture_loadfile("data/vr/tpadbut_colors_up.png");		if(!tpadbut_colors_glow[1]) return -1;
	tpadbut_colors_glow[2]	= yrTexture_loadfile("data/vr/tpadbut_colors_left.png");	if(!tpadbut_colors_glow[2]) return -1;
	tpadbut_colors_glow[3]	= yrTexture_loadfile("data/vr/tpadbut_colors_down.png");	if(!tpadbut_colors_glow[3]) return -1;
	picked_rg = 0.39f;
	picked_yb = 0.14f;
	picked_lum = 0.42f;;
	return 0;
}

static void color_shutdown(void)
{
	yrTexture_unload(tex_fill);
	yrTexture_unload(tpadbut_colors_glow[0]);
	yrTexture_unload(tpadbut_colors_glow[1]);
	yrTexture_unload(tpadbut_colors_glow[2]);
	yrTexture_unload(tpadbut_colors_glow[3]);
	yrTexture_unload(tpadbut_colors);
	yrModel_unload(mdl_indic);
	yrModel_unload(mdl_tip);
	tpadbut_colors_glow[3] = NULL;
	tpadbut_colors_glow[2] = NULL;
	tpadbut_colors_glow[1] = NULL;
	tpadbut_colors_glow[0] = NULL;
	tpadbut_colors = NULL;
	tex_fill = NULL;
	mdl_indic = NULL;
	mdl_tip = NULL;
}

static void color_settrackedid(uint32_t tr_id, unsigned tool_idx)
{
	tracked_id = tr_id;
	toolidx = tool_idx;
}

static void from_colorval(yrColor clr, float* picked_rg, float* picked_yb, float* picked_lum)
{
	float r = yrColor_red(clr);
	float g = yrColor_green(clr);
	float b = yrColor_blue(clr);
	*picked_yb = b/(r+g+b);
	float left = 1.0f - *picked_yb;
	*picked_rg = left * g/(r+g);
	*picked_lum = r*LUM_R + g*LUM_G + b*LUM_B;
}

static yrColor to_colorval(float rg, float yb, float lum)
{
	float b = yb;
	float g = rg;
	float r = 1.0f - g - b;
	float baselum = 0.0;
	baselum += r * LUM_R;
	baselum += g * LUM_G;
	baselum += b * LUM_B;
	float boost = lum / baselum;
	r *= boost;
	g *= boost;
	b *= boost;
	return yrColor_create(r, g, b, 1.0f);
}

static yrColor pickcolor(vec4f pos, vec4f dir, float len, float* rg, float* yb, float* lum)
{
	//position to point on plane
	pos = vec4f_sub(pos, saved_mat.col[3]);
	vec4f zero = {0.0f,0.0f,0.0f,1.0f};
	mat4f inv = saved_mat;
	inv.col[3] = zero;
	inv = mat4f_invert(inv);
	pos = mat4f_apply(inv, pos);
	dir = mat4f_apply(inv, dir);
	float dist = -pos.z / dir.z;
	if(dist < -len) return 0;
	if(dist > 0.0f) return 0;
	pos = vec4f_add(pos, vec4f_mul(dist, dir));
	pos.x += 0.06f;
	pos.y += 0.01f;

	//saved slots
	if(pos.x >= 0.01f && pos.x <= 0.05f)
	{
		//find slot
		for(size_t i = 0; i < 5; ++i) {
			float y = pos.y - 0.01f - i * (0.22651f / 5);
			if(y > 0.0f && y < (0.22651f / 5 - 0.01f)) {
				yrColor clr = yrInteraction_getsavedcolor(i);
				from_colorval(clr, rg, yb, lum);
				return clr;
			}
		}
		//no hit
		return 0;
	}
	//luminance
	else if(pos.x >= 0.32f && pos.x <= 0.36f)
	{
		float val = pos.y - 0.01f;
		val /= (0.22651f / 1.1f);
		if(val >= 0.0f && val <= 1.1f)
		{
			*lum = val;
			return to_colorval(*rg, *yb, *lum);
		}
		return 0;
	}
	//hue
	else if(pos.x >= 0.06f && pos.x <= 0.31f)
	{
		float val = pos.y - 0.01f;
		val /= 0.22651f;
		val = 1.0f - val;
		if(val >= 0.0f && val <= 1.0f)
		{
			float left = 1.0f - val;
			float foo = pos.x - 0.06f - 0.125f;
			foo /= 0.25f;
			if(fabsf(foo) <= left/2) {
				*yb = val;
				*rg = foo + (left/2);
				return to_colorval(*rg, *yb, *lum);
			}
		}
		return 0;
	}
	return 0;
}

static void color_tick(void)
{
	VRControllerState_t cbuttons = {0};
	ovrSystem_GetControllerState(tracked_id, &cbuttons);
	mat4f mat = yrVR_world_from_tracked(tracked_id);

	//draw color indicator
	vec4i zero = {0,0,0,0};
	mat4f indicmat = {{
		{0.2f,0.0f,0.0f,0.0f},
		{0.0f,0.0f,-0.2f,0.0f},
		{0.0f,0.2f,0.0f,0.0f},
		{0.0f,-0.001f,0.12f,1.0f},
	}};
	yrModel_render(mdl_indic, tex_fill, indicmat, zero, yrInteraction_getpencolor(), tracked_id);
	
	//prepare vr window mat
	mat4f cpickmat = {{
		{1.0f,0.0f,0.0f,0.0f},
		{0.0f,0.7071067811865476f,-0.7071067811865476f, 0.0f},
		{0.0f,0.7071067811865476f,0.7071067811865476f, 0.0f},
		{-0.125f,0.01f,-0.01f,1.0f},
	}};
	cpickmat = mat4f_combine(mat, cpickmat);
	if(!poslocked) saved_mat = cpickmat;
	
	//interaction
	float touchx = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Touchpad - EVRButtonId_k_EButton_Axis0].x;
	float touchy = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Touchpad - EVRButtonId_k_EButton_Axis0].y;
	int touchd = 0!=(cbuttons.ulButtonPressed & (1ull << EVRButtonId_k_EButton_SteamVR_Touchpad));
	float trig = cbuttons.rAxis[EVRButtonId_k_EButton_SteamVR_Trigger - EVRButtonId_k_EButton_Axis0].x;

	//draw buttons
	yrRenderTrackedOverlay_clear(toolidx);
	if(touchd || touchx != 0.0f || touchy != 0.0f) {
		int glowidx;
		if(fabsf(touchx) > fabsf(touchy))	glowidx = (touchx > 0.0f) ? 0:2;
		else								glowidx = (touchy > 0.0f) ? 1:3;
		yrRenderTrackedOverlay_draw(toolidx, tpadbut_colors_glow[glowidx], -1.0f, -1.0f, 1.0f, 1.0f, touchd ? 0xFFFF8010 : 0x80FF8010);
	}
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_colors, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	yrRenderTrackedOverlay_render(toolidx);
	
	//touchpad "buttons"
	if(!touchdown && touchd) {
		if(fabsf(touchx) > fabsf(touchy)) {
			if(touchx > 0.0f) {
				//right: store color
				yrInteraction_setsavedcolor(active_slot, yrInteraction_getpencolor());
			} else {
				//left: load color
				yrColor clr = yrInteraction_getsavedcolor(active_slot);
				yrInteraction_setpencolor(clr);
				from_colorval(clr, &picked_rg, &picked_yb, &picked_lum);
			}
		} else {
			if(touchy > 0.0f) {
				//up: select slot
				active_slot = (active_slot + 1) % 5;
			} else {
				//down: select slot
				if(active_slot > 0) active_slot -= 1;
				else active_slot = 4;
			}
		}
	}
	touchdown = touchd;

	//color picking
	if(!trigdown && trig > 0.3f) poslocked = !poslocked;
	trigdown = (trig > 0.3f);

	if(poslocked) {
		mat4f tipmat = {{{0.0f,0.0f,0.0f,0.0f},PICK_NOTDIR,PICK_DIR,PICK_OFF}};
		tipmat.col[0] = vec3f_cross(tipmat.col[1],tipmat.col[2]);
		vec4i zero = {0,0,0,0};
		yrModel_render(mdl_tip, tex_fill, tipmat, zero, yrInteraction_getpencolor(), tracked_id);
	}
}

static int color_canfreeze(void)
{
	return 1;
}

static void	color_forceidle(void)
{
	;//nothing
}

static int color_getpokepos(vec4f* pos, vec4f* dir, float* len)
{
	if(poslocked) {
		mat4f mat = yrVR_world_from_tracked(tracked_id);
		mat4f tipmat = {{{0.0f,0.0f,0.0f,0.0f},PICK_NOTDIR,PICK_DIR,PICK_OFF}};
		tipmat.col[0] = vec3f_cross(tipmat.col[1],tipmat.col[2]);
		vec4i zero = {0,0,0,0};

		tipmat = mat4f_combine(mat, tipmat);
		*len = PICK_LEN;
		*pos = tipmat.col[3];
		*dir = tipmat.col[2];

		return 1;
	}
	else {
		return 0;
	}
}

static void color_renderwindow(void)
{
	if(!poslocked && tracked_id == 0xFFFFFFFFul) return;
	
	//draw color picker
	yrRender_colorpick(saved_mat, picked_rg, picked_yb, picked_lum);

	//draw background and color slots
	yrRenderOverlay_clear(OVERLAY_COLORPICK);
	yrRenderOverlay_draw(OVERLAY_COLORPICK, tex_fill, 0.0f, 0.0f, 0.37f * UV_PER_METER, 0.23651f * UV_PER_METER, 0x80000000ul);
	yrRenderOverlay_draw(OVERLAY_COLORPICK, tex_fill, 0.319f * UV_PER_METER, 0.009f * UV_PER_METER, 0.361f * UV_PER_METER, 0.22751f * UV_PER_METER, 0x80FFFFFFul);
	for(size_t i = 0; i < 5; ++i) {
		float loy = 0.01f + i * (0.22651f / 5);
		float hiy = loy + (0.22651f / 5 - 0.01f);
		float lox = 0.01f;
		float hix = 0.05f;
		loy *= UV_PER_METER;
		hiy *= UV_PER_METER;
		lox *= UV_PER_METER;
		hix *= UV_PER_METER;
		//edge around color
		if(i == active_slot) {
			yrRenderOverlay_draw(OVERLAY_COLORPICK, tex_fill, lox - 0.01f, loy - 0.01f, hix + 0.01f, hiy + 0.01f, 0xFFFFFFFFul);
		} else {
			yrRenderOverlay_draw(OVERLAY_COLORPICK, tex_fill, lox - 0.004f, loy - 0.004f, hix + 0.004f, hiy + 0.004f, 0x80FFFFFFul);
		}
		//actual color
		yrRenderOverlay_draw(OVERLAY_COLORPICK, tex_fill, lox, loy, hix, hiy, yrInteraction_getsavedcolor(i));
	}
	vec4i off = yrVR_get_coord_offset();
	yrQuad foo;
	foo.uv_draw[0] = 0.0f;
	foo.uv_draw[1] = 0.0f;
	foo.width = 0.37f;
	foo.height = 0.23651f;
	vec4f bar[3] = {{-0.06f,-0.01f,-0.001f,1.0f},{0.31f,-0.01f,-0.001f,1.0f},{-0.06f,0.22651f,-0.001f,1.0f}};
	foo.v[0] = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat4f_apply(saved_mat, bar[0]))));
	foo.v[1] = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat4f_apply(saved_mat, bar[1]))));
	foo.v[2] = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat4f_apply(saved_mat, bar[2]))));
	yrRenderOverlay_render(OVERLAY_COLORPICK, &foo, 0);
}

static void color_poke(void)
{
	if(!poslocked && tracked_id == 0xFFFFFFFFul) return;

	vec4f pickpos;
	vec4f pickdir;
	float picklen;
	yrColor picked = 0;
	size_t i = 0;
	
	while(1) {
		i = yrInteraction_getpokepos(&pickpos,&pickdir,&picklen, i);
		if(picklen > 0.0f) {
			picked = pickcolor(pickpos, pickdir, picklen,
							   &picked_rg, &picked_yb, &picked_lum);
			if(picked) {
				yrInteraction_setpencolor(picked);
				break;
			}
		}
		else break;
	}
}