#include "mapviewer.h"

void CCamera::Init()
{
	SetYaw(1.4387f);
	SetPitch(-0.5176f);
	SetPosition(852.0f, -1120.0f, 260.0f);
	ReadSavedPositions();
}

void CCamera::CalcLookVector()
{
	GLfloat t1, t2;

	/* Map both angles to 0 - 2*pi */
	if (Pitch < 0)
		Pitch += 2*M_PI;
	else if (Pitch > 2*M_PI)
		Pitch -= 2*M_PI;

	if (Yaw < 0)
		Yaw += 2*M_PI;
	else if (Yaw > 2*M_PI)
		Yaw-= 2*M_PI;
	if (Pitch < M_PI*3/2 && Pitch > M_PI/2)
		Up[2] = -1.0f;
	else
		Up[2] = 1.0f;

	LookVector[0] = cos(Pitch);
	LookVector[1] = 0;
	LookVector[2] = sin(Pitch);

	t1 = (LookVector[0] * cos(Yaw)) - (LookVector[1] * sin(Yaw));
	t2 = (LookVector[1] * cos(Yaw)) + (LookVector[0] * sin(Yaw));
	LookVector[0] = t1;
	LookVector[1] = t2;
}

void CCamera::CalcCameraPosition()
{
	GLfloat target[3];

	/* get absolute position of target */
	target[0] = LookVector[0]*ArcLength + Position[0];
	target[1] = LookVector[1]*ArcLength + Position[1];
	target[2] = LookVector[2]*ArcLength + Position[2];

	CalcLookVector();

	/* New camera position */
	Position[0] = target[0] - LookVector[0]*ArcLength;
	Position[1] = target[1] - LookVector[1]*ArcLength;
	Position[2] = target[2] - LookVector[2]*ArcLength;
}

GLfloat CCamera::NormalizeVector(GLfloat *v)
{
	GLfloat d;

	d = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	v[0] /= d;
	v[1] /= d;
	v[2] /= d;

	return d;
}

void CCamera::SetYaw(GLfloat y)
{
	Yaw = y;
	CalcLookVector();
}

GLfloat CCamera::GetYaw() { return Yaw; }

void CCamera::SetPitch(GLfloat p)
{
	Pitch = p;
	CalcLookVector();
}

GLfloat CCamera::GetPitch() { return Pitch; }

void CCamera::SetPosition(GLfloat x, GLfloat y, GLfloat z)
{
	Position[0] = x;
	Position[1] = y;
	Position[2] = z;
}

void CCamera::ArcUpDown(GLfloat ang)
{
	Pitch += ang;
	CalcCameraPosition();
}
void CCamera::ArcLeftRight(GLfloat ang)
{
	Yaw += ang;
	CalcCameraPosition();
}

void CCamera::Dolly(GLfloat dist)
{
	/* Similar to CalcCameraPosition */

	GLfloat target[3];

	target[0] = LookVector[0]*ArcLength + Position[0];
	target[1] = LookVector[1]*ArcLength + Position[1];
	target[2] = LookVector[2]*ArcLength + Position[2];

	ArcLength += dist;

	Position[0] = target[0] - LookVector[0]*ArcLength;
	Position[1] = target[1] - LookVector[1]*ArcLength;
	Position[2] = target[2] - LookVector[2]*ArcLength;
}

void CCamera::MoveTargetInOut(GLfloat dist)
{
	ArcLength += dist;
}

void CCamera::SetTarget(GLfloat x, GLfloat y, GLfloat z)
{
	GLfloat pitch, yaw, dist;
	GLfloat dotproduct;
	GLfloat direction[3];
	GLfloat helper[3];

	direction[0] = x - Position[0];
	direction[1] = y - Position[1];
	direction[2] = z - Position[2];

	dist = NormalizeVector(direction);

	/* Project onto x-y-plane, calculate dot product and angle */
	helper[0] = direction[0];
	helper[1] = direction[1];
	helper[2] = 0;
	NormalizeVector(helper);
	dotproduct = direction[0]*helper[0] +
	             direction[1]*helper[1] +
	             direction[2]*helper[2];
	pitch = acos(dotproduct);

	/* To the same with projected vector and vector in x-direction */
	direction[0] = 1.0f;
	direction[1] = 0;
	direction[2] = 0;
	dotproduct = direction[0]*helper[0] +
	             direction[1]*helper[1] +
	             direction[2]*helper[2];
	yaw = acos(dotproduct);

	ArcLength = dist;
	Pitch = -pitch;
	Yaw = yaw;
	CalcLookVector();
}

