#include "Camera.h"

Camera::Camera()
{
	_nodeFollowed = nullptr;
    _cameraPosition = XMFLOAT4(0.0f, 0.0f, 0.0f, 0.0f);
    _moveLeftRight = 0.0f;
    _moveForwardBack = 0.0f;
    _cameraYaw = 0.0f;
    _cameraPitch = 0.0f;
    _cameraRoll = 0.0f;
	_followPositionOnly = false;
	_followOffset = XMFLOAT3(0.0f, 0.0f, 0.0f);
}

Camera::~Camera()
{
}

void Camera::SetPitch(float pitch)
{
    _cameraPitch += XMConvertToRadians(pitch);
}

void Camera::SetTotalPitch(float pitch)
{
	_cameraPitch = XMConvertToRadians(pitch);
}

float Camera::GetPitch() const
{
	return XMConvertToDegrees(_cameraPitch);
}

void Camera::SetYaw(float yaw)
{
    _cameraYaw += XMConvertToRadians(yaw);
}

void Camera::SetTotalYaw(float yaw)
{
	_cameraYaw = XMConvertToRadians(yaw);
}

float Camera::GetYaw() const
{
	return XMConvertToDegrees(_cameraYaw);
}

void Camera::SetRoll(float roll)
{
    _cameraRoll += XMConvertToRadians(roll);
}

void Camera::SetTotalRoll(float roll)
{
	_cameraRoll = XMConvertToRadians(roll);
}

float Camera::GetRoll() const
{
	return XMConvertToDegrees(_cameraRoll);
}

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

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

XMMATRIX Camera::GetViewMatrix(void)
{
    return XMLoadFloat4x4(&_viewMatrix);
}

XMVECTOR Camera::GetCameraPosition(void)
{
    return XMLoadFloat4(&_cameraPosition);
}

void Camera::SetCameraPosition(float x, float y, float z)
{
	SetCameraPosition(XMFLOAT4(x, y, z, 0.0f));
}

void Camera::SetCameraPosition(XMVECTOR vectorIn)
{
	XMFLOAT4 floatIn;
	XMStoreFloat4(&floatIn, vectorIn);
	SetCameraPosition(floatIn);
}

void Camera::SetCameraPosition(XMFLOAT4 floatIn)
{
	_cameraPosition = floatIn;
}

void Camera::SetFollowNode(shared_ptr<ObjectNode> nodeFollowed, XMFLOAT3 followOffset, bool positionOnly)
{
	_nodeFollowed = nodeFollowed;
	_followOffset = followOffset;
	_followPositionOnly = positionOnly;
}

void Camera::Update(void)
{
	XMVECTOR cameraPosition;
	XMVECTOR cameraTarget;
	XMVECTOR cameraRight;
	XMVECTOR cameraForward;
	XMVECTOR cameraUp;

	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);

	if (_nodeFollowed == nullptr)
	{
		// Yaw (rotation around the Y axis) will have an impact on the forward and right vectors
		XMMATRIX cameraRotationYaw = XMMatrixRotationAxis(defaultUp, _cameraYaw);
		cameraRight = XMVector3TransformCoord(defaultRight, cameraRotationYaw);
		cameraForward = XMVector3TransformCoord(defaultForward, cameraRotationYaw);

		// Pitch (rotation around the X axis) impact the up and forward vectors
		XMMATRIX cameraRotationPitch = XMMatrixRotationAxis(cameraRight, _cameraPitch);
		cameraUp = XMVector3TransformCoord(defaultUp, cameraRotationPitch);
		cameraForward = XMVector3TransformCoord(cameraForward, cameraRotationPitch);

		// Roll (rotation around the Z axis) will impact the Up and Right vectors
		XMMATRIX cameraRotationRoll = XMMatrixRotationAxis(cameraForward, _cameraRoll);
		cameraUp = XMVector3TransformCoord(cameraUp, cameraRotationRoll);
		cameraRight = XMVector3TransformCoord(cameraRight, cameraRotationRoll);

		// Adjust the camera position by the appropriate amount forward/back and left/right
		cameraPosition = XMLoadFloat4(&_cameraPosition) + _moveLeftRight * cameraRight + _moveForwardBack * cameraForward;
		XMStoreFloat4(&_cameraPosition, cameraPosition);

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

		// Calculate a vector that tells us the direction the camera is looking in
		cameraTarget = cameraPosition + XMVector3Normalize(cameraForward);
	}
	else
	{
		if (_followPositionOnly)
		{			
			_cameraYaw = 0.0f;
			_cameraPitch = 0.0f;
			_cameraRoll = 0.0f;
		}
		else
		{			
			_cameraYaw = XMConvertToRadians(_nodeFollowed->GetYaw());
			_cameraPitch = XMConvertToRadians(_nodeFollowed->GetPitch());
			_cameraRoll = XMConvertToRadians(_nodeFollowed->GetRoll());
		}

		// Yaw (rotation around the Y axis) will have an impact on the forward and right vectors
		XMMATRIX cameraRotationYaw = XMMatrixRotationAxis(defaultUp, _cameraYaw);
		cameraRight = XMVector3TransformCoord(defaultRight, cameraRotationYaw);
		cameraForward = XMVector3TransformCoord(defaultForward, cameraRotationYaw);

		// Pitch (rotation around the X axis) impact the up and forward vectors
		XMMATRIX cameraRotationPitch = XMMatrixRotationAxis(cameraRight, _cameraPitch);
		cameraUp = XMVector3TransformCoord(defaultUp, cameraRotationPitch);
		cameraForward = XMVector3TransformCoord(cameraForward, cameraRotationPitch);

		// Roll (rotation around the Z axis) will impact the Up and Right vectors
		XMMATRIX cameraRotationRoll = XMMatrixRotationAxis(cameraForward, _cameraRoll);
		cameraUp = XMVector3TransformCoord(cameraUp, cameraRotationRoll);
		cameraRight = XMVector3TransformCoord(cameraRight, cameraRotationRoll);

		XMFLOAT4 nodePosition;
		XMStoreFloat4(&nodePosition, _nodeFollowed->GetNodePosition());
		if (_followPositionOnly)
		{
			cameraPosition = XMVectorSet(nodePosition.x + _followOffset.x, nodePosition.y + _followOffset.y, nodePosition.z + _followOffset.z, 1.0f);
			cameraTarget = cameraPosition + XMVector3Normalize(cameraForward);
		}
		else
		{	
			// Trying some rotation so that the camera actually follows the model instead of staying in place
			XMVECTOR offset = XMVectorSet(_followOffset.x, _followOffset.y, _followOffset.z, 1.0f);
			// The pitch and roll rotations really mess it up and i dont have time to look into it, the simple turn follow is good enough for now
			XMMATRIX offsetRotationMatrix = XMMatrixRotationAxis(defaultUp, _cameraYaw); // *XMMatrixRotationAxis(defaultForward, _cameraPitch);

			cameraPosition = _nodeFollowed->GetNodePosition() + XMVector3TransformCoord(offset, offsetRotationMatrix);
			cameraTarget = XMLoadFloat4(&nodePosition) + XMVector3Normalize(cameraForward);
		}
		// Set the camera position
		XMStoreFloat4(&_cameraPosition, cameraPosition);
	}

	// and calculate our view matrix
	XMStoreFloat4x4(&_viewMatrix, XMMatrixLookAtLH(cameraPosition, cameraTarget, cameraUp));
}