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

static uint32_t tracked_id = 0xfffffffful;
static unsigned toolidx = 0;

enum ModeTouch { mtIdleRot, mtIdleDist, mtRotXY, mtRotZ, mtDist, mt_end};
static yrQuad* grabbed = NULL;
static yrQuad grabbed_restore;
static float grabdist;
static vec4f v_off[3];
static vec4f rotbase[2];
static enum ModeTouch mode;
static int touchdown;
static int trigdown;
static int sweeping;

static void move_shutdown(void);
static void move_settrackedid(uint32_t, unsigned);
static void move_tick(void);
static int  move_canfreeze(void);
static void move_forceidle(void);
static int	move_getpokepos(vec4f* pos, vec4f* dir, float* len) {return 0;}

static yrTexture tpadbut_rot = NULL;
static yrTexture tpadbut_dist = NULL;
static yrTexture tpadbut_rot_glow = NULL;
static yrTexture tpadbut_dist_glow = NULL;
static yrTexture tpadbut_dot = NULL;
static yrTexture tpadbut_axisXY = NULL;
static yrTexture tpadbut_axisZ = NULL;
static yrTexture tpadbut_axisD = NULL;

#define DEADZONE 0.2f
#define ROT_SPEED 0.05f
#define DIST_SPEED 0.1f
#define DIST_MIN 0.0f
#define DIST_MAX 64.0f

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)
{
	*st_sd	= move_shutdown;
	*ft_sti	= move_settrackedid;
	*ft_t	= move_tick;
	*ft_cf	= move_canfreeze;
	*ft_fi	= move_forceidle;
	*ft_gpp = move_getpokepos;
	*ft_rw	= nullfunc;
	*ft_p	= nullfunc;

	tracked_id = 0xfffffffful;
	toolidx = 0;
	grabbed = NULL;
	mode = mtIdleDist;
	touchdown = 0;
	trigdown = 0;
	sweeping = 0;

	tpadbut_rot			= yrTexture_loadfile("data/vr/tpadbut_move_rot.png");		if(!tpadbut_rot) return -1;
	tpadbut_dist		= yrTexture_loadfile("data/vr/tpadbut_move_dist.png");		if(!tpadbut_dist) return -1;
	tpadbut_rot_glow	= yrTexture_loadfile("data/vr/tpadbut_move_rot_glow.png");	if(!tpadbut_rot_glow) return -1;
	tpadbut_dist_glow	= yrTexture_loadfile("data/vr/tpadbut_move_dist_glow.png");	if(!tpadbut_dist_glow) return -1;
	tpadbut_dot			= yrTexture_loadfile("data/vr/tpadbut_move_dot.png");		if(!tpadbut_dot) return -1;
	tpadbut_axisXY		= yrTexture_loadfile("data/vr/tpadbut_move_axisXY.png");	if(!tpadbut_axisXY) return -1;
	tpadbut_axisZ		= yrTexture_loadfile("data/vr/tpadbut_move_axisZ.png");		if(!tpadbut_axisZ) return -1;
	tpadbut_axisD		= yrTexture_loadfile("data/vr/tpadbut_move_axisD.png");		if(!tpadbut_axisD) return -1;

	return 0;
}

static void move_shutdown(void)
{
	yrTexture_unload(tpadbut_rot);
	yrTexture_unload(tpadbut_dist);
	yrTexture_unload(tpadbut_rot_glow);
	yrTexture_unload(tpadbut_dist_glow);
	yrTexture_unload(tpadbut_dot);
	yrTexture_unload(tpadbut_axisXY);
	yrTexture_unload(tpadbut_axisZ);
	yrTexture_unload(tpadbut_axisD);
	tpadbut_rot = NULL;
	tpadbut_dist = NULL;
	tpadbut_rot_glow = NULL;
	tpadbut_dist_glow = NULL;
	tpadbut_dot = NULL;
	tpadbut_axisXY = NULL;
	tpadbut_axisZ = NULL;
	tpadbut_axisD = NULL;
}

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

