package leenkx.logicnode;

import iron.object.Object;
import iron.math.Vec4;
import iron.math.Mat4;
import iron.system.Time;

class SetObjectDelayedLocationNode extends LogicNode {
	public var use_local_space: Bool = false;

	private var initialOffset: Vec4 = null;
	private var targetPos: Vec4 = new Vec4();
	private var currentPos: Vec4 = new Vec4();
	private var deltaVec: Vec4 = new Vec4();
	private var tempVec: Vec4 = new Vec4();
	
	private var lastParent: Object = null;
	private var invParentMatrix: Mat4 = null;
	
	public function new(tree: LogicTree) {
		super(tree);
	}

	override function run(from: Int) {
		var follower: Object = inputs[1].get();
		var target: Object = inputs[2].get();
		var delay: Float = inputs[3].get();
		
		if (follower == null || target == null) return runOutput(0);
		
		if (initialOffset == null) {
			initialOffset = new Vec4();
			var followerPos = follower.transform.world.getLoc();
			var targetPos = target.transform.world.getLoc();
			initialOffset.setFrom(followerPos);
			initialOffset.sub(targetPos);
		}
		
		targetPos.setFrom(target.transform.world.getLoc());
		currentPos.setFrom(follower.transform.world.getLoc());
		
		tempVec.setFrom(targetPos).add(initialOffset);
		
		deltaVec.setFrom(tempVec).sub(currentPos);

		if (deltaVec.length() < 0.001 && delay < 0.01) {
			runOutput(0);
			return;
		}
		
		if (delay == 0.0) {
			currentPos.setFrom(tempVec);
		} else {
			var smoothFactor = Math.exp(-Time.delta / Math.max(0.0001, delay));
			currentPos.x = tempVec.x + (currentPos.x - tempVec.x) * smoothFactor;
			currentPos.y = tempVec.y + (currentPos.y - tempVec.y) * smoothFactor;
			currentPos.z = tempVec.z + (currentPos.z - tempVec.z) * smoothFactor;
		}
		if (use_local_space && follower.parent != null) {
			if (follower.parent != lastParent || invParentMatrix == null) {
				lastParent = follower.parent;
				invParentMatrix = Mat4.identity();
				invParentMatrix.getInverse(follower.parent.transform.world);
			}
			tempVec.setFrom(currentPos);
			tempVec.applymat(invParentMatrix); 
			follower.transform.loc.set(tempVec.x, tempVec.y, tempVec.z);
		} else {
			follower.transform.loc.set(currentPos.x, currentPos.y, currentPos.z);
		}
		follower.transform.buildMatrix();
		runOutput(0);
	}
}