#include "camera.h"

Camera::Camera(void)
{
	Reset();
}

Camera::Camera(const DXMatrix& matview, const DXMatrix& matproj)
{
	SetViewMatrix(matview);
	SetProjMatrix(matproj);
}

Camera::~Camera(void)
{
}

void Camera::Reset(void)
{
	m_vPosition = DXVector3(0.f,0.f,0.f);
	D3DXQuaternionIdentity(&m_qtRotation);
	D3DXMatrixIdentity((D3DXMATRIX*)&m_matView);
	D3DXMatrixIdentity((D3DXMATRIX*)&m_matProj);
	m_Near = 0;
	m_Far = 0;
}

void Camera::SetRotation(const D3DXQUATERNION& rot)
{
	m_qtRotation = rot;
	D3DXMatrixRotationQuaternion((D3DXMATRIX*)&m_matView,&m_qtRotation);
	const DXVector3 up = GetViewUpVector();
	const DXVector3 right = GetViewRightVector();
	const DXVector3 dir = GetViewDirVector();
	m_matView._41 = -D3DXVec3Dot(right, m_vPosition);
	m_matView._42 = -D3DXVec3Dot(up, m_vPosition);
	m_matView._43 = -D3DXVec3Dot(dir, m_vPosition);
}

void Camera::SetLookAt(const DXVector3& lookat, const DXVector3& up)
{
	D3DXMatrixLookAtLH((D3DXMATRIX*)&m_matView, m_vPosition, lookat, up);
	D3DXQuaternionRotationMatrix(&m_qtRotation, (D3DXMATRIX*)&m_matView);
}

void Camera::SetProjMatrix(const DXMatrix& matProj)
{
	m_matProj = matProj; 
	m_Near = -m_matProj._43 / m_matProj._33; 
	m_Far = m_matProj._43 / (1 - m_matProj._33); 
}

void Camera::SetViewMatrix(const DXMatrix& matView)
{
	m_matView = matView;
	//D3DXQuaternionRotationMatrix(&m_qtRotation,(D3DXMATRIX*)&m_matView); 
	//m_vPosition = DXVector3(0,0,0);

	D3DXQUATERNION rot;
	D3DXQuaternionRotationMatrix(&m_qtRotation,(D3DXMATRIX*)&m_matView); 
	m_vPosition = -DXVector3(m_matView._41, m_matView._42, m_matView._43);
	DXMatrix v = m_matView;
	v._41 = v._42 = v._43 = 0;
	D3DXMatrixInverse((D3DXMATRIX*)&v, NULL, (D3DXMATRIX*)&v);
	D3DXVec3TransformNormal(m_vPosition, m_vPosition, (D3DXMATRIX*)&v);
}

void Camera::RotateOriented(const DXVector3& angle)
{
	D3DXQUATERNION tmprot;
	if(angle.x)
	{
		D3DXQuaternionRotationAxis(&tmprot, DXVector3(1.0f, 0.0f, 0.0f), angle.x);
		m_qtRotation *= tmprot;
	}
	if(angle.y)
	{
		D3DXQuaternionRotationAxis(&tmprot, DXVector3(0.0f, 1.0f, 0.0f), angle.y);
		m_qtRotation *= tmprot;
	}
	if(angle.z)
	{
		D3DXQuaternionRotationAxis(&tmprot, DXVector3(0.0f, 0.0f, 1.0f), angle.z);
		m_qtRotation *= tmprot;
	}

	D3DXMatrixRotationQuaternion((D3DXMATRIX*)&m_matView,&m_qtRotation);
	const DXVector3 up = GetViewUpVector();
	const DXVector3 right = GetViewRightVector();
	const DXVector3 dir = GetViewDirVector();
	m_matView._41 = -D3DXVec3Dot(right, m_vPosition);
	m_matView._42 = -D3DXVec3Dot(up, m_vPosition);
	m_matView._43 = -D3DXVec3Dot(dir, m_vPosition);
}