void CCamera::TurnLeftRight(GLfloat ang)
{
	Yaw += ang;
	CalcLookVector();
}

void CCamera::TurnUpDown(GLfloat ang)
{
	if (!(Pitch + ang < M_PI*3/2 && Pitch + ang > M_PI/2)) {
		Pitch += ang;
		CalcLookVector();
	}
}

void CCamera::MoveForwardBackward(GLfloat dist)
{
	Position[0] += LookVector[0]*dist;
	Position[1] += LookVector[1]*dist;
	Position[2] += LookVector[2]*dist;
}

void CCamera::MoveUpDown(GLfloat dist)
{
	Position[2] += dist;
}

void CCamera::PanLeftRight(GLfloat dist)
{
	GLfloat direction[3];
	GLfloat up[] = { 0.0f, 0.0f, 1.0f };

	direction[0] = up[1]*LookVector[2] - up[2]*LookVector[1];
	direction[1] = up[2]*LookVector[0] - up[0]*LookVector[2];
	direction[2] = up[0]*LookVector[1] - up[1]*LookVector[0];

	Position[0] += direction[0]*dist;
	Position[1] += direction[1]*dist;
	Position[2] += direction[2]*dist;
}

void CCamera::PanUpDown(GLfloat dist)
{
	GLfloat v2[3];
	GLfloat direction[3];
	GLfloat up[] = { 0.0f, 0.0f, 1.0f };

	/* Calculate help vector */
	v2[0] = up[1]*LookVector[2] - up[2]*LookVector[1];
	v2[1] = up[2]*LookVector[0] - up[0]*LookVector[2];
	v2[2] = up[0]*LookVector[1] - up[1]*LookVector[0];

	direction[0] = v2[1]*LookVector[2] - v2[2]*LookVector[1];
	direction[1] = v2[2]*LookVector[0] - v2[0]*LookVector[2];
	direction[2] = v2[0]*LookVector[1] - v2[1]*LookVector[0];

	Position[0] += direction[0]*dist;
	Position[1] += direction[1]*dist;
	Position[2] += direction[2]*dist;
}

GLfloat CCamera::GetFOV() { return FOV; }

void CCamera::ChangeFOV(GLfloat ang) { FOV += ang; }

