#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"

#define PEN_OFF_X (0.0f)
#define PEN_OFF_Y (-0.0186f)
#define PEN_OFF_Z (0.21f)
#define PEN_OFF {PEN_OFF_X,PEN_OFF_Y,PEN_OFF_Z,1.0f}
#define PEN_DIR_X (0.0f)
#define PEN_DIR_Y (-0.08715574274765817f)
#define PEN_DIR_Z (0.9961946980917455f)
#define PEN_LENGTH (0.03f)
#define PEN_DIR {PEN_DIR_X,PEN_DIR_Y,PEN_DIR_Z,0.0f}
#define PEN_NOTDIR {0.0f,1.0f,0.0f,0.0f}

#define ERASE_WIDTH (0.10f)
#define ERASE_HEIGHT (0.03f)
#define ERASE_DEPTH (0.04f)
#define ERASE_RADIUS (0.05f)
#define ERASE_STEP (0.01f)
//define a transformation that makes the -1,1 cube the erase volume
#define ERASE_COL0 {ERASE_WIDTH/2, 0.0f, 0.0f, 0.0f}
#define ERASE_COL1 {0.0f, -ERASE_HEIGHT * 0.7071067811865476f / 2, ERASE_HEIGHT * 0.7071067811865476f / 2, 0.0f}
#define ERASE_COL2 {0.0f, -ERASE_DEPTH * 0.7071067811865476f / 2, -ERASE_DEPTH * 0.7071067811865476f / 2, 0.0f}
#define ERASE_COL3 {0.0f, -0.02707107f, -0.02707107f, 1.0f}
#define ERASE_MAT {{ERASE_COL0,ERASE_COL1,ERASE_COL2,ERASE_COL3}}

static uint32_t tracked_id = 0xfffffffful;
static yrModel mdl_pen_indic = NULL;
static yrModel mdl_pen_tip = NULL;
static yrTexture tex_pen_indic_on = NULL;
static yrTexture tex_pen_indic_off = NULL;
static yrTexture tex_pen_tip = NULL;
static yrTexture tex_dot = NULL;
enum mode { modeIdle, modePen, modeErase, mode_end };
static enum mode pen_mode;

static struct {
	vec4f erasevol[8];
	yrQuad* q;
	uint64_t edited_mask;
	float lastdot[2];
	int flipsign;
	int started;
	int haptic_count;
	uint32_t tile_restore[64];
} edit;

static void pen_shutdown(void);
static void pen_settrackedid(uint32_t,unsigned);
static void pen_tick(void);
static int  pen_canfreeze(void);
static void pen_forceidle(void);
static int	pen_getpokepos(vec4f* pos, vec4f* dir, float* len);

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)
{
	*st_sd	= pen_shutdown;
	*ft_sti	= pen_settrackedid;
	*ft_t	= pen_tick;
	*ft_cf	= pen_canfreeze;
	*ft_fi	= pen_forceidle;
	*ft_gpp = pen_getpokepos;
	*ft_rw	= nullfunc;
	*ft_p	= nullfunc;

	mdl_pen_indic =		yrModel_load("data/vr/pen_indic.ymd");				 if(!mdl_pen_indic) return -1;
	mdl_pen_tip =		yrModel_load("data/vr/pen_tip.ymd");				 if(!mdl_pen_tip) return -1;
	tex_pen_indic_on =	yrTexture_loadfile("data/vr/pen_indic_on.png");	 if(!tex_pen_indic_on) return -1;
	tex_pen_indic_off =	yrTexture_loadfile("data/vr/pen_indic_off.png");	 if(!tex_pen_indic_off) return -1;
	tex_pen_tip =		yrTexture_loadfile("data/vr/pen_tip.png");			 if(!tex_pen_tip) return -1;
	tex_dot =			yrTexture_loadfile("data/vr/dot.png");				 if(!tex_dot) return -1;

	return 0;
}