void Camera::Rotate(const DXVector3& angle)
{
	D3DXQUATERNION tmprot; 
	if(angle.x)
	{
		D3DXMATRIX matrot;
		D3DXMatrixRotationQuaternion(&matrot, &m_qtRotation); 
		DXVector3 ax(1.0f, 0.0f, 0.0f);
		D3DXQuaternionRotationAxis(&tmprot,D3DXVec3TransformNormal(ax, ax, &matrot), angle.x);
		m_qtRotation *= tmprot;
	}
	if(angle.y)
	{
		D3DXMATRIX matrot;
		D3DXMatrixRotationQuaternion(&matrot, &m_qtRotation); 
		DXVector3 ax(0.0f, 1.0f, 0.0f);
		D3DXQuaternionRotationAxis(&tmprot,D3DXVec3TransformNormal(ax, ax, &matrot), angle.y);
		m_qtRotation *= tmprot;
	}
	if(angle.z)
	{
		D3DXMATRIX matrot;
		D3DXMatrixRotationQuaternion(&matrot, &m_qtRotation); 
		DXVector3 ax(0.0f, 0.0f, 1.0f);
		D3DXQuaternionRotationAxis(&tmprot,D3DXVec3TransformNormal(ax, ax, &matrot), angle.z);
		m_qtRotation *= tmprot;
	}

	//D3DXMatrixRotationQuaternion(&m_matView,&D3DXQUATERNION(-m_qtRotation.x,-m_qtRotation.y,-m_qtRotation.z,m_qtRotation.w));
	D3DXMatrixRotationQuaternion((D3DXMATRIX*)&m_matView,&m_qtRotation);
	const DXVector3 up = GetViewUpVector(); 
	const DXVector3 right = GetViewRightVector();
	const DXVector3 dir = GetViewDirVector();
	m_matView._41 = -D3DXVec3Dot(right, m_vPosition);
	m_matView._42 = -D3DXVec3Dot(up, m_vPosition);
	m_matView._43 = -D3DXVec3Dot(dir, m_vPosition);
}

void Camera::Move(const DXVector3& dp)
{
	SetPosition(m_vPosition + dp);
}

void Camera::MoveOriented(const DXVector3& dp)
{
	SetPosition(m_vPosition + dp.x * GetViewRightVector() + dp.y * GetViewUpVector() + dp.z * GetViewDirVector());
}

void Camera::SetPosition(const DXVector3& pos)
{
	m_vPosition = pos;
	const DXVector3 up = GetViewUpVector();
	const DXVector3 right = GetViewRightVector();
	const DXVector3 dir = GetViewDirVector();
	m_matView._41 = -D3DXVec3Dot(right, m_vPosition);
	m_matView._42 = -D3DXVec3Dot(up, m_vPosition);
	m_matView._43 = -D3DXVec3Dot(dir, m_vPosition);
}

DXVector3 Camera::GetYawPitchRoll(void) const
{
	DXVector3 out;
	out.x = asin(-m_matView._32); 
	float test = cos(out.x); 
	if(test > 0.001f)
	{
		out.z = atan2(m_matView._12, m_matView._22);
		out.y = atan2(m_matView._31, m_matView._33); 
    } 
    else
    { 
		out.z = atan2(-m_matView._21, m_matView._11); 
		out.y = 0.0; 
	} 
	return out;
}

void Camera::FlipSide(void)
{
	m_matView._11 = -m_matView._11;
	m_matView._21 = -m_matView._21;
	m_matView._31 = -m_matView._31;
	m_matView._41 = -m_matView._41;
}