static int move_canfreeze(void)
{
	return grabbed == NULL;
}

static void pickup(tracehit h, const mat4f mat);
static void drop(void);
static void input_trig(float trig, int wasdown, const mat4f mat, const vec4i off);
static void input_touch(float x, float y, int down, int wasdown);
static void move(const mat4f mat, const vec4i off);

static void	move_forceidle(void)
{
	drop();
}

static void move_tick(void)
{
	//get controller state
	VRControllerState_t cbuttons = {0};
	ovrSystem_GetControllerState(tracked_id, &cbuttons);
	mat4f mat = yrVR_world_from_tracked(tracked_id);
	vec4i off = yrVR_get_coord_offset();

	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;

	//handle inputs
	input_trig(trig, trigdown, mat, off);
	input_touch(touchx, touchy, touchd, touchdown);

	touchdown = touchd;
	trigdown = (1.0f == trig);

	//handle move
	if(grabbed) move(mat, off);
}

static void input_trig(float trig, int wasdown, const mat4f mat, const vec4i off)
{
	//trace and show beam if trigger held while nothing is picked
	tracehit h = {0};
	if(trig > 0.3f && !grabbed && !yrQuadStore_editlocked()) {
		vec4i o = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat.col[3])));
		h = yrInteraction_trace(o, vec4f_neg(mat.col[2]), DIST_MIN, DIST_MAX);
		vec4f n = h.q ? h.q->normal : mat.col[2];
		yrRender_beam(toolidx, mat.col[3], vec4f_neg(mat.col[2]), n, h.dist, 0xFF808040);
	}
	//drop and pickup logic
	int trigevent = !wasdown && (trig == 1.0f);
	if(trigevent) {
		if(grabbed) drop();
		else sweeping = 1;
	}
	if(trig != 1.0f) sweeping = 0;
	if(sweeping && h.q) pickup(h, mat);
	//beam to grabbed
	if(grabbed) {
		vec4f n = grabbed->normal;
		yrRender_beam(toolidx, mat.col[3], vec4f_neg(mat.col[2]), n, grabdist, 0xFFFFFF80);
	}
}

static void pickup(tracehit h, const mat4f mat)
{
	YR_ASSERT(!grabbed);
	grabbed = h.q;
	grabbed_restore = *h.q;
	grabdist = h.dist;
	memset(rotbase, 0, sizeof(rotbase));
	rotbase[0].x = 1.0f;
	rotbase[1].y = 1.0f;

	v_off[0] = vec4f_from_vec4i(vec4i_sub(h.q->v[0], h.pos));
	v_off[1] = vec4f_from_vec4i(vec4i_sub(h.q->v[1], h.pos));
	v_off[2] = vec4f_from_vec4i(vec4i_sub(h.q->v[2], h.pos));
	vec4f oooi = {0.0f,0.0f,0.0f,1.0f};
	mat4f invmat = {mat.col[0],mat.col[1],mat.col[2],oooi};
	invmat = mat4f_invert(invmat);
	v_off[0] =  mat4f_apply(invmat, v_off[0]);
	v_off[1] =  mat4f_apply(invmat, v_off[1]);
	v_off[2] =  mat4f_apply(invmat, v_off[2]);

	yrQuadStore_editlock(grabbed);
	yrTileStore_important(grabbed->ext->tile_max_idx, (TileID*) grabbed->ext->tiles);
}

static void drop(void)
{
	if(grabbed) {
		yrQuadStore_editlock(NULL);
		yrUndo_append(utQuad, &grabbed_restore, grabbed, NULL, NULL);
		grabbed = NULL;
	}
}

static void touch_RotXY(float x, float y);
static void touch_RotZ(float x, float y);
static void touch_Dist(float x, float y);
static void draw_IdleDist(float x, float y, int down);
static void draw_IdleRot(float x, float y, int down);

