//======== (C) Copyright 2002 Charles G. Cleveland All rights reserved. =========
//
// The copyright to the contents herein is the property of Charles G. Cleveland.
// The contents may be used and/or copied only with the written permission of
// Charles G. Cleveland, or in accordance with the terms and conditions stipulated in
// the agreement/contract under which the contents have been supplied.
//
// Purpose: 
//
// $Workfile: MathUtil.cpp $
// $Date: 2002/07/25 16:59:03 $
//
//-------------------------------------------------------------------------------
// $Log: MathUtil.cpp,v $
// Revision 1.7  2002/07/25 16:59:03  flayra
// -Linux changes
//
// Revision 1.6  2002/07/23 16:53:46  Flayra
// - Added VectorDistance2D, added document headers
//
//===============================================================================
#include <cmath>
#include "stdio.h"
#include "stdlib.h"
#include <math.h>

//#include "hud.h"
//#include "cl_util.h"
#include <string.h>
#include "nowarnings.h"
#include "MathUtil.h"
#include "common/vec_op.h"
#include "common/mathlib.h"

#define max(a,b)    (((a) > (b)) ? (a) : (b))
#define min(a,b)    (((a) < (b)) ? (a) : (b))

// Ignore "double to float possible loss of data" warning
#pragma warning (disable: 4244)

double sqrt(double x);

bool FindCollisionPointOnPlane(const float* inOrigin, const float* inRayVector, const float* inPlaneABCD, float* outPoint)
{
	bool theSuccess = false;
	
	// Solve for parametric t
	float thePlaneA = inPlaneABCD[0];
	float thePlaneB = inPlaneABCD[1];
	float thePlaneC = inPlaneABCD[2];
	float thePlaneD = inPlaneABCD[3];

	float theDenom = (thePlaneA*inRayVector[0] + thePlaneB*inRayVector[1] + thePlaneC*inRayVector[2]);
	if(fabs(theDenom) > kFloatTolerance)
	{
		float theT = -(thePlaneA*inOrigin[0] + thePlaneB*inOrigin[1] + thePlaneC*inOrigin[2] + thePlaneD)/theDenom;
		
		// Now we have t, solve for the endpoint
		outPoint[0] = inOrigin[0] + theT*inRayVector[0];
		outPoint[1] = inOrigin[1] + theT*inRayVector[1];
		outPoint[2] = inOrigin[2] + theT*inRayVector[2];
		theSuccess = true;
	}
	
	//	float theA = inPlaneABCD[0];
	//	float theB = inPlaneABCD[1];
	//	float theC = inPlaneABCD[2];
	//	float theD = inPlaneABCD[3];
	//
	//	float theDenom = (theA*inRayVector[0] + theB*inRayVector[1] + theC*inRayVector[2]);
	//	if(fabs(theDenom) > kFloatTolerance)
	//	{
	//		float theT = -1*(theA*inOrigin[0] + theB*inOrigin[1] + theC*inOrigin[2] + theD)/theDenom;
	//		outPoint[0] = inOrigin[0] + inRayVector[0]*theT;
	//		outPoint[1] = inOrigin[1] + inRayVector[1]*theT;
	//		outPoint[2] = inOrigin[2] + inRayVector[2]*theT;
	//		theSuccess = true;
	//	}
	
	return theSuccess;
}

bool IsVectorBetweenBoundingVectors(const float* inOrigin, const float* inRay, const float* inVecOne, const float* inVecTwo)
{
	bool theSuccess = false;
	
	// The plane normal is opposite to our view
	float thePlaneABCD[4];
	thePlaneABCD[0] = 0;//inRay[0];
	thePlaneABCD[1] = 0;//inRay[1];
	thePlaneABCD[2] = 1;//inRay[2];
	thePlaneABCD[3] = 0;

	// Put plane far away by plugging in far away point (ax + by + cz + d = 0)
//	float theT = 5000;
//	float thePoint[3];
//	thePoint[0] = inOrigin[0] + theT*thePlaneABCD[0];
//	thePoint[1] = inOrigin[1] + theT*thePlaneABCD[1];
//	thePoint[2] = inOrigin[2] + theT*thePlaneABCD[2];
//
//	// Find plane D using this point
//	thePlaneABCD[3] = -(thePlaneABCD[0]*thePoint[0] + thePlaneABCD[1]*thePoint[1] + thePlaneABCD[2]*thePoint[2]);

	// Solve for each vector hitting the plane
	float theVecPoint[3];
	float theVecOnePoint[3];
	float theVecTwoPoint[3];
	
	// If they all hit the plane
	if(FindCollisionPointOnPlane(inOrigin, inRay, thePlaneABCD, theVecPoint))
	{
		if(FindCollisionPointOnPlane(inOrigin, inVecOne, thePlaneABCD, theVecOnePoint))
		{
			if(FindCollisionPointOnPlane(inOrigin, inVecTwo, thePlaneABCD, theVecTwoPoint))
			{
				// Get the collision point for each solution
				float theMaxX = max(theVecOnePoint[0], theVecTwoPoint[0]);
				float theMaxY = max(theVecOnePoint[1], theVecTwoPoint[1]);
				float theMaxZ = max(theVecOnePoint[2], theVecTwoPoint[2]);
				
				float theMinX = min(theVecOnePoint[0], theVecTwoPoint[0]);
				float theMinY = min(theVecOnePoint[1], theVecTwoPoint[1]);
				float theMinZ = min(theVecOnePoint[2], theVecTwoPoint[2]);
				
				// If it is between them, the vector is between them
				float theVecX = theVecPoint[0];
				float theVecY = theVecPoint[1];
				float theVecZ = theVecPoint[2];
				if(	(theVecX >= (theMinX - kFloatTolerance)) && (theVecX <= (theMaxX + kFloatTolerance)) && 
					(theVecY >= (theMinY - kFloatTolerance)) && (theVecY <= (theMaxY + kFloatTolerance)) && 
					(theVecZ >= (theMinZ - kFloatTolerance)) && (theVecZ <= (theMaxZ + kFloatTolerance)))
				{
					theSuccess = true;
				}
			}
		}
	}
	
	return theSuccess;
}