static void pen_shutdown(void)
{
	yrTexture_unload(tex_dot);
	yrTexture_unload(tex_pen_tip);
	yrTexture_unload(tex_pen_indic_off);
	yrTexture_unload(tex_pen_indic_on);
	yrModel_unload(mdl_pen_tip);
	yrModel_unload(mdl_pen_indic);
	tex_dot = NULL;
	tex_pen_tip = NULL;
	tex_pen_indic_off = NULL;
	tex_pen_indic_on = NULL;
	mdl_pen_tip = NULL;
	mdl_pen_indic = NULL;
}

static void pen_settrackedid(uint32_t t_id, unsigned tool_idx)
{
	pen_forceidle();
	tracked_id = t_id;
}

static yrQuad* detect_start_pen(mat4f mat, vec4i off)
{
	//trace pen
	vec4f pen_off = PEN_OFF;
	vec4f pen_dir = PEN_DIR;
	vec4i pen_ioff = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, mat4f_apply(mat, pen_off))));
	pen_dir = mat4f_apply(mat, pen_dir);
	tracehit th = yrInteraction_trace(pen_ioff, pen_dir, -PEN_LENGTH, PEN_LENGTH/2);
	//return hit
	return th.q;
}

static yrQuad* detect_start_erase(mat4f mat, vec4i off)
{
	//set up corners and dir
	vec4f cornerf[4] = {
		{  1.0f,  1.0f, -1.0f, 1.0f},
		{  1.0f, -1.0f, -1.0f, 1.0f},
		{ -1.0f,  1.0f, -1.0f, 1.0f},
		{ -1.0f, -1.0f, -1.0f, 1.0f}};
	vec4i corner[4];
	vec4f dir = {0.0f, 0.0f, 1.0f, 0.0f};

	mat4f tool_from_erase = ERASE_MAT;
	mat4f world_from_erase = mat4f_combine(mat, tool_from_erase);
	for(size_t i = 0; i < 4; ++i) {
		cornerf[i] = mat4f_apply(world_from_erase, cornerf[i]);
		corner[i] = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY, cornerf[i])));
	}
	dir = vec3f_normalized(mat4f_apply(world_from_erase, dir));

	//trace erase corner x 4
	return yrInteraction_erasetrace(corner, dir, ERASE_DEPTH, ERASE_RADIUS);
}

static void pen_finish(void)
{
	yrRenderOverlay_apply(OVERLAY_EDIT,edit.q,edit.edited_mask, 0);
	yrUndo_append(utTile, NULL, edit.q, edit.tile_restore, edit.q->ext->tiles);
	yrQuadStore_editlock(NULL);
	edit.q->flags |= QUAD_CLEARCACHE;
	edit.started = 0;
	pen_mode = modeIdle;
}

static void erase_finish(void)
{
	yrRenderOverlay_apply(OVERLAY_EDIT, edit.q, edit.edited_mask, 1);
	yrUndo_append(utTile, NULL, edit.q, edit.tile_restore, edit.q->ext->tiles);
	yrQuadStore_editlock(NULL);
	edit.q->flags |= QUAD_CLEARCACHE;
	edit.started = 0;
	pen_mode = modeIdle;
}

static void pen_drawdot(float u, float v, yrColor c)
{
	if(edit.haptic_count != 0xff) ++edit.haptic_count;
	edit.lastdot[0] = u;
	edit.lastdot[1] = v;
	float lou = u - 0.01f;
	float lov = v - 0.01f;
	float hiu = u + 0.01f;
	float hiv = v + 0.01f;
	yrRenderOverlay_draw(OVERLAY_EDIT, tex_dot, lou,lov,hiu,hiv,c);
	int ilox = (int) max(lou, 0.0f);
	int iloy = (int) max(lov, 0.0f);
	int ihix = (int) min(hiu, 7.0f);
	int ihiy = (int) min(hiv, 7.0f);
	uint64_t mask = 0;
	mask |= (1ull << (ilox + edit.q->tile_width * iloy));
	mask |= (1ull << (ihix + edit.q->tile_width * iloy));
	mask |= (1ull << (ilox + edit.q->tile_width * ihiy));
	mask |= (1ull << (ihix + edit.q->tile_width * ihiy));
	edit.edited_mask |= mask;
}

