//ODECamera.cpp

#include "ODECameraPC.h"

#define CAMERA_COLLIDE_BITS 10
#define RAY_COLLIDE_BITS 20
#define WATER_LEVEL 0

ODECamera::ODECamera(ODEWorld* world, dReal posx, dReal posy, dReal posz, dReal height)
: ODEObject(true, false, "", "", posx, posy, posz)
{
	radius = 0.7 * height;
	pitch = 0;
	yaw = 0;
	dampingFactor = 40;
	//cameraSpeedScalar = 8;
	cameraSpeedScalar = 6;
	maxCameraSpeed = 15;
	jumpingPowerScalar = 10;
	underwaterfog = false;
	viewOffsetFromCenter = 2.0;

	//ray starts at center of sphere; it's max length is 
	//setup to intersect slopes shallower than 45 degree angles 
	maxImportantRayIntersectionLength = radius*1.4; 

	upVector.x = 0;
	upVector.y = 1;
	upVector.z = 0;

	//don't initialize these vectors; this will happen later
	//lookAt.x = 0;
	//lookAt.y = 0;
	//lookAt.z = -1;
	//forwardVector.x = 0;
	//forwardVector.y = 0;
	//forwardVector.z = -1;

	// ODE stuff
	thisBodyID = dBodyCreate(world->GetWorldID());
	thisGeomID = dCreateSphere(world->GetSpaceID(), radius);
	dGeomSetCollideBits (thisGeomID, CAMERA_COLLIDE_BITS);
	//thisGeomID = dCreateBox(0, radius*2, radius*2, radius*2);
	dGeomSetBody(thisGeomID, thisBodyID);

	////resetting mass for camera only
	//dReal newMass = 1.0;
	//dMass mass;
	//dBodyGetMass(thisBodyID, &mass);
	//dMassAdjust(&mass, newMass);
	//dBodySetMass(thisBodyID, &mass);

	point3d startPoint = {posx, posy, posz};
	SetPosition(startPoint);

	//setup altitude ray
	altitudeRayID = dCreateRay(world->GetSpaceID(), 2*radius);
	//dSpaceAdd(world->GetSpaceID(), altitudeRayID);
	dGeomSetCollideBits (altitudeRayID, RAY_COLLIDE_BITS);

	theWorld = world;
}

ODECamera::~ODECamera()
{
	dBodyDestroy(thisBodyID);
	dGeomDestroy(thisGeomID);
}

bool ODECamera::Update(dReal deltaTime)
{
	if (position.y > WATER_LEVEL) //above water
	{
		//only dampen movement if touching ground
		if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
			&& theWorld->GetRayIntersectionDepth() != -1)
		{
			ZeroXZForces();
			DampenXZVelocity(deltaTime);
		}
	}
	else
	{
		WaterDamping(deltaTime);
		dBodyAddForce(thisBodyID, 0, -9.81*7, 0);
	}

	//std::cout << "depth = " << theWorld->GetRayIntersectionDepth() << std::endl;

	//update ray position/orientation
	dVector3 downVector = {0, -1, 0};
	const dReal* rayPosition = dBodyGetPosition(thisBodyID);
	dGeomRaySet(altitudeRayID, rayPosition[0], rayPosition[1],
		rayPosition[2], downVector[0], downVector[1], downVector[2]);

	ODEObject::Update(deltaTime);

	return true;
}

void ODECamera::PlaceCamera()
{
	//we don't want this body to be disabled
	dBodyEnable(thisBodyID);

	// getting position
	const dReal* bodyPosition;
	bodyPosition = dBodyGetPosition(thisBodyID);

	if (yaw >= 360 || yaw <= -360)
	{
		yaw = 0;
	}

	//setting pitch limits
	if (pitch > 90)
	{
		pitch = 90;
	}

	if (pitch < -90)
	{
		pitch = -90;
	}

	dReal cosYaw = (dReal)cos(DEG2RAD(yaw));
	dReal sinYaw = (dReal)sin(DEG2RAD(yaw));
	dReal sinPitch = (dReal)sin(DEG2RAD(pitch));

	//calculate new lookAt vector
	lookAt.x = dReal(bodyPosition[0] + cosYaw);
	lookAt.y = dReal(bodyPosition[1] + sinPitch);
	lookAt.z = dReal(bodyPosition[2] + sinYaw);

	//setting forward vector
	forwardVector.x = lookAt.x - bodyPosition[0];
	forwardVector.y = lookAt.y - bodyPosition[1];
	forwardVector.z = lookAt.z - bodyPosition[2];

	//setting new camera position and orientation
	gluLookAt(bodyPosition[0], bodyPosition[1] + viewOffsetFromCenter, bodyPosition[2], 
		lookAt.x, lookAt.y + viewOffsetFromCenter, lookAt.z, upVector.x, upVector.y, upVector.z);
}

void ODECamera::ZeroXZForces()
{
	const dReal* force = dBodyGetForce(thisBodyID);
	dBodySetForce(thisBodyID, 0, force[1], 0);
}

