package lnx;

import iron.math.Vec4;
import iron.math.Quat;
import iron.math.Mat4;
import iron.system.Input;
import iron.object.Object;
import iron.object.BoneAnimation;
import leenkx.trait.physics.PhysicsWorld;
import leenkx.trait.internal.CameraController;

class FirstPersonController extends CameraController {

#if (!lnx_physics)
	public function new() { super(); }
#else

	static inline var rotationSpeed = 0.5;

	public function new() {
		super();
		notifyOnInit(function() {
			PhysicsWorld.active.notifyOnPreUpdate(preUpdate);
			notifyOnUpdate(update);
			
			notifyOnRemove(function() {
				PhysicsWorld.active.removePreUpdate(preUpdate);
			});

		});
	}

	function findAnimation(o:Object):BoneAnimation {
		if (o.animation != null) return cast o.animation;
		for (c in o.children) {
			var co = findAnimation(c);
			if (co != null) return co;
		}
		return null;
	}

	var xVec = Vec4.xAxis();
	var zVec = Vec4.zAxis();
	var angle = 0.0;
	var nextFrameRot = 0.0;
	var anim:BoneAnimation = null;
	var q = new Quat();
	var mat = Mat4.identity();

	function preUpdate() {
		if (Input.occupied || !body.ready) return;
		
		var mouse = Input.getMouse();
		var kb = Input.getKeyboard();
		
		if (mouse.started() && !mouse.locked) mouse.lock();
		else if (kb.started("escape") && mouse.locked) mouse.unlock();
		
		if (nextFrameRot != 0.0) {
			var origin = object.getChild("CameraOrigin");
			origin.transform.rotate(xVec, nextFrameRot);
			origin.transform.buildMatrix();
		}
		nextFrameRot = 0;

		if (mouse.moved) {
			var d = mouse.movementY / 250;
			if (angle + d < 0.25 && angle + d > -0.25) {
				angle += d;
				nextFrameRot = mouse.movementY / 250 * rotationSpeed;
			}
		}
		
		if (anim == null) {
			anim = findAnimation(object.getChild("Armature"));
			anim.notifyOnUpdate(updateBones);
		}

		if (mouse.moved) transform.rotate(zVec, -mouse.movementX / 250 * rotationSpeed);
		body.syncTransform();
	}

	function updateBones() {
		// Fetch bone
		var bone1 = anim.getBone("mixamorig:LeftForeLnx");
		var bone2 = anim.getBone("mixamorig:RightForeLnx");
		// Fetch bone matrix - this is in local bone space for now
		var m1 = anim.getBoneMat(bone1);
		var m2 = anim.getBoneMat(bone2);
		var m1b = anim.getBoneMatBlend(bone1);
		var m2b = anim.getBoneMatBlend(bone2);
		var a1 = anim.getAbsMat(bone1.parent);
		var a2 = anim.getAbsMat(bone2.parent);

		// Rotate hand bones to aim with gun
		// Some raw math follows..
		var tx = m1._30;
		var ty = m1._31;
		var tz = m1._32;
		m1._30 = 0;
		m1._31 = 0;
		m1._32 = 0;
		mat.getInverse(a1);
		q.fromAxisAngle(mat.right(), angle);
		m1.applyQuat(q);
		m1._30 = tx;
		m1._31 = ty;
		m1._32 = tz;
		
		var tx = m2._30;
		var ty = m2._31;
		var tz = m2._32;
		m2._30 = 0;
		m2._31 = 0;
		m2._32 = 0;
		mat.getInverse(a2);
		var v = mat.right();
		v.mult(-1);
		// Todo: We do not do inverse kinematics just yet, right hand moves unnaturally
		// Point the hand down to the ground for now
		q.fromAxisAngle(v, -angle / 3.0);
		m2.applyQuat(q);
		m2._30 = tx;
		m2._31 = ty;
		m2._32 = tz;

		// Animation blending is in progress, we need to rotate those bones too
		if (m1b != null && m2b != null) {
			var tx = m1b._30;
			var ty = m1b._31;
			var tz = m1b._32;
			m1b._30 = 0;
			m1b._31 = 0;
			m1b._32 = 0;
			mat.getInverse(a1);
			q.fromAxisAngle(mat.right(), angle);
			m1b.applyQuat(q);
			m1b._30 = tx;
			m1b._31 = ty;
			m1b._32 = tz;
			
			var tx = m2b._30;
			var ty = m2b._31;
			var tz = m2b._32;
			m2b._30 = 0;
			m2b._31 = 0;
			m2b._32 = 0;
			mat.getInverse(a2);
			var v = mat.right();
			v.mult(-1);
			q.fromAxisAngle(v, -angle / 3.0);
			m2b.applyQuat(q);
			m2b._30 = tx;
			m2b._31 = ty;
			m2b._32 = tz;
		}
	}

	function update() {
		if (!body.ready) return;

		// Keep vertical
		body.setAngularFactor(0, 0, 0);
		camera.buildMatrix();
	}
#end
}