package com.pigeoncoop.extensions;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.math.*;
import com.badlogic.gdx.utils.Array;

/**
 * 3D transform system similar to Unity3D for libGDX
 * @author Tim Aksu - timur.s.aksu@gmail.com
 */
public class Transform {

	private static final String TAG = "Transform";

	private Vector3 			_localPosition;
	private Quaternion 			_localRotation;
	private Vector3 			_localScale;

	private Transform 			_parent;
	private Array<Transform> 	_children;
	private Matrix4 			_localToWorldMatrix;

	public Transform()
	{
		_localPosition = new Vector3(0,0,0);
		_localRotation = new Quaternion();
		_localScale = new Vector3(1,1,1);

		_parent = null;
		_children = new Array<Transform>();
		_localToWorldMatrix = new Matrix4(_localPosition, _localRotation, _localScale);
	}

	/**
	 *  Matrix that transforms a point from local space into world space
	 * @return A copy of this transforms local to world matrix
	 */
	public Matrix4 GetLocalToWorldMatrix()
	{
		return _localToWorldMatrix.cpy();
	}

	/**
	 * @return The current position in local space
	 */
	public Vector3 GetLocalPosition()
	{
		return _localPosition.cpy();
	}
	/**
	 * @return The current rotation in local space
	 */
	public Quaternion GetLocalRotation()
	{
		return _localRotation.cpy();
	}

	/**
	 * @return The current scale in local space
	 */
	public Vector3 GetLocalScale()
	{
		return _localScale.cpy();
	}

	/**
	 * @return The current position in world space
	 */
	public Vector3 GetPosition()
	{
		Vector3 result = new Vector3();
		_localToWorldMatrix.getTranslation(result);
		return result;
	}
	/**
	 * @return The current rotation in world space
	 */
	public Quaternion GetRotation()
	{
		Quaternion result = new Quaternion();
		_localToWorldMatrix.getRotation(result,true);
		return result;
	}

	/**
	 * @return The current scale in world space
	 */
	public Vector3 GetScale()
	{
		Vector3 result = new Vector3();
		_localToWorldMatrix.getScale(result);
		return result;
	}

	/**
	 * @param position The new local position
	 */
	public void SetLocalPosition(Vector3 position)
	{
		_localPosition.set(position);
		UpdateMatricies();
	}

	/**
	 * @param rotation The new local rotation
	 */
	public void SetLocalRotation(Quaternion rotation)
	{
		_localRotation.set(rotation);
		UpdateMatricies();
	}

	/**
	 * @param scale The new local scale
	 */
	public void SetLocalScale(Vector3 scale)
	{
		_localScale.set(scale);
		UpdateMatricies();
	}

	/**
	 * @param position The world position to match in local space
	 */
	public void SetPosition(Vector3 position)
	{
		if(_parent == null)
			_localPosition.set(position);
		else
			_localPosition.set(position.mul(_parent.GetLocalToWorldMatrix().inv()));

		UpdateMatricies();
	}

	/**
	 * @param rotation The world rotation to match in local space
	 */
	public void SetRotation(Quaternion rotation)
	{
		if(_parent == null)
		{
			_localRotation.set(rotation);
		}
		else
		{
			Quaternion temp = new Quaternion();
			_localRotation.set(rotation.mul(_parent.GetLocalToWorldMatrix().inv().getRotation(temp,true)));
		}

		UpdateMatricies();
	}

	/**
	 * @param scale The world scale to match in local space
	 */
	public void SetScale(Vector3 scale)
	{
		if(_parent == null)
			_localScale.set(scale);
		else
			_localScale.set(scale.mul(_parent.GetLocalToWorldMatrix().inv()));

		UpdateMatricies();
	}

	/**
	 * @return Current Root of the Transform hierarchy
	 * that this Transform exists in
	 */
	public Transform GetRoot()
	{
		if(_parent == null)
			return this;
		else
			return _parent.GetRoot();
	}

	/**
	 * @param target The transform to check
	 * @return True if this transform is a child of target (directly or indirectly)
	 */
	public boolean IsChildOf(Transform target)
	{
		boolean isChild = false;

		for (Transform child: target._children)
		{
			if(child == this)
				isChild = true;
			else
				isChild = IsChildOf(child);

			if(isChild)
				break;
		}

		return isChild;
	}

	/**
	 * @return The current parent (null if no parent)
	 */
	public Transform GetParent()
	{
		return _parent;
	}

	/**
	 * Sets the parent. Also modifies the transform
	 * to match its world space transform in its new local space.
	 *
	 * Calls RemoveParent if current parent is not null.
	 *
	 * Circular parenting will log an error and not do anything.
	 * This will result in an error: A->B->C->A
	 * @param to The transform to parent to
	 */
	public void SetParent(Transform to)
	{
		if(to.IsChildOf(this))
		{
			Gdx.app.error(TAG, "Prevented circular parenting");
			return;
		}

		if(_parent != null)
			RemoveParent();

		Matrix4 parentWorldToLocalMatrix = to.GetLocalToWorldMatrix().inv().mul(GetLocalToWorldMatrix());;

		parentWorldToLocalMatrix.getTranslation(_localPosition);
		parentWorldToLocalMatrix.getRotation(_localRotation,true);
		parentWorldToLocalMatrix.getScale(_localScale);

		to._children.add(this);
		_parent = to;
		to.UpdateMatricies();

	}

	/**
	 * Removed the current parent. Also modifies the transform
	 * to match its old local space transform in world space
	 */
	public void RemoveParent()
	{

		if(_parent == null)
		{
			return;
		}
		else
		{
			_localPosition = GetPosition();
			_localRotation = GetRotation();
			_localScale = GetScale();

			_parent._children.removeValue(this, true);
			_parent = null;

			UpdateMatricies();
		}
	}

	private void UpdateMatricies()
	{
		_localToWorldMatrix =  new Matrix4(_localPosition, _localRotation, _localScale);

		if(_parent != null)
		{
			_localToWorldMatrix = new Matrix4(_parent._localToWorldMatrix).mul(_localToWorldMatrix);
		}

		for(Transform child: _children)
		{
			child.UpdateMatricies();
		}
	}
}