//ODESchoolOfFish.cpp

#include "ODESchoolOfFish.h"

ODESchoolOfFish::ODESchoolOfFish(ODEWorld* world, dReal posx, dReal posy, dReal posz, int numFish, 
								 std::list<Base3DObject*>* objectList)
: ODEObject(false, false, "", "", posx, posy, posz)
{
	numberOfFish = numFish;
	fishThinkingPeriod = 0.1;
	ODEBox* fishPtr=NULL;

	for (int i=0; i<numFish; i++)
	{
		fishPtr = new ODEBox(world, true, true, FISH_MODELS_DIR, FISH_FILENAME, posx, posy, 
			posz - FISH_LENGTH*2*i, 0, 0, 0);
		fishList.push_back(fishPtr);
		objectList->push_back(fishPtr);
	}

	//testing vec to angle functions
	vector3d vector = {1, 0, 1};
	vector3d angles;
	VectorToAngles(vector, angles);

	//std::cout << "vector = <" << vector.x << ", " << vector.y << ", " << vector.z << ">" << std::endl;
	//std::cout << "Euler angles = <" << angles.x << ", " << angles.y << ", " << angles.z << ">" << std::endl;
}

ODESchoolOfFish::~ODESchoolOfFish()
{
	//This now gets done through the global list.

	//// Clean up...
	//while (!fishList.empty())
	//{
	//	delete fishList.back();
	//	fishList.pop_back();
	//}
}

bool ODESchoolOfFish::Update(dReal deltaTime)
{
	//fishThinkingPeriod -= deltaTime;

	//for each fish, if the fish is underwater, dampen velocity, then 
	//apply boids rules (http://www.red3d.com/cwr/boids/)

	//make sure to avoid autodisabling
	std::list<ODEBox*>::iterator fishIter;
	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	{
		if (!dBodyIsEnabled((*fishIter)->GetBodyID()))
		{
			dBodyEnable((*fishIter)->GetBodyID());
			vector3d vel = (*fishIter)->GetVelocity();
			vel.y += 0.5;
			dBodySetLinearVel((*fishIter)->GetBodyID(), vel.x, vel.y, vel.z);
		}
	}
	
	//if (fishThinkingPeriod <= 0)
	//{
	//	//calculate average heading/position once per period
	//	UpdateAverageHeading();
	//	UpdateAveragePosition();
	//}


	//calculate average heading/position once per period
	UpdateAverageHeading();
	UpdateAveragePosition();

	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	{
		//if (fishThinkingPeriod <= 0 && (*fishIter)->GetPosition().y < WATER_LEVEL)
		if ((*fishIter)->GetPosition().y < WATER_LEVEL)
		{
			//This now gets done through the global list.
			//(*fishIter)->DampenVelocity(UNDERWATER_DAMPING_FACTOR);

			//only let fish add forces if they are under the max speed
			if (abs((*fishIter)->GetVelocity().x) < MAX_FISH_SPEED &&
				abs((*fishIter)->GetVelocity().y) < MAX_FISH_SPEED &&
				abs((*fishIter)->GetVelocity().z) < MAX_FISH_SPEED)
			{
				UseBoidsRules(*fishIter);

				//add some more upward force
				(*fishIter)->AddForce(0, SWIMMING_STRENGTH*4, 0);
			}

			//Now add torque to make fish face the direction they are moving

			//Get vector that represents heading.
			const dReal* forces = dBodyGetForce((*fishIter)->GetBodyID());
			vector3d forceVector = {forces[0], forces[1], forces[2]};

			//Get vector that represents the direction this fish is facing.
			dQuaternion Q;
			const dReal* qValues = dBodyGetQuaternion((*fishIter)->GetBodyID());
			Q[0] = qValues[0];
			Q[1] = qValues[1];
			Q[2] = qValues[2];
			Q[3] = qValues[3];
			dMatrix3 R;
			dQtoR(Q, R);
			dVector3 nonRotatedVector = {0.0, 0.0, -1.0}; //might need to be changed
			dVector3 tempFacingVector;
			dMULTIPLY1_331(tempFacingVector, R, nonRotatedVector);
			vector3d facingVector = {tempFacingVector[0], tempFacingVector[1], tempFacingVector[2]};

			//Add torque that is proportional to facingVector x forceVector.
			//The following function automatically unifies each vector.
			vector3d torqueVector = VectorCrossProduct(facingVector, forceVector);
			torqueVector.x /= 20;
			torqueVector.y /= 20;
			torqueVector.z /= 20;
			dBodyAddTorque((*fishIter)->GetBodyID(), torqueVector.x, torqueVector.y, torqueVector.z);

			////Get vector that represents heading.
			//const dReal* forces = dBodyGetForce((*fishIter)->GetBodyID());
			//vector3d forceVector = {forces[0], forces[1], forces[2]};

			////Set matrix to be aligned with heading.
			//dMatrix3 R;
			//vector3d EulerAngles;
			//VectorToAngles(forceVector, EulerAngles);
			//dRFromEulerAngles(R, EulerAngles.x, EulerAngles.y, EulerAngles.z);
			////dBodySetRotation((*fishIter)->GetBodyID(), R);
			//dQuaternion Q;
			//dRtoQ(R, Q);
			//dBodySetQuaternion((*fishIter)->GetBodyID(), Q);

			////set orientation to make fish face the direction they are moving
			//vector3d forwardVector = (*fishIter)->GetVelocity();
			//dMatrix3 R;
			//dRFromEulerAngles(R, forwardVector.x, forwardVector.y, forwardVector.z);
			//dQuaternion Q;
			//dRtoQ (R, Q);
			//dBodySetQuaternion((*fishIter)->GetBodyID(), Q);
		}
		else //fish is above water
		{
			dBodyAddRelTorque((*fishIter)->GetBodyID(), 0, 30, 0);
			//dBodyAddTorque((*fishIter)->GetBodyID(), 0, 0, 15);
		}
	}

	//if (fishThinkingPeriod <= 0)
	//{
	//	fishThinkingPeriod = 0.1;
	//}

	ODEObject::Update(deltaTime);

	return true;
}

