#include "ObjectNode.h"

ObjectNode::ObjectNode()
{
	_nodeRootPosition = XMFLOAT4(0.0f, 0.0f, 0.0f, 0.0f);
	_moveLeftRight = 0.0f;
	_moveForwardBack = 0.0f;
	_nodeYaw = 0.0f;
	_nodePitch = 0.0f;
	_nodeRoll = 0.0f;
	XMStoreFloat4x4(&_originalOrientation, XMMatrixIdentity());
}

void ObjectNode::Update(XMFLOAT4X4& currentWorldTransform)
{
	XMVECTOR nodePosition;
	XMVECTOR nodeRight;
	XMVECTOR nodeForward;
	XMVECTOR nodeUp;

	XMVECTOR defaultForward = XMVectorSet(0.0f, 0.0f, 1.0f, 0.0f);
	XMVECTOR defaultRight = XMVectorSet(1.0f, 0.0f, 0.0f, 0.0f);
	XMVECTOR defaultUp = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);

	// Yaw (rotation around the Y axis) will have an impact on the forward and right vectors
	XMMATRIX nodeRotationYaw = XMMatrixRotationAxis(defaultUp, _nodeYaw);
	nodeRight = XMVector3TransformCoord(defaultRight, nodeRotationYaw);
	nodeForward = XMVector3TransformCoord(defaultForward, nodeRotationYaw);

	// Pitch (rotation around the X axis) impact the up and forward vectors
	XMMATRIX nodeRotationPitch = XMMatrixRotationAxis(nodeRight, _nodePitch);
	nodeUp = XMVector3TransformCoord(defaultUp, nodeRotationPitch);
	nodeForward = XMVector3TransformCoord(nodeForward, nodeRotationPitch);

	// Roll (rotation around the Z axis) will impact the Up and Right vectors
	XMMATRIX nodeRotationRoll = XMMatrixRotationAxis(nodeForward, _nodeRoll);
	nodeUp = XMVector3TransformCoord(nodeUp, nodeRotationRoll);
	nodeRight = XMVector3TransformCoord(nodeRight, nodeRotationRoll);

	// Adjust the node position by the appropriate amount forward/back and left/right
	nodePosition = XMLoadFloat4(&_nodeRootPosition) + _moveLeftRight * nodeRight + _moveForwardBack * nodeForward;
	XMStoreFloat4(&_nodeRootPosition, nodePosition);

	// Reset the amount we are moving
	_moveLeftRight = 0.0f;
	_moveForwardBack = 0.0f;

	XMMATRIX worldTransformation = XMLoadFloat4x4(&_originalOrientation) * nodeRotationYaw * nodeRotationPitch * nodeRotationRoll * XMMatrixTranslation(_nodeRootPosition.x, _nodeRootPosition.y, _nodeRootPosition.z);
	XMStoreFloat4x4(&currentWorldTransform, worldTransformation);
}

void ObjectNode::SetPitch(float pitch)
{
	_nodePitch += XMConvertToRadians(pitch);
}

void ObjectNode::SetTotalPitch(float pitch)
{
	_nodePitch = XMConvertToRadians(pitch);
}

float ObjectNode::GetPitch() const
{
	return XMConvertToDegrees(_nodePitch);
}

void ObjectNode::SetYaw(float yaw)
{
	_nodeYaw += XMConvertToRadians(yaw);
}

void ObjectNode::SetTotalYaw(float yaw)
{
	_nodeYaw = XMConvertToRadians(yaw);
}

float ObjectNode::GetYaw() const
{
	return XMConvertToDegrees(_nodeYaw);
}

void ObjectNode::SetRoll(float roll)
{
	_nodeRoll += XMConvertToRadians(roll);
}

void ObjectNode::SetTotalRoll(float roll)
{
	_nodeRoll = XMConvertToRadians(roll);
}

float ObjectNode::GetRoll() const
{
	return XMConvertToDegrees(_nodeRoll);
}

void ObjectNode::SetLeftRight(float leftRight)
{
	_moveLeftRight = leftRight;
}

float ObjectNode::GetLeftRight() const
{
	return _moveLeftRight;
}

void ObjectNode::SetForwardBack(float forwardBack)
{
	_moveForwardBack = forwardBack;
}

float ObjectNode::GetForwardBack() const
{
	return _moveForwardBack;
}

XMVECTOR ObjectNode::GetNodePosition(void)
{
	return XMLoadFloat4(&_nodeRootPosition);
}

void ObjectNode::SetNodePosition(float x, float y, float z)
{
	SetNodePosition(XMFLOAT4(x, y, z, 0.0f));
}

void ObjectNode::SetNodePosition(XMVECTOR vectorIn)
{
	XMFLOAT4 floatIn;
	XMStoreFloat4(&floatIn, vectorIn);
	SetNodePosition(floatIn);
}

void ObjectNode::SetNodePosition(XMFLOAT4 floatIn)
{
	_nodeRootPosition = floatIn;
}

void ObjectNode::SetStartOrientation(FXMMATRIX originalOrientation)
{
	XMStoreFloat4x4(&_originalOrientation, originalOrientation);
}