#include "Camera.hpp"


Camera::Camera() {
    SetFrustrum(90.f, (1280.f/1024.f), 1.0f, 100.f);
}

void Camera::SetFrustrum(float fov, float r, float zn, float zf) {
    //if (!frameDirty)
	this->fov = fov;
    M3D_MATRIX pMat = M3D_TransformMatrixFrustrumFovLH(M3D_Deg2Rad(fov), r, zn, zf);
    M3D_V4StoreF4x4(&mProjMat, pMat);
	frustrumUpdated = true;
}

void Camera::SetFrustrumOrthographic(float w, float h, float zn, float zf) {
    //if (!frameDirty)
	this->fov = 90.f;
    M3D_MATRIX pMat = M3D_TransformMatrixOrthographicLH(w, h, zn, zf);
    M3D_V4StoreF4x4(&mProjMat, pMat);
	frustrumUpdated = true;
}

void Camera::UpdateCamView() {
    M3D_VECTOR P = M3D_V4LoadF3(&mPos);
    M3D_VECTOR L = M3D_V4LoadF3(&mLook);
    M3D_VECTOR U = M3D_V4LoadF3(&mUp);

    M3D_V4StoreF4x4(&mViewMat, M3D_TransformMatrixCamLookToLH(P, L, U));
	camMoved = true;
}

void Camera::LookAt(M3D_VECTOR pos, M3D_VECTOR target, M3D_VECTOR worldUp) {
	M3D_VECTOR L = M3D_V3Normalize(M3D_V4Subtract(target, pos));
	M3D_VECTOR R = M3D_V3Normalize(M3D_V3Cross(worldUp, L));
	M3D_VECTOR U = M3D_V3Cross(L, R);

	M3D_V4StoreF3(&mPos, pos);
	M3D_V4StoreF3(&mLook, L);
	M3D_V4StoreF3(&mRight, R);
	M3D_V4StoreF3(&mUp, U);
}

void Camera::LookAt(const M3D_F3& pos, const M3D_F3& target, const M3D_F3& up) {
	M3D_VECTOR P = M3D_V4LoadF3(&pos);
	M3D_VECTOR T = M3D_V4LoadF3(&target);
	M3D_VECTOR U = M3D_V4LoadF3(&up);

	LookAt(P, T, U);
}

void Camera::Strafe(float d) {
	// mPosition += d*mRight
	M3D_VECTOR s = M3D_V4Replicate(d);
	M3D_VECTOR r = M3D_V4LoadF3(&mRight);
	M3D_VECTOR p = M3D_V4LoadF3(&mPos);
	M3D_V4StoreF3(&mPos, M3D_V4MultiplyAdd(s, r, p));
}

void Camera::Walk(float d) {
	// mPosition += d*mLook
	M3D_VECTOR s = M3D_V4Replicate(d);
	M3D_VECTOR l = M3D_V4LoadF3(&mLook);
	M3D_VECTOR p = M3D_V4LoadF3(&mPos);
	M3D_V4StoreF3(&mPos, M3D_V4MultiplyAdd(s, l, p));
}

void Camera::Fly(float d) {
	// mPosition += d*mRight
	M3D_VECTOR s = M3D_V4Replicate(d);
	M3D_VECTOR u = M3D_V4LoadF3(&mUp);
	M3D_VECTOR p = M3D_V4LoadF3(&mPos);
	M3D_V4StoreF3(&mPos, M3D_V4MultiplyAdd(s, u, p));
}

void Camera::Pitch(float angle) {
	// Rotate up and look vector about the right vector.
	M3D_MATRIX R = M3D_TransformMatrixRotationAxis(M3D_V4LoadF3(&mRight), angle);

	M3D_V4StoreF3(&mUp, M3D_V3TransformNormal(M3D_V4LoadF3(&mUp), R));
	M3D_V4StoreF3(&mLook, M3D_V3TransformNormal(M3D_V4LoadF3(&mLook), R));
}

void Camera::Yaw(float angle) {
	// Rotate the basis vectors about the world y-axis.
	M3D_MATRIX R = M3D_TransformMatrixRotationY(angle);

	M3D_V4StoreF3(&mRight, M3D_V3TransformNormal(M3D_V4LoadF3(&mRight), R));
	M3D_V4StoreF3(&mUp, M3D_V3TransformNormal(M3D_V4LoadF3(&mUp), R));
	M3D_V4StoreF3(&mLook, M3D_V3TransformNormal(M3D_V4LoadF3(&mLook), R));
}