static void input_touch(float x, float y, int down, int wasdown)
{
	if(mode == mtIdleDist || mode == mtIdleRot)
	{
		//detect mode swap
		if(wasdown && !down && x > 0.33f) {
			mode = (mode == mtIdleDist) ? mtIdleRot : mtIdleDist;
		}
		//detect drag start
		float dist = x*x + y*y;
		const float range2 = 0.111111111111f;
		if(grabbed && !down && dist < range2) {
			mode = (mode == mtIdleDist) ? mtDist : mtRotXY;
		}
		dist = (x + 0.66666666f)*(x + 0.66666666f) + y*y;
		if(grabbed && !down && (mode == mtIdleRot) && dist < range2) {
			mode = mtRotZ;
		}
	}
	//detect drag release
	int touch = (x != 0.0f) || (y != 0.0f);
	if(!touch) {
		if(mode == mtRotXY || mode == mtRotZ) {
			mode = mtIdleRot;
		}
		else if(mode == mtDist) {
			mode = mtIdleDist;
		}
	}
	//handle drag
	switch(mode) {
		case mtRotXY:		touch_RotXY(x,y); break;
		case mtRotZ:		touch_RotZ(x,y); break;
		case mtDist:		touch_Dist(x,y); break;
		case mtIdleDist:	draw_IdleDist(x,y,down); break;
		case mtIdleRot:		draw_IdleRot(x,y,down); break;
		default: YR_ASSERT(0); break;
	}
}

static void move(const mat4f mat, const vec4i off)
{
	YR_ASSERT(grabbed);
	vec4i ioff[3];
	mat4f rotation = {{rotbase[0], rotbase[1], vec3f_cross(rotbase[0],rotbase[1]), {0,0,0,1}}};
	rotation = mat4f_combine(mat, rotation);
	ioff[0] = vec4i_from_vec4f(mat4f_apply(rotation, v_off[0]));
	ioff[1] = vec4i_from_vec4f(mat4f_apply(rotation, v_off[1]));
	ioff[2] = vec4i_from_vec4f(mat4f_apply(rotation, v_off[2]));

	vec4i qoff = vec4i_add(off,
						   vec4i_from_vec4f(vec4f_mul(YR_ACCURACY,
													  vec4f_add(mat.col[3],
																vec4f_mul(-grabdist,
																		  mat.col[2])))));
	grabbed->v[0] = vec4i_add(qoff, ioff[0]);
	grabbed->v[1] = vec4i_add(qoff, ioff[1]);
	grabbed->v[2] = vec4i_add(qoff, ioff[2]);
	yrQuadStore_update(grabbed);
}


static void touch_RotXY(float x, float y)
{
	//calc rotation matrix
	float dX = (y - copysignf(DEADZONE, y))/(1.0f - DEADZONE);
	if(dX * y < 0.0f) dX = 0.0f;
	dX = copysignf(dX*dX, dX);
	float dY = (x - copysignf(DEADZONE, x))/(1.0f - DEADZONE);
	if(dY * x < 0.0f) dY = 0.0f;
	dY = copysignf(dY*dY, dY);
	mat4f m = {0};
	float cosX = cosf(-ROT_SPEED * dX);
	float sinX = sinf(-ROT_SPEED * dX);
	float cosY = cosf(-ROT_SPEED * dY);
	float sinY = sinf(-ROT_SPEED * dY);
	m.col[0].c[0] = cosY;
	m.col[0].c[1] = -sinX * sinY;
	m.col[0].c[2] = cosX * sinY;
	m.col[1].c[0] = 0.0f;
	m.col[1].c[1] = cosX;
	m.col[1].c[2] = sinX;
	m.col[2].c[0] = -sinY;
	m.col[2].c[1] = -sinX * cosY;
	m.col[2].c[2] = cosX * cosY;
	m.col[3].c[3] = 1.0f;

	//apply rotation and normalize basis
	rotbase[0] = vec3f_normalized(mat4f_apply(m, rotbase[0]));
	rotbase[1] = mat4f_apply(m, rotbase[1]);
	vec4f foo = vec3f_cross(rotbase[0], rotbase[1]);
	rotbase[1] = vec3f_normalized(vec3f_cross(foo, rotbase[0]));

	//draw axis + nub
	yrRenderTrackedOverlay_clear(toolidx);
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_axisXY, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	float dotY = (fabsf(y) > 0.875f) ? copysignf(y, 0.875f) : y;
	float dotX = (fabsf(x) > 0.875f) ? copysignf(x, 0.875f) : x;
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_dot, dotX - 0.25f, dotY - 0.25f, dotX + 0.25f, dotY + 0.25f, 0xFFFFFFFF);
	yrRenderTrackedOverlay_render(toolidx);
}