void ODESchoolOfFish::Draw()
{
	//This now gets done through the global list.

	//std::list<ODEBox*>::iterator fishIter;
	//for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	//{
	//	(*fishIter)->Draw();
	//}
}

void ODESchoolOfFish::MakeDisplayList()
{
	//This now gets done through the global list.

	//std::list<ODEBox*>::iterator fishIter;
	//for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	//{
	//	(*fishIter)->MakeDisplayList();
	//}
}

void ODESchoolOfFish::UseBoidsRules(ODEBox* thisFish)
{
	//Pre-conditions: averageHeading and averagePosition must have just been
	//updated during this frame

	point3d thisFishPosition = thisFish->GetPosition();

	//Separation
	std::list<ODEBox*>::iterator fishIter;
	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	{
		if ((*fishIter) != thisFish)
		{
			point3d thatFishPosition = (*fishIter)->GetPosition();
			dReal distance = Distance3D(thisFishPosition, thatFishPosition);
			if (distance < SEPARATION_DISTANCE)
			{
				vector3d moveAwayVector = {thisFishPosition.x - thatFishPosition.x, 
					thisFishPosition.y - thatFishPosition.y, thisFishPosition.z - thatFishPosition.z};

				//moveAwayVector = UnifyVector(moveAwayVector);
				moveAwayVector.x *= (SWIMMING_STRENGTH*5/distance);
				moveAwayVector.y *= (SWIMMING_STRENGTH*5/distance);
				moveAwayVector.z *= (SWIMMING_STRENGTH*5/distance);

				thisFish->AddForce(moveAwayVector.x, moveAwayVector.y, moveAwayVector.z);
			}
		}
	}

	//Alignment
	thisFish->AddForce(averageHeading.x*SWIMMING_STRENGTH, averageHeading.y*SWIMMING_STRENGTH, 
		averageHeading.z*SWIMMING_STRENGTH);

	//Cohesion
	vector3d moveCloserVector = {averagePosition.x - thisFishPosition.x, 
		averagePosition.y - thisFishPosition.y, averagePosition.z - thisFishPosition.z};

	thisFish->AddForce(moveCloserVector.x*2*SWIMMING_STRENGTH, moveCloserVector.y*2*SWIMMING_STRENGTH, 
		moveCloserVector.z*2*SWIMMING_STRENGTH);
}

void ODESchoolOfFish::UpdateAverageHeading()
{
	averageHeading.x = 0;
	averageHeading.y = 0;
	averageHeading.z = 0;

	std::list<ODEBox*>::iterator fishIter;
	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	{
		vector3d thisHeading = (*fishIter)->GetVelocity();
		//vector3d thisHeading = UnifyVector((*fishIter)->GetVelocity());

		averageHeading.x += thisHeading.x;
		averageHeading.y += thisHeading.y;
		averageHeading.z += thisHeading.z;
	}

	averageHeading.x /= numberOfFish;
	averageHeading.y /= numberOfFish;
	averageHeading.z /= numberOfFish;

	//averageHeading = UnifyVector(averageHeading);
}

void ODESchoolOfFish::UpdateAveragePosition()
{
	averagePosition.x = 0;
	averagePosition.y = 0;
	averagePosition.z = 0;

	std::list<ODEBox*>::iterator fishIter;
	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
	{
		point3d thisPosition = (*fishIter)->GetPosition();

		averagePosition.x += thisPosition.x;
		averagePosition.y += thisPosition.y;
		averagePosition.z += thisPosition.z;
	}

	averagePosition.x /= numberOfFish;
	averagePosition.y /= numberOfFish;
	averagePosition.z /= numberOfFish;
}