void RotateFloatValuesByVector(float& ioX, float& ioY, float& ioZ, /*const float* inBaseVector,*/ const float* inVector)
{
	// Get rotation vector
	//float theBaseRotationAngles[3];
	//VectorAngles((float*)inBaseVector, theBaseRotationAngles);
	
	// Get rotation vector
	float theRotationAngles[3];
	VectorAngles((float*)inVector, theRotationAngles);

	// Subtract out frame of reference
	//theRotationAngles[0] -= theBaseRotationAngles[0];
	//theRotationAngles[1] -= theBaseRotationAngles[1];
	//theRotationAngles[2] -= theBaseRotationAngles[2];
	
	// Rotate the first three parameters as a point
	float theSourceValues[3] = {ioX, ioY, ioZ};
				
	float theMatrix[3][4];
	AngleMatrix(theRotationAngles, theMatrix);
	
	float theRotatedValues[3];
	VectorRotate(theSourceValues, theMatrix, theRotatedValues);
	
	ioX = (int)theRotatedValues[0];
	ioY = (int)theRotatedValues[1];
	ioZ = (int)theRotatedValues[2];
}

float WrapFloat(float inValue, float inMin, float inMax)
{
    const float theRange = inMax - inMin;
	
    if (inValue < inMin)
    {
        inValue += floor((inMax - inValue) / theRange) * theRange;
    }    
	
    if (inValue >= inMax)
    {
        inValue -= floor(((inValue - inMin) / theRange)) * theRange;
    }
    
    return inValue;
}

void CreateOrthoNormalBasis(float inZAxis[3], float outXAxis[3], float outYAxis[3])
{
	VectorNormalize(inZAxis);

	// check if in vector is z
	float theUp[3] = { 0, 0, 1 };
	if(fabs(DotProduct(theUp, inZAxis)) >= (1.0 - kFloatTolerance))
	{
		// Use y instead
		theUp[0] = 0;
		theUp[1] = 1;
		theUp[2] = 0;
	}

	CrossProduct(inZAxis, theUp, outXAxis);
	VectorNormalize(outXAxis);

	CrossProduct(outXAxis, inZAxis, outYAxis);
	VectorNormalize(outYAxis);
}

int RoundIntToNearestIncrementOf(int inValue, int inIncrement)
{
	int theValue = inValue;
	if(inIncrement > 0)
	{
		theValue = ((inValue + inIncrement/2)/inIncrement)*inIncrement;
	}
	return theValue;
}

void TransformVector(const float v[3], const float xAxis[3], const float yAxis[3], const float zAxis[3], float result[3])
{
	float temp[3];
	for (int i = 0; i < 3; ++i)
	{
		temp[i] = v[0] * xAxis[i] + v[1] * yAxis[i] + v[2] * zAxis[i];
	}
	for (int j = 0; j < 3; ++j)
	{
		result[j] = temp[j];
	}
}

void RotateValuesByVector(int32& ioX, int32& ioY, int32& ioZ, /*const float* inBaseVector,*/ const float* inVector)
{
	float ioFloatX = ioX;
	float ioFloatY = ioY;
	float ioFloatZ = ioZ;

	RotateFloatValuesByVector(ioFloatX, ioFloatY, ioFloatZ, inVector);

	ioX = (int32)ioFloatX;
	ioY = (int32)ioFloatY;
	ioZ = (int32)ioFloatZ;
}
//#ifndef AVH_SERVER
void VectorAngles( const float *forward, float *angles )
{
	float	tmp, yaw, pitch;
	
	if (forward[1] == 0 && forward[0] == 0)
	{
		yaw = 0;
		if (forward[2] > 0)
			pitch = 90;
		else
			pitch = 270;
	}
	else
	{
		yaw = (atan2(forward[1], forward[0]) * 180 / M_PI);
		if (yaw < 0)
			yaw += 360;

		tmp = sqrt (forward[0]*forward[0] + forward[1]*forward[1]);
		pitch = (atan2(forward[2], tmp) * 180 / M_PI);
		if (pitch < 0)
			pitch += 360;
	}
	
	angles[0] = pitch;
	angles[1] = yaw;
	angles[2] = 0;
}