void ODECamera::DampenXZVelocity(dReal dtime)
{
	const dReal* velocity = dBodyGetLinearVel(thisBodyID);

	dReal vx = velocity[0];
	dReal vz = velocity[2];

	if (vx > 0)
	{
		vx -= dtime*dampingFactor;
		
		if (vx < 0)
		{
			vx = 0;
		}
	}
	else
	{
		vx += dtime*dampingFactor;

		if (vx > 0)
		{
			vx = 0;
		}
	}

	if (vz > 0)
	{
		vz -= dtime*dampingFactor;
		
		if (vz < 0)
		{
			vz = 0;
		}
	}
	else
	{
		vz += dtime*dampingFactor;

		if (vz > 0)
		{
			vz = 0;
		}
	}

	dBodySetLinearVel(thisBodyID, vx, velocity[1], vz);
}

void ODECamera::WaterDamping(dReal dtime)
{
	const dReal* velocity = dBodyGetLinearVel(thisBodyID);

	dReal vx = velocity[0];
	dReal vy = velocity[1];
	dReal vz = velocity[2];

	if (vx > 0)
	{
		vx -= dtime*dampingFactor;
		
		if (vx < 0)
		{
			vx = 0;
		}
	}
	else
	{
		vx += dtime*dampingFactor;

		if (vx > 0)
		{
			vx = 0;
		}
	}

	if (vy > 0) //moving up underwater
	{
		//make damping factor scale with velocity
		dReal scaledDampingFactor = dampingFactor * (vy/10);
		vy -= dtime*scaledDampingFactor;
		
		if (vy < 0)
		{
			vy = 0;
		}
	}
	else //falling down underwater
	{
		//make damping factor scale with velocity
		dReal scaledDampingFactor = dampingFactor * (-vy/10);
		vy += dtime*scaledDampingFactor;

		if (vy > 0)
		{
			vy = 0;
		}
	}

	if (vz > 0) 
	{
		vz -= dtime*dampingFactor;
		
		if (vz < 0)
		{
			vz = 0;
		}
	}
	else
	{
		vz += dtime*dampingFactor;

		if (vz > 0)
		{
			vz = 0;
		}
	}

	dBodySetLinearVel(thisBodyID, vx, vy, vz);
}

void ODECamera::AddRotation(dReal xAxis, dReal yAxis)
{
	pitch += xAxis;
	yaw -= yAxis;
	//dBodySetAngularVel(thisBodyID, xRotation, yRotation, 0);
}

void ODECamera::MoveForward()
{
	//can only move if touching ground or underwater
	if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
		&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	{
		const dReal* velocity = dBodyGetLinearVel(thisBodyID);
		dReal vx = velocity[0];
		dReal vy = velocity[1];
		dReal vz = velocity[2];

		if (abs(velocity[0]) < maxCameraSpeed)
		{
			vx += forwardVector.x*cameraSpeedScalar;
		}

		if (abs(velocity[1]) < maxCameraSpeed)
		{
			vy += forwardVector.y*cameraSpeedScalar;
		}

		if (abs(velocity[2]) < maxCameraSpeed)
		{
			vz += forwardVector.z*cameraSpeedScalar;
		}

		SetLinearVelocity(vx, vy, vz);
	}

	////can only move if touching ground or underwater
	//if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
	//	&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	//{
	//	SetLinearVelocity(forwardVector.x*cameraSpeedScalar, forwardVector.y*cameraSpeedScalar, 
	//		forwardVector.z*cameraSpeedScalar);
	//}
}

void ODECamera::MoveBackward()
{
	//can only move if touching ground or underwater
	if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
		&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	{
		const dReal* velocity = dBodyGetLinearVel(thisBodyID);
		dReal vx = velocity[0];
		dReal vy = velocity[1];
		dReal vz = velocity[2];

		if (abs(velocity[0]) < maxCameraSpeed)
		{
			vx -= forwardVector.x*cameraSpeedScalar;
		}

		if (abs(velocity[1]) < maxCameraSpeed)
		{
			vy -= forwardVector.y*cameraSpeedScalar;
		}

		if (abs(velocity[2]) < maxCameraSpeed)
		{
			vz -= forwardVector.z*cameraSpeedScalar;
		}

		SetLinearVelocity(vx, vy, vz);
	}

	////can only move if touching ground or underwater
	//if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
	//	&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	//{
	//	SetLinearVelocity(-forwardVector.x*cameraSpeedScalar, -forwardVector.y*cameraSpeedScalar, 
	//		-forwardVector.z*cameraSpeedScalar);
	//}
}

void ODECamera::MoveLeft()
{
	//can only move if touching ground or underwater
	if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
		&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	{
		const dReal* velocity = dBodyGetLinearVel(thisBodyID);
		dReal vx = velocity[0];
		dReal vy = velocity[1];
		dReal vz = velocity[2];

		vector3d leftVector;
		leftVector = VectorCrossProduct(upVector, forwardVector);

		if (abs(velocity[0]) < maxCameraSpeed)
		{
			vx += leftVector.x*cameraSpeedScalar;
		}

		if (abs(velocity[1]) < maxCameraSpeed)
		{
			vy += leftVector.y*cameraSpeedScalar;
		}

		if (abs(velocity[2]) < maxCameraSpeed)
		{
			vz += leftVector.z*cameraSpeedScalar;
		}

		SetLinearVelocity(vx, vy, vz);
	}

	////can only move if touching ground or underwater
	//if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
	//	&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	//{
	//	vector3d leftVector;
	//	leftVector = VectorCrossProduct(upVector, forwardVector);
	//	SetLinearVelocity(leftVector.x*cameraSpeedScalar, leftVector.y*cameraSpeedScalar, 
	//		leftVector.z*cameraSpeedScalar);
	//}
}