//
//void Camera::Move(DXVector3 dir)
//{
//	m_vPosition.x += dir.x * cos(m_RotSide) + dir.y * -sin(m_RotSide);
//	m_vPosition.y += dir.y * cos(m_RotSide) + dir.x * sin(m_RotSide);
//
//	if(dir.z)
//	{
//		DXVector3 lookdir = GetLookDirection();
//		m_vPosition += lookdir * dir.z;
//	}
//}
//
//void Camera::RotPoint(DXVector3 point,float rotup,float rotside)
//{
//	DXVector3 r = m_vPosition - point;
//
//	DXVector3 uprotvect(cos(m_RotSide),sin(m_RotSide),0.f);
//
//	D3DXMATRIX rot;
//	if(rotup != 0.f)
//	{
//		D3DXMatrixRotationAxis(&rot,&uprotvect,rotup);
//		D3DXVec3TransformCoord(&r,&r,&rot);
//	}
//	if (rotside != 0.f)
//	{
//		D3DXMatrixRotationZ(&rot,rotside);
//		D3DXVec3TransformCoord(&r,&r,&rot);
//	}
//
//	DXVector3 newpos = r + point;
//
//	m_RotSide = m_RotSide + rotside;
//	m_RotUp = m_RotUp - rotup;
//
//	m_vPosition = newpos;
//}
//
//void Camera::Roll(float roll)
//{
//
//}
//
//DXVector3 Camera::GetCamUpVector(void)
//{
//	DXVector3 dir,right,up;
//
//	dir.x = sin(m_RotSide) * sin(m_RotUp);
//	dir.y = -cos(m_RotSide) * sin(m_RotUp);
//	dir.z = -cos(m_RotUp);
//
//	up.x = 0;
//	up.y = -cos(m_RotUp);
//	up.z = sin(m_RotUp);
//
//	D3DXMATRIX rotmat;
//	D3DXMatrixRotationZ(&rotmat,m_RotSide);
//	D3DXVec3TransformCoord(&up,&up,&rotmat);
//
//	D3DXMATRIX rollmat;
//	D3DXMatrixRotationAxis(&rollmat,&dir,m_Roll);
//	D3DXVec3TransformCoord(&up,&up,&rollmat);
//
//	return up;
//}
//
//DXVector3 Camera::GetCamRightVector(void)
//{
//	DXVector3 dir,right,up;
//
//	dir.x = sin(m_RotSide) * sin(m_RotUp);
//	dir.y = -cos(m_RotSide) * sin(m_RotUp);
//	dir.z = -cos(m_RotUp);
//
//	up.x = 0;
//	up.y = -cos(m_RotUp);
//	up.z = sin(m_RotUp);
//
//	D3DXMATRIX rotmat;
//	D3DXMatrixRotationZ(&rotmat,m_RotSide);
//	D3DXVec3TransformCoord(&up,&up,&rotmat);
//
//	D3DXMATRIX rollmat;
//	D3DXMatrixRotationAxis(&rollmat,&dir,m_Roll);
//	D3DXVec3TransformCoord(&up,&up,&rollmat);
//
//	if(m_bFlipSide)
//		D3DXVec3Cross(&right,&dir,&up);
//	else
//		D3DXVec3Cross(&right,&up,&dir);
//
//	return right;
//}
//
//DXVector3 Camera::GetLookDirection(void)
//{
//	DXVector3 lkdir;
//	lkdir.x = sin(m_RotSide) * sin(m_RotUp);
//	lkdir.y = -cos(m_RotSide) * sin(m_RotUp);
//	lkdir.z = -cos(m_RotUp);
//	return lkdir;
//}
//void Camera::SetLookDirection(DXVector3 dir)
//{
//	D3DXVec3Normalize(&dir,&dir);
//	DXVector2 v(dir.x,dir.y);
//	D3DXVec2Normalize(&v,&v);
//	if(v.x > 0.f)
//		m_RotSide = acos(-v.y); 
//	else
//		m_RotSide = -acos(-v.y); 
//	m_RotUp = asin(dir.z) + D3DX_PI/2.f; 
//}
//
//D3DXMATRIX Camera::GetViewMatrix(void)
//{
//	D3DXMATRIX viewmat;
//
//	DXVector3 dir,right,up;
//
//	dir.x = sin(m_RotSide) * sin(m_RotUp);
//	dir.y = -cos(m_RotSide) * sin(m_RotUp);
//	dir.z = -cos(m_RotUp);
//
//	up.x = 0;
//	up.y = -cos(m_RotUp);
//	up.z = sin(m_RotUp);
//
//	D3DXMATRIX rotmat;
//	D3DXMatrixRotationZ(&rotmat,m_RotSide);
//	D3DXVec3TransformCoord(&up,&up,&rotmat);
//
//	D3DXMATRIX rollmat;
//	D3DXMatrixRotationAxis(&rollmat,&dir,m_Roll);
//	D3DXVec3TransformCoord(&up,&up,&rollmat);
//
//	if(m_bFlipSide)
//		D3DXVec3Cross(&right,&dir,&up);
//	else
//		D3DXVec3Cross(&right,&up,&dir);
//	//xaxis.x yaxis.x zaxis.x 0
//	//xaxis.y yaxis.y zaxis.y 0
//	//xaxis.z yaxis.z zaxis.z 0
//	//-pos.x -pos.y -pos.z 1
//
//	viewmat._11 = right.x;
//	viewmat._12 = up.x;
//	viewmat._13 = dir.x;
//	viewmat._14 = 0.f;
//	viewmat._21 = right.y;
//	viewmat._22 = up.y;
//	viewmat._23 = dir.y;
//	viewmat._24 = 0.f; 
//	viewmat._31 = right.z;
//	viewmat._32 = up.z;
//	viewmat._33 = dir.z;
//	viewmat._34 = 0.f;
//	viewmat._41 = -D3DXVec3Dot(&right,&m_vPosition);
//	viewmat._42 = -D3DXVec3Dot(&up,&m_vPosition);
//	viewmat._43 = -D3DXVec3Dot(&dir,&m_vPosition);
//	viewmat._44 = 1.f;
//
//	return viewmat;
//}