void VectorInverse ( float *v )
{
	v[0] = -v[0];
	v[1] = -v[1];
	v[2] = -v[2];
}

void VectorMA (const float *veca, float scale, const float *vecb, float *vecc)
{
	vecc[0] = veca[0] + scale*vecb[0];
	vecc[1] = veca[1] + scale*vecb[1];
	vecc[2] = veca[2] + scale*vecb[2];
}

float Length(const float *v)
{
	int		i;
	float	length;
	
	length = 0;
	for (i=0 ; i< 3 ; i++)
		length += v[i]*v[i];
	length = sqrt (length);		// FIXME
	
	return length;
}

float VectorNormalize (float *v)
{
	float	length, ilength;
	
	length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
	length = sqrt (length);		// FIXME
	
	if (length)
	{
		ilength = 1/length;
		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	}
	
	return length;
	
}

void VectorScale (const float *in, float scale, float *out)
{
	out[0] = in[0]*scale;
	out[1] = in[1]*scale;
	out[2] = in[2]*scale;
}
//#endif

void VectorRotate (const float* in1, const float in2[3][4], float* out)
{
	out[0] = DotProduct(in1, in2[0]);
	out[1] = DotProduct(in1, in2[1]);
	out[2] = DotProduct(in1, in2[2]);
}

double VectorDistance(const float* in1, const float* in2)
{
	float theXDiff = in1[0] - in2[0];
	float theYDiff = in1[1] - in2[1];
	float theZDiff = in1[2] - in2[2];

	return sqrt(theXDiff*theXDiff + theYDiff*theYDiff + theZDiff*theZDiff);
}

double VectorDistance2D(const float* in1, const float* in2)
{
	float theXDiff = in1[0] - in2[0];
	float theYDiff = in1[1] - in2[1];
	
	return sqrt(theXDiff*theXDiff + theYDiff*theYDiff);
}

// Added by Neoptolemus

void VectorGetClosestPointOnLine(const float* inLineFrom, const float* inLineTo, const float* inTestPosition, float *outClosestPoint)
{
	
	float vVector1[3];
	VectorSubtract(inTestPosition, inLineFrom, vVector1);

	float vVector2[3];
	VectorSubtract(inLineTo, inLineFrom, vVector2);

	VectorNormalize(vVector2);

	float d = VectorDistance(inLineTo, inLineFrom);
	float t = DotProduct(vVector2, vVector1);

	if (t <= 0)
	{
		outClosestPoint[0] = inLineFrom[0];
		outClosestPoint[1] = inLineFrom[1];
		outClosestPoint[2] = inLineFrom[2];

		return;
	}

	if (t >= d)
	{
		outClosestPoint[0] = inLineTo[0];
		outClosestPoint[1] = inLineTo[1];
		outClosestPoint[2] = inLineTo[2];

		return;
	}

	
	float vVector3[3];

	VectorScale(vVector2, t, vVector3);

	outClosestPoint[0] = inLineFrom[0] + vVector3[0];
	outClosestPoint[1] = inLineFrom[1] + vVector3[1];
	outClosestPoint[2] = inLineFrom[2] + vVector3[2];

}

float VectorDistanceFromLine(const float* inLineFrom, const float* inLineTo, const float* inTestPosition)
{
	float nearestPointToLine[3];
	VectorGetClosestPointOnLine(inLineFrom, inLineTo, inTestPosition, nearestPointToLine);

	return VectorDistance(inTestPosition, nearestPointToLine);
}

void VectorGetMidPointOnLine(const float* inLineFrom, const float* inLineTo, float* outPosition)
{
	float vVector1[3];
	VectorSubtract(inLineTo, inLineFrom, vVector1);
	VectorScale(vVector1, 0.5f, vVector1);

	VectorAdd(inLineFrom, vVector1, outPosition);
}


// Added by mmcguire.

void VectorsToAngles(const float forward[3], const float right[3], const float up[3], float angles[3])
{
   
    float y,r,p;
    float sy;
    
	if(abs(forward[2]) < 0.9999)
	{
		y = atan2(forward[1], forward[0]);
	    sy = sin(y);
		if (abs(sy) < 0.1)
		{
			p = atan2(-forward[2], forward[0] / cos(y));
		}
		else
		{
			p = atan2(-forward[2], forward[1] / sy);
		}
		r = atan2(-right[2], up[2]);
	}
	else //gimbal lock; best we can do is assume roll = 0 and set pitch = pitch + actual roll
	{
		p = forward[2] > 0 ? -M_PI/2 : M_PI/2;
		y = atan2(right[0],-right[1]);
		r = 0;
	}

    angles[2] = r * (180 / M_PI); // Roll
    angles[0] = p * (180 / M_PI); // Pitch
    angles[1] = y * (180 / M_PI); // Yaw
}