#include "Camera.hpp"


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

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

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

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