#include "inc.h"
#include "camera.h"

#define PI 3.14159265359f
#ifndef min
#define min(a, b) (a < b ? a : b)
#endif  

#define DEGTORAD(r) ((r)/180.0f*M_PI)

using rw::Quat;
using rw::V3d;

void
CCamera::process(Pad *pad)
{
	float sensitivity = 1.0f;

	const float zoomspeed = 0.2f;
	const float orbitspeed = 0.04f;

	if(pad->now.square) zoom(zoomspeed * sensitivity);
	if(pad->now.cross) zoom(-zoomspeed * sensitivity);
	orbit(pad->now.lX*orbitspeed*sensitivity, -pad->now.lY*orbitspeed*sensitivity);
	turn(-pad->now.rX*orbitspeed*sensitivity, -pad->now.rY*orbitspeed*sensitivity);
	if(pad->now.Dup) dolly(zoomspeed * sensitivity);
	if(pad->now.Ddown) dolly(-zoomspeed * sensitivity);
	if(pad->now.Dright) pan(zoomspeed * sensitivity, 0.0f);
	if(pad->now.Dleft) pan(-zoomspeed * sensitivity, 0.0f);
}

void
CCamera::update(void)
{
	if(m_rwcam){
		m_rwcam->setFOV(m_fov, m_aspectRatio);

		rw::Frame *f = m_rwcam->getFrame();
		if(f){
			m_at = normalize(sub(m_target, m_position));
			f->matrix.lookAt(m_at, m_up);
			f->matrix.pos = m_position;
			f->updateObjects();
		}
	}
}


void
CCamera::setTarget(V3d target)
{
	m_position = sub(m_position, sub(m_target, target));
	m_target = target;
}

float
CCamera::getHeading(void)
{
	V3d dir = sub(m_target, m_position);
	float a = atan2(dir.y, dir.x)-PI/2.0f;
	return m_localup.z < 0.0f ? a-PI : a;
}

void
CCamera::turn(float yaw, float pitch)
{
	V3d dir = sub(m_target, m_position);
	V3d zaxis = { 0.0f, 0.0f, 1.0f };
	Quat r = Quat::rotation(yaw, zaxis);
	dir = rotate(dir, r);
	m_localup = rotate(m_localup, r);

	V3d right = normalize(cross(dir, m_localup));
	r = Quat::rotation(pitch, right);
	dir = rotate(dir, r);
	m_localup = normalize(cross(right, dir));
	if(m_localup.z >= 0.0) m_up.z = 1.0;
	else m_up.z = -1.0f;

	m_target = add(m_position, dir);
}

void
CCamera::orbit(float yaw, float pitch)
{
	V3d dir = sub(m_target, m_position);
	V3d zaxis = { 0.0f, 0.0f, 1.0f };
	Quat r = Quat::rotation(yaw, zaxis);
	dir = rotate(dir, r);
	m_localup = rotate(m_localup, r);

	V3d right = normalize(cross(dir, m_localup));
	r = Quat::rotation(-pitch, right);
	dir = rotate(dir, r);
	m_localup = normalize(cross(right, dir));
	if(m_localup.z >= 0.0) m_up.z = 1.0;
	else m_up.z = -1.0f;
	m_position = sub(m_target, dir);
}

void
CCamera::dolly(float dist)
{
	V3d dir = setlength(sub(m_target, m_position), dist);
	m_position = add(m_position, dir);
	m_target = add(m_target, dir);
}

void
CCamera::zoom(float dist)
{
	V3d dir = sub(m_target, m_position);
	float curdist = length(dir);
	if(dist >= curdist)
		dist = curdist-0.3f;
	dir = setlength(dir, dist);
	m_position = add(m_position, dir);
}

void
CCamera::pan(float x, float y)
{
	V3d dir = normalize(sub(m_target, m_position));
	V3d right = normalize(cross(dir, m_up));
	V3d localup = normalize(cross(right, dir));
	dir = add(scale(right, x), scale(localup, y));
	m_position = add(m_position, dir);
	m_target = add(m_target, dir);
}

void
CCamera::setDistanceFromTarget(float dist)
{
	V3d dir = sub(m_position, m_target);
	dir = scale(normalize(dir), dist);
	m_position = add(m_target, dir);
}

float
CCamera::distanceTo(V3d v)
{
	return length(sub(m_position, v));
}

float
CCamera::distanceToTarget(void)
{
	return length(sub(m_position, m_target));
}

// calculate minimum distance to a sphere at the target
// so the whole sphere is visible
float
CCamera::minDistToSphere(float r)
{
	float t = min(m_rwcam->viewWindow.x, m_rwcam->viewWindow.y);
	float a = atanf(t);	// half FOV angle
	return r/sinf(a);
}

CCamera::CCamera()
{
	m_position.set(0.0f, 6.0f, 0.0f);
	m_target.set(0.0f, 0.0f, 0.0f);

	m_up.set(0.0f, 0.0f, 1.0f);
	m_localup = m_up;
	m_fov = 70.0f;
	m_aspectRatio = 1.0f;
	m_rwcam = nil;
}

bool
CCamera::IsSphereVisible(rw::Sphere *sph, rw::Matrix *xform)
{
	rw::Sphere sphere = *sph;
	rw::V3d::transformPoints(&sphere.center, &sphere.center, 1, xform);
	return m_rwcam->frustumTestSphere(&sphere) != rw::Camera::SPHEREOUTSIDE;
}