vector3d ODESchoolOfFish::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 ODESchoolOfFish::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;
}

void ODESchoolOfFish::VectorToAngles(vector3d a, vector3d &theta)
{
	float X=a.x;
	float Y=a.y;
	float Z=a.z;

	////Set Z angle
	//if(X==0)
	//{
	//	theta.z=PI/2*sign(Y);//if x is 0 then figure theta from sign of Y
	//}
	//else
	//{
	//	theta.z=(float)atan(Y/X);//else make arc tangent
	//}
	//if(X< 0)
	//{
	//	theta.z+=PI;// if x is negative, add 180 deg.
	//}

	////Set Y angle
	//if(X==0)
	//{
	//	theta.y=PI/2*sign(Z);
	//}
	//else	
	//{
	//	theta.y=(float)atan(Z/X);
	//}
	//if(X< 0)
	//{
	//	theta.y+=PI;
	//}

	////Set X angle
	//if(Z==0)
	//{
	//	theta.x=PI/2*sign(Y);
	//}
	//else	
	//{
	//	theta.x=(float)atan(Y/Z);
	//}
	//if(Z< 0)
	//{
	//	theta.x+=PI;
	//}
	
	//New, fixed version...

	//Set Z angle
	if(X==0)
	{
		theta.z=PI/2*sign(Y);//if x is 0 then figure theta from sign of Y
	}
	else
	{
		theta.z=(float)atan(Y/X);//else make arc tangent
	}
	if(X< 0)
	{
		theta.z+=PI;
	}

	//Set Y angle
	if(Z==0)
	{
		theta.y=PI/2*sign(Z);
	}
	else	
	{
		theta.y=(float)atan(X/Z);
	}
	if(Z< 0)
	{
		theta.y+=PI;
	}

	//Set X angle
	if(Y==0)
	{
		theta.x=PI/2*sign(Y);
	}
	else	
	{
		theta.x=(float)atan(Z/Y);
	}
	if(Y< 0)
	{
		theta.x+=PI;
	}

	/*
	if(a[2]>=0&&a[1]>=0)theta[0]=(float)acos(a[2]);
	if(a[0]>=0&&a[1]>=0)theta[1]=(float)acos(a[0]);
	
	if(a[2]>=0&&a[1]<0)theta[0]=(float)acos(a[2])*-1;
	if(a[0]>=0&&a[1]<0)theta[1]=(float)acos(a[0])*-1;
	
	if(a[2]<0&&a[1]<0)theta[0]=(float)acos(a[2]*-1)+PI;
	if(a[0]<0&&a[1]<0)theta[1]=(float)acos(a[0]*-1)+PI;
	
	if(a[2]<0&&a[1]>=0)theta[0]=PI-(float)acos(a[2]);
	if(a[0]<0&&a[1]>=0)theta[1]=PI-(float)acos(a[0]);
	*/
	
	//for(int i=0;i<3;i++)
	//{
	//	if(theta[i]<=-2*PI)theta[i]+=2*PI;
	//	if(theta[i]>2*PI)theta[i]-=2*PI;
	//}


	if(theta.x<=-2*PI)
	{
		theta.x+=2*PI;
	}
	if(theta.x>2*PI)
	{
		theta.x-=2*PI;
	}
	if(theta.y<=-2*PI)
	{
		theta.y+=2*PI;
	}
	if(theta.y>2*PI)
	{
		theta.y-=2*PI;
	}
	if(theta.z<=-2*PI)
	{
		theta.z+=2*PI;
	}
	if(theta.z>2*PI)
	{
		theta.z-=2*PI;
	}
}

int ODESchoolOfFish::sign(float a)
{
	if(a==0)return 0;
	return int(a/fabs(a));
}

dReal ODESchoolOfFish::Distance3D(point3d point1, point3d point2)
{
	dReal distance = pow(point1.x - point2.x, 2) + pow(point1.y - point2.y, 2) + 
		pow(point1.z - point2.z, 2);
	return sqrt(distance);
}

//bool ODESchoolOfFish::VectorHasLengthZero(vector3d vec)
//{
//	if (vec.x == 0 && vec.y == 0 && vec.z == 0)
//	{
//		return true;
//	}
//	else
//	{
//		return false;
//	}
//}

//This now gets done in the constructor since we'll need to do this every time anyway.
//void ODESchoolOfFish::AddFishToGlobalObjectList(std::list<Base3DObject*> objectList)
//{
//	std::list<ODEBox*>::iterator fishIter;
//	for (fishIter = fishList.begin(); fishIter != fishList.end(); fishIter++)
//	{
//		objectList.push_back(*fishIter);
//	}
//}