void ODECamera::MoveRight()
{
	//can only move if touching ground or underwater
	if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
		&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	{
		const dReal* velocity = dBodyGetLinearVel(thisBodyID);
		dReal vx = velocity[0];
		dReal vy = velocity[1];
		dReal vz = velocity[2];

		vector3d rightVector;
		rightVector = VectorCrossProduct(forwardVector, upVector);

		if (abs(velocity[0]) < maxCameraSpeed)
		{
			vx += rightVector.x*cameraSpeedScalar;
		}

		if (abs(velocity[1]) < maxCameraSpeed)
		{
			vy += rightVector.y*cameraSpeedScalar;
		}

		if (abs(velocity[2]) < maxCameraSpeed)
		{
			vz += rightVector.z*cameraSpeedScalar;
		}

		SetLinearVelocity(vx, vy, vz);
	}

	////can only move if touching ground or underwater
	//if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
	//	&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	//{
	//	vector3d rightVector;
	//	rightVector = VectorCrossProduct(forwardVector, upVector);
	//	SetLinearVelocity(rightVector.x*cameraSpeedScalar, rightVector.y*cameraSpeedScalar, 
	//		rightVector.z*cameraSpeedScalar);
	//}
}

void ODECamera::Jump()
{
	//can only move if touching ground or underwater
	if (theWorld->GetRayIntersectionDepth() < maxImportantRayIntersectionLength
		&& theWorld->GetRayIntersectionDepth() != -1 || position.y < WATER_LEVEL)
	{
		const dReal* velocity = dBodyGetLinearVel(thisBodyID);
		dReal vx = velocity[0];
		dReal vy = velocity[1];
		dReal vz = velocity[2];

		if (abs(velocity[1]) < maxCameraSpeed)
		{
			vy += upVector.y*jumpingPowerScalar;
		}

		SetLinearVelocity(vx, vy, vz);
	}
}

void ODECamera::SetLinearVelocity(dReal x, dReal y, dReal z)
{
	dBodySetLinearVel(thisBodyID, x, y, z);

	//const dReal* velocity = dBodyGetLinearVel(thisBodyID);
	//dReal vx = velocity[0] + x;
	//dReal vy = velocity[1] + y;
	//dReal vz = velocity[2] + z;
	//dBodySetLinearVel(thisBodyID, vx, vy, vz);
}

void ODECamera::Draw()
{
	if (position.y + viewOffsetFromCenter > WATER_LEVEL) //above water
	{
		if (underwaterfog == true)
		{
			//reset normal fog
			float fogcolor[4] = {.7, .7, .6, 1};
			glFogfv(GL_FOG_COLOR, fogcolor);
			//glFogi(GL_FOG_DENSITY, 1);
			glFogi(GL_FOG_START, 0);
			glFogi(GL_FOG_END, 600);
			glFogi(GL_FOG_MODE, GL_LINEAR);
			glEnable(GL_FOG);
			underwaterfog = false;
		}
	}
	else
	{
		if (underwaterfog == false)
		{
			//"underwater fog"
			float fogcolor[4] = {0, .2, .4, 1};
			glFogfv(GL_FOG_COLOR, fogcolor);
			//glFogi(GL_FOG_DENSITY, 1);
			glFogi(GL_FOG_START, 0);
			glFogi(GL_FOG_END, 200);
			glFogi(GL_FOG_MODE, GL_LINEAR);
			glEnable(GL_FOG);
			underwaterfog = true;
		}
	}
}

//dReal ODECamera::GetHeight()
//{
//	return height;
//}

//double ODECamera::GetRadius()
//{
//	return radius;
//}

vector3d ODECamera::GetForwardVector()
{
	return forwardVector;
}

vector3d ODECamera::UnifyVector(vector3d vec)
{
	dReal xSquared = vec.x*vec.x;
	dReal ySquared = vec.y*vec.y;
	dReal zSquared = vec.z*vec.z;
	dReal length = sqrt(xSquared + ySquared + zSquared);
	vector3d unitvec = {vec.x/length, vec.y/length, vec.z/length};
	return unitvec;
}

vector3d ODECamera::VectorCrossProduct(vector3d vec1, vector3d vec2)
{
	vec1 = UnifyVector(vec1);
	vec2 = UnifyVector(vec2);
	vector3d newVec;

	newVec.x = vec1.y*vec2.z - vec1.z*vec2.y;
	newVec.y = vec1.z*vec2.x - vec1.x*vec2.z;
	newVec.z = vec1.x*vec2.y - vec1.y*vec2.x;

	newVec = UnifyVector(newVec);

	return newVec;
}