static void pen_lineto(float u, float v, float smoothing, yrColor c)
{
	u -= smoothing * (u - edit.lastdot[0]);
	v -= smoothing * (v - edit.lastdot[1]);
	float dx = u - edit.lastdot[0];
	float dy = v - edit.lastdot[1];
	float dist = sqrtf(dx*dx + dy*dy);
	float spacing = 0.01f / 3;
	if(spacing > dist) return;
	dx /= dist;
	dy /= dist;
	
	for(float i = 0.0f; i < dist - spacing; i += spacing) {
		pen_drawdot(edit.lastdot[0] + spacing * dx, edit.lastdot[1] + spacing * dy, c);
	}
}

static vec4f pen_toquadspace(vec4i penpos, float* u, float* v)
{
	//transform to quad space
	vec4f out = vec4f_mul(1.0f / YR_ACCURACY,
						  vec4f_from_vec4i(vec4i_sub(penpos,
													 edit.q->v[0])));
	out = mat4f_apply(edit.q->ext->invtransform, out);
	if(edit.flipsign) out.z = -out.z;
	*u = edit.q->uv_draw[0] + out.x * UV_PER_METER;
	*v = edit.q->uv_draw[1] + out.y * UV_PER_METER;
	return out;
}

static void erase_drawsingle(vec4f* volume)
{
	if(edit.haptic_count != 0xff) edit.haptic_count += 1;
	//check bounds
	float foo,bar,baz,qux;
	foo = min(volume[0].x, volume[1].x);
	bar = min(volume[2].x, volume[3].x);
	baz = min(volume[4].x, volume[5].x);
	qux = min(volume[6].x, volume[7].x);
	foo = min(foo, bar);
	bar = min(baz, qux);
	float lou = min(foo,bar);
	foo = min(volume[0].y, volume[1].y);
	bar = min(volume[2].y, volume[3].y);
	baz = min(volume[4].y, volume[5].y);
	qux = min(volume[6].y, volume[7].y);
	foo = min(foo, bar);
	bar = min(baz, qux);
	float lov = min(foo,bar);
	foo = max(volume[0].x, volume[1].x);
	bar = max(volume[2].x, volume[3].x);
	baz = max(volume[4].x, volume[5].x);
	qux = max(volume[6].x, volume[7].x);
	foo = max(foo, bar);
	bar = max(baz, qux);
	float hiu = max(foo,bar);
	foo = max(volume[0].y, volume[1].y);
	bar = max(volume[2].y, volume[3].y);
	baz = max(volume[4].y, volume[5].y);
	qux = max(volume[6].y, volume[7].y);
	foo = max(foo, bar);
	bar = max(baz, qux);
	float hiv = max(foo,bar);

	//erase
	yrRenderOverlay_mask_erase(OVERLAY_EDIT,ERASE_DEPTH,volume, edit.q->color);

	//mark edited tiles
	int ilox = (int) max(lou, 0.0f);
	int iloy = (int) max(lov, 0.0f);
	int ihix = (int) min(hiu, 7.0f);
	int ihiy = (int) min(hiv, 7.0f);
	uint64_t mask = 0;
	mask |= (1ull << (ilox + edit.q->tile_width * iloy));
	mask |= (1ull << (ihix + edit.q->tile_width * iloy));
	mask |= (1ull << (ilox + edit.q->tile_width * ihiy));
	mask |= (1ull << (ihix + edit.q->tile_width * ihiy));
	edit.edited_mask |= mask;
}

