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

#define ARC_G (-9.81f)
#define ARC_V0 (14.39f)
#define ARC_ABORT (0.9f)
#define DUR_CLOSE (100)
#define DUR_MARGIN (23)
#define DUR_OPEN (244)

static uint32_t tracked_id = 0xfffffffful;
static unsigned toolidx = 0;
static float arc_time = -1.0f;
static yrModel mdl_compass = NULL;
static yrTexture tex_compass = NULL;
static yrTexture tpadbut_teleport = NULL;
static yrTexture tpadbut_teleport_glow = NULL;
static uint64_t time_unlock = 0;
static uint64_t time_open = 0;
static vec4i destination;

static void teleport_shutdown(void);
static void teleport_settrackedid(uint32_t, unsigned);
static void teleport_tick(void);
static int  teleport_canfreeze(void);
static void teleport_forceidle(void);
static int	teleport_getpokepos(vec4f* pos, vec4f* dir, float* len) {return 0;}

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)
{
	*st_sd	= teleport_shutdown;
	*ft_sti	= teleport_settrackedid;
	*ft_t	= teleport_tick;
	*ft_cf	= teleport_canfreeze;
	*ft_fi	= teleport_forceidle;
	*ft_gpp = teleport_getpokepos;
	*ft_rw	= nullfunc;
	*ft_p	= nullfunc;

	tracked_id = 0xfffffffful;
	toolidx = 0;
	arc_time = -1.0f;
	time_unlock = 0;
	time_open = 0xFFFFFFFFFFFFFFFFull;
	mdl_compass = yrModel_load("data/vr/compass.ymd");			if(!mdl_compass)	{ return -1; }
	tex_compass = yrTexture_loadfile("data/vr/compass.png");	if(!tex_compass)	{ return -1; }
	tpadbut_teleport		= yrTexture_loadfile("data/vr/tpadbut_teleport.png");			if(!tpadbut_teleport) return -1;
	tpadbut_teleport_glow	= yrTexture_loadfile("data/vr/tpadbut_teleport_glow.png");		if(!tpadbut_teleport_glow) return -1;

	return 0;
}

static void teleport_shutdown(void)
{
	yrTexture_unload(tex_compass);
	yrModel_unload(mdl_compass);
	tex_compass = NULL;
	mdl_compass = NULL;
	yrTexture_unload(tpadbut_teleport);
	yrTexture_unload(tpadbut_teleport_glow);
	tpadbut_teleport = NULL;
	tpadbut_teleport_glow = NULL;
}

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

static void teleport_tick(void)
{
	uint64_t now = yr_get_microtime();
	if(now > time_open) {
		yrVR_set_coord_offset(destination);
		ovrCompositor_FadeToColor(DUR_OPEN/1000.0f, 0.0f, 0.0f, 0.0f, 0.0f);
		time_open = 0xFFFFFFFFFFFFFFFFull;
	}
	//get pos and buttons
	VRControllerState_t cbuttons = {0};
	ovrSystem_GetControllerState(tracked_id, &cbuttons);
	mat4f mat = yrVR_world_from_tracked(tracked_id);
	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));

	//draw buttons
	yrRenderTrackedOverlay_clear(toolidx);
	if((touchd || touchx != 0.0f || touchy != 0.0f) && (now > time_unlock)) {
		yrRenderTrackedOverlay_draw(toolidx, tpadbut_teleport_glow, -1.0, -1.0f, 1.0f, 1.0f, touchd ? 0xFFFF8010 : 0x80FF8010);
	}
	yrRenderTrackedOverlay_draw(toolidx, tpadbut_teleport, -1.0f, -1.0f, 1.0f, 1.0f, 0xFFFFFFFF);
	yrRenderTrackedOverlay_render(toolidx);

	if(touchd && (now > time_unlock))
	{
		//calculate & draw arc
		vec4f forward = {0.0f,0.0f,-1.0f,0.0f};
		forward = mat4f_apply(mat, forward);
		float zc = mat.col[3].z;
		float zb = forward.z * ARC_V0;
		float za = ARC_G;
		arc_time = (-zb - sqrtf(zb*zb - 4*za*zc))/(2*za);
		if(arc_time > 0) {
			vec4f start = mat.col[3];
			mat4f hmdmat = yrVR_world_from_tracked(0);
			vec4f hmd_off = hmdmat.col[3];
			arc_time = (forward.z > ARC_ABORT) ? -0.014f : arc_time; //negate time to indicate aborted teleport
			yrRender_teleport(start, vec4f_mul(ARC_V0, forward), arc_time, hmd_off);
		}
	}
	else {
		//do teleport
		if(arc_time > 0) {
			vec4i off = yrVR_get_coord_offset();
			mat4f hmdmat = yrVR_world_from_tracked(0);
			vec4f forward = {0.0f,0.0f,-1.0f,0.0f};
			forward = mat4f_apply(mat, forward);
			vec4f foo = vec4f_mul(ARC_V0 * arc_time, forward);
			foo = vec4f_add(foo, mat.col[3]);
			foo = vec4f_sub(foo, hmdmat.col[3]);
			foo = vec4f_mul(YR_ACCURACY, foo);
			off = vec4i_add(off, vec4i_from_vec4f(foo));
			off.z = 0;
			//set the new offset
			if(forward.z <= ARC_ABORT) {
				destination = off;
				time_open = now + (DUR_CLOSE+DUR_MARGIN) * 1000;
				time_unlock = time_open + (DUR_OPEN+DUR_MARGIN) * 1000;
				ovrCompositor_FadeToColor(DUR_CLOSE/1000.0f, 0.0f, 0.0f, 0.0f, 1.0f);
			}
			//done
			arc_time = -1.0f;
		}
	}
	//base model is already rendered by the interaction system
	//now render a compass next to it
	vec4f compass_offset = {0.0f,-0.03f,-0.06f,1.0f};
	vec4i compass_center = vec4i_add(yrVR_get_coord_offset(), vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat4f_apply(mat, compass_offset))));
	compass_center.w = 0;
	compass_center.z = 0;
	vec4f compass_z = vec3f_normalized(vec4f_from_vec4i(vec4i_neg(compass_center)));
	vec4f up = {{0.0f,0.0f,1.0f,0.0f}};
	vec4f compass_x = vec3f_normalized(vec3f_cross(compass_z, up));
	vec4f compass_y = vec3f_cross(compass_x, compass_z);
	mat4f world_from_compass = {{compass_x, compass_y, compass_z, {0.0f,0.0f,0.0f,1.0f}}};
	mat4f world_from_tool = mat;
	mat4f tool_from_compass = mat4f_combine(mat4f_invert(world_from_tool), world_from_compass);
	tool_from_compass.col[3] = compass_offset;
	vec4i zero = {0,0,0,0};
	yrModel_render(mdl_compass, tex_compass, tool_from_compass, zero, 0xFF2288FFul, tracked_id);
}

static int teleport_canfreeze(void)
{
	return arc_time <= 0.0f;
}

static void	teleport_forceidle(void)
{
	//abort current teleport
	arc_time = -1;
}