void CCamera::UpdateFrustum(void)
{
	GLfloat modl[16];
	GLfloat proj[16];
	GLfloat clip[16];
	GLfloat d;

	glGetFloatv(GL_PROJECTION_MATRIX, proj);
	glGetFloatv(GL_MODELVIEW_MATRIX, modl);

	clip[0] = modl[0] * proj[0];
	clip[1] = modl[1] * proj[5];
	clip[2] = modl[2] * proj[10] + modl[3] * proj[14];
	clip[3] = modl[2] * proj[11];
	clip[4] = modl[4] * proj[0];
	clip[5] = modl[5] * proj[5];
	clip[6] = modl[6] * proj[10] + modl[7] * proj[14];
	clip[7] = modl[6] * proj[11];
	clip[8] = modl[8] * proj[0];
	clip[9] = modl[9] * proj[5];
	clip[10] = modl[10] * proj[10] + modl[11] * proj[14];
	clip[11] = modl[10] * proj[11];
	clip[12] = modl[12] * proj[0];
	clip[13] = modl[13] * proj[5];
	clip[14] = modl[14] * proj[10] + modl[15] * proj[14];
	clip[15] = modl[14] * proj[11];

	/* Right plane */
	Frustum[0][0] = clip[3] - clip[0];
	Frustum[0][1] = clip[7] - clip[4];
	Frustum[0][2] = clip[11] - clip[8];
	Frustum[0][3] = clip[15] - clip[12];

	/* Left plane */
	Frustum[1][0] = clip[3] + clip[0];
	Frustum[1][1] = clip[7] + clip[4];
	Frustum[1][2] = clip[11] + clip[8];
	Frustum[1][3] = clip[15] + clip[12];

	/* Bottom plane */
	Frustum[2][0] = clip[3] + clip[1];
	Frustum[2][1] = clip[7] + clip[5];
	Frustum[2][2] = clip[11] + clip[9];
	Frustum[2][3] = clip[15] + clip[13];

	/* Top plane */
	Frustum[3][0] = clip[3] - clip[1];
	Frustum[3][1] = clip[7] - clip[5];
	Frustum[3][2] = clip[11] - clip[9];
	Frustum[3][3] = clip[15] - clip[13];

	/* Far plane */
	Frustum[4][0] = clip[3] + clip[2];
	Frustum[4][1] = clip[7] + clip[6];
	Frustum[4][2] = clip[11] + clip[10];
	Frustum[4][3] = clip[15] + clip[14];

	/* Near plane */
	Frustum[4][0] = clip[3] - clip[2];
	Frustum[4][1] = clip[7] - clip[6];
	Frustum[4][2] = clip[11] - clip[10];
	Frustum[4][3] = clip[15] - clip[14];

	/* Normalize everything */
	d = sqrt(Frustum[0][0] * Frustum[0][0] + Frustum[0][1] * Frustum[0][1] +
	         Frustum[0][2] * Frustum[0][2]);
	Frustum[0][0] /= d;
	Frustum[0][1] /= d;
	Frustum[0][2] /= d;
	Frustum[0][3] /= d;

	d = sqrt(Frustum[1][0] * Frustum[1][0] + Frustum[1][1] * Frustum[1][1] +
	         Frustum[1][2] * Frustum[1][2]);
	Frustum[1][0] /= d;
	Frustum[1][1] /= d;
	Frustum[1][2] /= d;
	Frustum[1][3] /= d;

	d = sqrt(Frustum[2][0] * Frustum[2][0] + Frustum[2][1] * Frustum[2][1] +
	         Frustum[2][2] * Frustum[2][2]);
	Frustum[2][0] /= d;
	Frustum[2][1] /= d;
	Frustum[2][2] /= d;
	Frustum[2][3] /= d;

	d = sqrt(Frustum[3][0] * Frustum[3][0] + Frustum[3][1] * Frustum[3][1] +
	         Frustum[3][2] * Frustum[3][2]);
	Frustum[3][0] /= d;
	Frustum[3][1] /= d;
	Frustum[3][2] /= d;
	Frustum[3][3] /= d;

	d = sqrt(Frustum[4][0] * Frustum[4][0] + Frustum[4][1] * Frustum[4][1] +
	         Frustum[4][2] * Frustum[4][2]);
	Frustum[4][0] /= d;
	Frustum[4][1] /= d;
	Frustum[4][2] /= d;
	Frustum[4][3] /= d;

	d = sqrt(Frustum[5][0] * Frustum[5][0] + Frustum[5][1] * Frustum[5][1] +
	         Frustum[5][2] * Frustum[5][2]);
	Frustum[5][0] /= d;
	Frustum[5][1] /= d;
	Frustum[5][2] /= d;
	Frustum[5][3] /= d;
}

bool CCamera::isPointInFrustum(const GLfloat *p)
{
	int i;

	for (i = 0; i < 6; i++) {
		if (Frustum[i][0] * p[0] + Frustum[i][1] * p[1] +
		    Frustum[i][2] * p[2] + Frustum[i][3] <= 0)
			return false;
	}
	return true;
}

bool CCamera::isSphereInFrustum(const GLfloat *p, const GLfloat r)
{
	int i;

	for (i = 0; i < 6; i++) {
		if (Frustum[i][0] * p[0] + Frustum[i][1] * p[1] +
		    Frustum[i][2] * p[2] + Frustum[i][3] <= -r)
			return false;
	}
	return true;
}

bool CCamera::isTranslatedPointInFrustum(const GLfloat *p)
{
	GLfloat q[3];

	q[0] = p[0] + Position[0];
	q[1] = p[1] + Position[1];
	q[2] = p[2] + Position[2];
	return isPointInFrustum(q);
}

bool CCamera::isTranslatedSphereInFrustum(const GLfloat *p, const GLfloat r)
{
	GLfloat q[3];

	q[0] = p[0] + Position[0];
	q[1] = p[1] + Position[1];
	q[2] = p[2] + Position[2];
	return isSphereInFrustum(q, r);
}

void CCamera::Look()
{
	LookLeftEye();
}

void CCamera::LookLeftEye()
{
	gluLookAt(Position[0], Position[1], Position[2],
		  Position[0]+LookVector[0], Position[1]+LookVector[1],
		  Position[2]+LookVector[2],
		  Up[0], Up[1], Up[2]);
	UpdateFrustum();
}