void Camera::BuildView(const DXVector3& Position, const DXVector3& ViewDir, const DXVector3& ViewRight, const DXVector3& ViewUp)
{
	m_matView._11 = ViewRight.x;
	m_matView._12 = ViewUp.x;
	m_matView._13 = ViewDir.x;
	m_matView._14 = 0.f;
	m_matView._21 = ViewRight.y;
	m_matView._22 = ViewUp.y;
	m_matView._23 = ViewDir.y;
	m_matView._24 = 0.f; 
	m_matView._31 = ViewRight.z;
	m_matView._32 = ViewUp.z;
	m_matView._33 = ViewDir.z;
	m_matView._34 = 0.f;
	m_matView._41 = -D3DXVec3Dot(ViewRight, Position);
	m_matView._42 = -D3DXVec3Dot(ViewUp, Position);
	m_matView._43 = -D3DXVec3Dot(ViewDir, Position);
	m_matView._44 = 1.f;
}

void Camera::GetMousePickRay(DXVector3& outOrig, DXVector3& outDir, const int MouseX, const int MouseY, const D3DVIEWPORT9* const ViewPort) const
{
	DXVector3 vPickRayDir;
    DXVector3 vPickRayOrig;
	DXVector3 vec;
	
	vec.x =  ( ( ( 2.0f * MouseX ) / ViewPort->Width ) - 1.f ) / m_matProj._11;
    vec.y = -( ( ( 2.0f * MouseY ) / ViewPort->Height ) - 1.f ) / m_matProj._22;
    vec.z =  1.0f;

    // Get the inverse view matrix
    D3DXMATRIX m;
    D3DXMatrixInverse( &m, NULL, (D3DXMATRIX*)&m_matView );

    // Transform the screen space pick ray into 3D space
    outDir.x  = vec.x*m._11 + vec.y*m._21 + vec.z*m._31; //* RightVector
    outDir.y  = vec.x*m._12 + vec.y*m._22 + vec.z*m._32; //* UpVector
    outDir.z  = vec.x*m._13 + vec.y*m._23 + vec.z*m._33; //* DirVectr
    outOrig.x = m._41;
    outOrig.y = m._42;
    outOrig.z = m._43;
}