static void touch_RotZ(float x, float y)
{
	//calc rotation matrix
	float dZ = (y - copysignf(DEADZONE, y))/(1.0f - DEADZONE);
	if(dZ * y < 0.0f) dZ = 0.0f;
	dZ = -copysignf(dZ*dZ, dZ);
	mat4f m = {0};
	float cosZ = cosf(ROT_SPEED * dZ);
	float sinZ = sinf(ROT_SPEED * dZ);
	m.col[0].c[0] = cosZ;
	m.col[0].c[1] = sinZ;
	m.col[1].c[0] = -sinZ;
	m.col[1].c[1] = cosZ;
	m.col[2].c[2] = 1.0f;
	m.col[3].c[3] = 1.0f;

	//apply rotation and normalize basis
	rotbase[0] = vec3f_normalized(mat4f_apply(m, rotbase[0]));
	rotbase[1] = mat4f_apply(m, rotbase[1]);
	vec4f foo = vec3f_cross(rotbase[0], rotbase[1]);
	rotbase[1] = vec3f_normalized(vec3f_cross(foo, rotbase[0]));

	//draw axis + nub
	yrRenderTrackedOverlay_clear(toolidx);
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_axisZ, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	float dotY = y * (2.0f / 3.0f);
	float dotX = -sqrtf(1.0f - y*y) * (2.0f / 3.0f);
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_dot, dotX - 0.25f, dotY - 0.25f, dotX + 0.25f, dotY + 0.25f, 0xFFFFFFFF);
	yrRenderTrackedOverlay_render(toolidx);
}

static void touch_Dist(float x, float y)
{
	//recalc dist
	float delta = y - copysignf(DEADZONE, y);
	if(delta * y < 0.0f) delta = 0.0f;
	grabdist += delta * DIST_SPEED;
	if(grabdist < DIST_MIN) grabdist = DIST_MIN;
	if(grabdist > DIST_MAX) grabdist = DIST_MAX;

	//draw axis + nub
	yrRenderTrackedOverlay_clear(toolidx);
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_axisD, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	float dotY = (fabsf(y) > 0.875f) ? copysignf(y, 0.875f) : y;
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_dot, -0.25f, dotY - 0.25f, 0.25f, dotY + 0.25f, 0xFFFFFFFF);
	yrRenderTrackedOverlay_render(toolidx);
}

static void draw_IdleDist(float x, float y, int down)
{
	yrRenderTrackedOverlay_clear(toolidx);
	//draw icons
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_dist, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	//draw glow on touch
	if(x > 0.33333333f)
		yrRenderTrackedOverlay_draw(toolidx, tpadbut_dist_glow, -1.0f, -1.0f, 1.0f, 1.0f, down ? 0xFFFF8010 : 0x80FF8010);
	yrRenderTrackedOverlay_render(toolidx);
}

static void draw_IdleRot(float x, float y, int down)
{
	yrRenderTrackedOverlay_clear(toolidx);
	//draw icons
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_rot, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	//draw glow on touch
	if(x > 0.33333333f)
		yrRenderTrackedOverlay_draw(toolidx, tpadbut_rot_glow, -1.0f, -1.0f, 1.0f, 1.0f, down ? 0xFFFF8010 : 0x80FF8010);
	yrRenderTrackedOverlay_render(toolidx);
}