static void erase_draw(vec4f* volume)
{
	//calculate lerp step
	vec4f delta[8];
	float length = 0.0f;
	for(size_t i = 0; i < 8; ++i) {
		delta[i] = vec4f_sub(volume[i], edit.erasevol[i]);
		float len = vec3f_length(delta[i]);
		if(len > length) length = len;
	}
	size_t steps = (size_t)(length / ERASE_STEP);
	for(size_t i = 0; i < 8; ++i) {
		delta[i] = vec4f_mul(1.0f/steps, delta[i]);
	}
	//do lerp
	for(size_t s = 0; s < steps; ++s) {
		for(size_t i = 0; i < 8; ++i) {
			edit.erasevol[i] = vec4f_add(edit.erasevol[i], delta[i]);
		}
		erase_drawsingle(edit.erasevol);
	}
}

static void pen_tick(void)
{	
	VRControllerState_t cbuttons = {0};
	ovrSystem_GetControllerState(tracked_id, &cbuttons);
	mat4f mat = yrVR_world_from_tracked(tracked_id);
	vec4i off = yrVR_get_coord_offset();
	yrColor color = yrInteraction_getpencolor();
	vec4f penpos;

	if(pen_mode == modePen) {
		vec4f pen_off = PEN_OFF;
		vec4f pen_dir = PEN_DIR;
		vec4i pen_ioff = vec4i_add(off, vec4i_from_vec4f(vec4f_mul(YR_ACCURACY,  mat4f_apply(mat, pen_off))));
		pen_dir = mat4f_apply(mat, pen_dir);
		//detect end
		tracehit hit = yrInteraction_intersect(edit.q, pen_ioff, pen_dir);
		if(hit.dist < -2*PEN_LENGTH || hit.dist > PEN_LENGTH)
		{
			pen_finish();
		}
		//draw
		else {
			yrRenderOverlay_render(OVERLAY_EDIT, edit.q, 1);
			//draw on the overlay
			float u,v;
			penpos = pen_toquadspace(pen_ioff, &u, &v);
			if(!edit.started)
			{
				if(fabs(penpos.z) <= 0.003f) {
					edit.started = 1;
					ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(300.0f * 1.0f));
					edit.flipsign = vec3f_dot(pen_dir, edit.q->normal) > 0.0f;
					pen_drawdot(u, v, color);
				}
			}
			else {
				if(penpos.z <= 0.003f) {
					pen_lineto(u, v, 0.70f, color);
				}
				else {
					pen_lineto(u, v, 0.70f, color);
					edit.started = 0;
					ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(300.0f * 1.0f));
				}
				if(edit.haptic_count >= 3) {
					float pen_intensity = 1.0f - (max(penpos.z + 0.002f, 0.0f)/0.005f);
					ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(300.0f * pen_intensity * 0.5f));
					edit.haptic_count = 0;
				}
			}
		}
		//render pen indicator
		vec4i zero = {0,0,0,0};
		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},
		}};
		yrModel_render(mdl_pen_indic, edit.started ? tex_pen_indic_on : tex_pen_indic_off, identity, zero, color, tracked_id);

		//render pen tip
		if(!edit.started) {
			//not drawing, normal position
			mat4f tipmat = {{{0.0f,0.0f,0.0f,0.0f},PEN_NOTDIR,PEN_DIR,PEN_OFF}};
			tipmat.col[0] = vec3f_cross(tipmat.col[1],tipmat.col[2]);
			tipmat.col[1] = vec3f_cross(tipmat.col[2],tipmat.col[0]);
			yrModel_render(mdl_pen_tip, tex_pen_tip, tipmat, zero, color, tracked_id);
		}
		else {
			//drawing, put tip on drawpos
			vec4i adjust = vec4i_from_vec4f(vec4f_mul(YR_ACCURACY,
													  vec4f_mul(edit.flipsign ? penpos.z : -penpos.z,
																edit.q->normal)));
			vec4i base = vec4i_from_vec4f(vec4f_mul(-YR_ACCURACY * PEN_LENGTH,
													pen_dir));
			vec4i tip_off = vec4i_add(pen_ioff, adjust);
			vec4i tip_base = vec4i_add(pen_ioff, base);
			vec4f actual_dir = vec4f_from_vec4i(vec4i_sub(tip_off, tip_base));
			actual_dir = vec3f_normalized(actual_dir);
			vec4f notdir = actual_dir;
			if(fabsf(notdir.x) < fabsf(notdir.y)) notdir.x = 1.0f;
			else notdir.y = 1.0f;
			notdir = vec3f_normalized(notdir);

			mat4f tipmat = identity;
			tipmat.col[2] = actual_dir;
			tipmat.col[1] = vec3f_normalized(vec3f_cross(actual_dir, notdir));
			tipmat.col[0] = vec3f_cross(actual_dir, tipmat.col[1]);
			tipmat.col[3] = vec4f_mul(1.0f/YR_ACCURACY, vec4f_from_vec4i(vec4i_sub(tip_off,off)));

			tipmat = mat4f_combine(mat4f_invert(mat), tipmat);
			vec4i zero = {0,0,0,0};
			yrModel_render(mdl_pen_tip, tex_pen_tip, tipmat, zero, color, tracked_id);
		}
	}
	else if(pen_mode == modeErase) {
		//draw overlay
		yrRenderOverlay_render(OVERLAY_EDIT, edit.q, 1);
		//check the normal to know whether the z should be inverted
		vec4f dir = ERASE_COL2;
		dir = mat4f_apply(mat, dir);
		float flip = copysignf(1.0f, -vec3f_dot(edit.q->normal, dir));
		//corners to quad space
		mat4f tool_from_erase = ERASE_MAT;
		mat4f world_from_erase = mat4f_combine(mat, tool_from_erase);
		vec4f qoffset = vec4f_mul(1.0f/YR_ACCURACY, vec4f_from_vec4i(vec4i_sub(off, edit.q->v[0])));
		qoffset.w = 0.0f;
		world_from_erase.col[3] = vec4f_add(world_from_erase.col[3], qoffset);
		mat4f quad_from_world = edit.q->ext->invtransform;
		quad_from_world.col[0].c[0] *= UV_PER_METER;
		quad_from_world.col[0].c[1] *= UV_PER_METER;
		quad_from_world.col[0].c[2] *= flip;
		quad_from_world.col[1].c[0] *= UV_PER_METER;
		quad_from_world.col[1].c[1] *= UV_PER_METER;
		quad_from_world.col[1].c[2] *= flip;
		quad_from_world.col[2].c[0] *= UV_PER_METER;
		quad_from_world.col[2].c[1] *= UV_PER_METER;
		quad_from_world.col[2].c[2] *= flip;
		quad_from_world.col[3].c[0] *= UV_PER_METER;
		quad_from_world.col[3].c[1] *= UV_PER_METER;
		quad_from_world.col[3].c[2] *= flip;
		quad_from_world.col[3].c[0] += edit.q->uv_draw[0];
		quad_from_world.col[3].c[1] += edit.q->uv_draw[1];
		mat4f quad_from_erase = mat4f_combine(quad_from_world, world_from_erase);
		vec4f corner[8] = {
			{  1.0f,  1.0f, 1.0f, 1.0f},
			{  1.0f, -1.0f, 1.0f, 1.0f},
			{ -1.0f, -1.0f, 1.0f, 1.0f},
			{ -1.0f,  1.0f, 1.0f, 1.0f},
			{  1.0f,  1.0f, -1.0f, 1.0f},
			{  1.0f, -1.0f, -1.0f, 1.0f},
			{ -1.0f, -1.0f, -1.0f, 1.0f},
			{ -1.0f,  1.0f, -1.0f, 1.0f}};
		for(size_t i = 0; i < 8; ++i) {
			corner[i] = mat4f_apply(quad_from_erase, corner[i]);
		}
		//detect eraser end
		if((corner[0].z >= 0.0f &&
			corner[1].z >= 0.0f &&
			corner[2].z >= 0.0f &&
			corner[3].z >= 0.0f) ||
		   (corner[0].z < -ERASE_DEPTH &&
			corner[1].z < -ERASE_DEPTH &&
			corner[2].z < -ERASE_DEPTH &&
			corner[3].z < -ERASE_DEPTH))
		{
			//end erase
			ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(300.0f * 1.0f));
			erase_finish();
		}
		//do eraser stuff
		else {
			//init edit.erasevol
			if(!edit.started) {
				edit.started = 1;
				memcpy(edit.erasevol, corner, sizeof(corner));
				ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(300.0f * 1.0f));
			}
			//draw
			erase_draw(corner);
			if(edit.haptic_count >= 5) {
				ovrSystem_TriggerHapticPulse(tracked_id, 0, (unsigned short)(139.0f * 1.0f));
				edit.haptic_count = 0;
			}
		}
		//draw tip and indicator
		vec4i zero = {0,0,0,0};
		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},
		}};
		mat4f tipmat = {{{0.0f,0.0f,0.0f,0.0f},PEN_NOTDIR,PEN_DIR,PEN_OFF}};
		tipmat.col[0] = vec3f_cross(tipmat.col[1],tipmat.col[2]);
		tipmat.col[1] = vec3f_cross(tipmat.col[2],tipmat.col[0]);
		
		yrModel_render(mdl_pen_indic, tex_pen_indic_on, identity, zero, color, tracked_id);
		yrModel_render(mdl_pen_tip, tex_pen_tip, tipmat, zero, color, tracked_id);
	}
	else if(pen_mode == modeIdle) {
		yrQuad* hitquad = NULL;
		if(!yrQuadStore_editlocked() && 
			(hitquad = detect_start_pen(mat, off)))
		{
			//init pen mode
			edit.q = hitquad;
			edit.edited_mask = 0;
			edit.started = 0;
			edit.haptic_count = 0;
			memcpy(edit.tile_restore, edit.q->ext->tiles, 64 * sizeof(uint32_t));
			yrRenderOverlay_clear(OVERLAY_EDIT);
			yrQuadStore_editlock(edit.q);
			yrTileStore_important(edit.q->ext->tile_max_idx, (TileID*) edit.q->ext->tiles);
			pen_mode = modePen;
		}
		else if(!yrQuadStore_editlocked() &&
				(hitquad = detect_start_erase(mat, off)))
		{
			//init erase mode
			edit.q = hitquad;
			edit.edited_mask = 0;
			edit.started = 0;
			edit.haptic_count = 0;
			memcpy(edit.tile_restore, edit.q->ext->tiles, 64 * sizeof(uint32_t));
			yrRenderOverlay_clear(OVERLAY_EDIT);
			yrQuadStore_editlock(edit.q);
			yrTileStore_important(edit.q->ext->tile_max_idx, (TileID*) edit.q->ext->tiles);
			pen_mode = modeErase;
		}
		//draw tip and indicator
		vec4i zero = {0,0,0,0};
		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},
		}};
		mat4f tipmat = {{{0.0f,0.0f,0.0f,0.0f},PEN_NOTDIR,PEN_DIR,PEN_OFF}};
		tipmat.col[0] = vec3f_cross(tipmat.col[1],tipmat.col[2]);
		tipmat.col[1] = vec3f_cross(tipmat.col[2],tipmat.col[0]);
		
		yrModel_render(mdl_pen_indic, tex_pen_indic_off, identity, zero, color, tracked_id);
		yrModel_render(mdl_pen_tip, tex_pen_tip, tipmat, zero, color, tracked_id);
	}
}

static int pen_canfreeze(void)
{
	return pen_mode == modeIdle;
}

static void	pen_forceidle(void)
{
	if(pen_mode == modePen) {
		pen_finish();
	}
	if(pen_mode == modeErase) {
		erase_finish();
	}
}

static int pen_getpokepos(vec4f* pos, vec4f* dir, float* len)
{
	if(pen_mode == modeIdle) {
		mat4f mat = yrVR_world_from_tracked(tracked_id);
		vec4f foo = PEN_OFF;
		vec4f bar = PEN_DIR;
		*pos = mat4f_apply(mat, foo);
		*dir = mat4f_apply(mat, bar);
		*len = PEN_LENGTH;
		return 1;
	}
	else {
		*len = -1.0f;
		return 0;
	}
}