void CCamera::LookRightEye()
{
	GLfloat pos[3];
	GLfloat direction[3];

	direction[0] = Up[1]*LookVector[2] - Up[2]*LookVector[1];
	direction[1] = Up[2]*LookVector[0] - Up[0]*LookVector[2];
	direction[2] = Up[0]*LookVector[1] - Up[1]*LookVector[0];

	pos[0] = Position[0] - direction[0]*EyeDist;
	pos[1] = Position[1] - direction[1]*EyeDist;
	pos[2] = Position[2] - direction[2]*EyeDist;

	gluLookAt(pos[0], pos[1], pos[2],
		  pos[0]+LookVector[0], pos[1]+LookVector[1],
		  pos[2]+LookVector[2],
	          Up[0], Up[1], Up[2]);
	UpdateFrustum();
}


float CCamera::DistanceTo(float *vector)
{
	float a[3];
	a[0] = vector[0] - Position[0];
	a[1] = vector[1] - Position[1];
	a[2] = vector[2] - Position[2];
	return sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
}

void CCamera::ReadSavedPositions()
{
	int i;
	char Buffer[512];
	FILE *file;
	char *camposfile;

	camposfile = (char *) malloc(strlen(progdir) +
	                             strlen("/campositions.conf"));
	strcpy(camposfile, progdir);
	strcat(camposfile, "/campositions.conf");

	file = efopen(camposfile, "r");
	for (i = 0; i < 12; i++) {
		if (ReadConfigLine(file, Buffer) == NULL)
			return;
		sscanf(Buffer, "%f %f %f %f %f %f",&CamPositions[i][0], 
	               &CamPositions[i][1], &CamPositions[i][2],
	               &CamPositions[i][3], &CamPositions[i][4], &FOV);
	}
	fclose(file);
}

void CCamera::SavePosition(int PosNumber)
{
	int i;
	FILE *file;
	char *camposfile;

	camposfile = (char *) malloc(strlen(progdir) +
	                             strlen("/campositions.conf"));
	strcpy(camposfile, progdir);
	strcat(camposfile, "/campositions.conf");

	file = efopen(camposfile, "w");
	CamPositions[PosNumber][0] = Position[0];
	CamPositions[PosNumber][1] = Position[1];
	CamPositions[PosNumber][2] = Position[2];
	CamPositions[PosNumber][3] = Yaw;
	CamPositions[PosNumber][4] = Pitch;
	for (i = 0; i < 12; i++) {
		fprintf(file, "%f %f %f %f %f %f\n", CamPositions[i][0], 
			CamPositions[i][1], CamPositions[i][2],
			CamPositions[i][3], CamPositions[i][4], FOV);
	}
}

void CCamera::LoadPosition(int PosNumber)
{
	Position[0] = CamPositions[PosNumber][0];
	Position[1] = CamPositions[PosNumber][1];
	Position[2] = CamPositions[PosNumber][2];
	Yaw = CamPositions[PosNumber][3];
	Pitch = CamPositions[PosNumber][4];
	CalcLookVector();
}

void CCamera::GetPosition(GLfloat *x, GLfloat *y, GLfloat *z)
{
	*x = Position[0];
	*y = Position[1];
	*z = Position[2];
}

GLfloat	CCamera::GetDistanceToTarget()
{
	return ArcLength;
}

void CCamera::SetDistanceToTarget(GLfloat dist)
{
	ArcLength = dist;
}

void CCamera::IncreaseEyeDist(GLfloat dist) { EyeDist += dist; }

void CCamera::DecreaseEyeDist(GLfloat dist) { EyeDist -= dist; }

void CCamera::DumpPos()
{
	cout << "Camera: " << Position[0] << " " << Position[1]
	     << " " << Position[2] << endl;
	cout << "Look Vector: " << LookVector[0] << " " << LookVector[1]
	     << " " << LookVector[2] << endl;
	cout << "Target: " << Position[0] + LookVector[0]*ArcLength
	     << " " << Position[1] + LookVector[1]*ArcLength
	     << " " << Position[2] + LookVector[2]*ArcLength << endl;
	cout << "Pitch and Yaw: " << Pitch << " " << Yaw << endl;
	cout << "FOV and TargetDist: " <<  FOV << " " << ArcLength << endl;
}

CCamera::CCamera()
{
	Position[0] = 0;
	Position[1] = 0;
	Position[2] = 0;
	LookVector[0] = 1;
	LookVector[1] = 0;
	LookVector[2] = 0;
	Yaw = 0;
	Pitch = 0;
	memset(CamPositions, 0, 5*12*sizeof(GLfloat));
	FOV = 45.0;
	EyeDist = 1;
	ArcLength = 10.0f;
}

CCamera::~CCamera()
{
}