//
// Asura_Platform_Maths.cpp
//
// Created by KOR - 09 October 2006
//
// Copyright (C) 2006 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

// This file's header.
#include "Core/Asura_Maths.h"

// Asura headers
#include "Debugging/Asura_Assert.h"
#include "Rendering/Asura_Colour.h"

const Asura_Vector_2   Asura_Maths::ZeroVector2( 0.0f, 0.0f );

const Asura_Vector_3   Asura_Maths::ZeroVector3(0.0f, 0.0f, 0.0f);

const Asura_Matrix_3x3 Asura_Maths::IdentityMatrix3x3( 1.0f, 0.0f, 0.0f,
                                                        0.0f, 1.0f, 0.0f,
                                                        0.0f, 0.0f, 1.0f );

const Asura_Quat       Asura_Maths::IdentityQuat( 0.f, 0.f, 0.f, 1.f );

//////////////////////////////////////////////////////////////////////////////
//
// FUNCTION     Asura_Bounding_Box::GetSqdMinDistFromBB().
// PARAMETERS   xOtherBB - The BB to be tested. Both BBs are assumed to be in
//                         the same space.
// RETURNS      Minimum distance between the two BBs.
// DESCRIPTION  See RETURNS.
// AUTHOR       MP - 27.11.2002.
//              Based on GetSqdMinDistFromPoint() - see below.
//
//////////////////////////////////////////////////////////////////////////////
float Asura_Bounding_Box::GetSqdMinDistFromBB( const Asura_Bounding_Box& xOtherBB ) const
{
    float fSqdDistance = 0.0f;
    float fDelta;

    if( xOtherBB.MaxX < MinX )
    {
        fDelta = xOtherBB.MaxX - MinX;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinX > MaxX )
    {
        fDelta = xOtherBB.MinX - MaxX;
        fSqdDistance += fDelta * fDelta;
    }

    if( xOtherBB.MaxY < MinY )
    {
        fDelta = xOtherBB.MaxY - MinY;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinY > MaxY )
    {
        fDelta = xOtherBB.MinY - MaxY;
        fSqdDistance += fDelta * fDelta;
    }

    if( xOtherBB.MaxZ < MinZ )
    {
        fDelta = xOtherBB.MaxZ - MinZ;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xOtherBB.MinZ > MaxZ )
    {
        fDelta = xOtherBB.MinZ - MaxZ;
        fSqdDistance += fDelta * fDelta;
    }

    return fSqdDistance;
}
// End function Asura_Bounding_Box::GetSqdMinDistFromBB().

//////////////////////////////////////////////////////////////////////////////
//
// FUNCTION     Asura_Bounding_Box::GetSqdMinDistFromPoint().
// PARAMETERS   xPoint - Point to be tested. This is assumed to be in the
//                       same space as the BB.
// RETURNS      Minimum distance from the point to the BB.
// DESCRIPTION  See RETURNS.
// AUTHOR       Original code: Magic Software. (Note that we're allowed to
//                             use it under the terms of their license.)
//              Asura port   : MP - 27.11.2002.
//
//////////////////////////////////////////////////////////////////////////////
float Asura_Bounding_Box::GetSqdMinDistFromPoint( const Asura_Vector_3& xPoint ) const
{
    float fSqdDistance = 0.0f;
    float fDelta;

    if( xPoint.x < MinX )
    {
        fDelta = xPoint.x - MinX;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.x > MaxX )
    {
        fDelta = xPoint.x - MaxX;
        fSqdDistance += fDelta * fDelta;
    }

    if( xPoint.y < MinY )
    {
        fDelta = xPoint.y - MinY;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.y > MaxY )
    {
        fDelta = xPoint.y - MaxY;
        fSqdDistance += fDelta * fDelta;
    }

    if( xPoint.z < MinZ )
    {
        fDelta = xPoint.z - MinZ;
        fSqdDistance += fDelta * fDelta;
    }
    else if( xPoint.z > MaxZ )
    {
        fDelta = xPoint.z - MaxZ;
        fSqdDistance += fDelta * fDelta;
    }

    return fSqdDistance;
}
// End function Asura_Bounding_Box::GetSqdMinDistFromPoint().

// essentially the same calculation as GetSqdMinDistFromPoint, but this returns the result as an unsquared vector
void Asura_Bounding_Box::CalcMinVectorIntoBox( const Asura_Vector_3& xPoint, Asura_Vector_3& xVector ) const
{
	xVector.x = xVector.y = xVector.z = 0.0f;

	if ( xPoint.x < MinX )
	{
		xVector.x = MinX - xPoint.x;
	}
	else if ( xPoint.x > MaxX )
	{
		xVector.x = MaxX - xPoint.x;
	}
	if ( xPoint.y < MinY )
	{
		xVector.y = MinY - xPoint.y;
	}
	else if ( xPoint.y > MaxY )
	{
		xVector.y = MaxY - xPoint.y;
	}
	if ( xPoint.z < MinZ )
	{
		xVector.z = MinZ - xPoint.z;
	}
	else if ( xPoint.z > MaxZ )
	{
		xVector.z = MaxZ - xPoint.z;
	}
}

void Asura_Bounding_Box::FromOBB( const Asura_Bounding_Box& xOBB, const Asura_Matrix_3x3& xOri, bool bRotateAboutMidpoint /*=false*/  )
{
    const Asura_Vector_3 xCentre  = xOBB.CalculateCentre();
    const Asura_Vector_3 xExtents = xOBB.CalculateExtents();

    float fHalfAABBExtentX = 0.5f * (   Asura_Maths::Abs(xOri.e11 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e12 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e13 * xExtents.z) );

    float fHalfAABBExtentY = 0.5f * (   Asura_Maths::Abs(xOri.e21 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e22 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e23 * xExtents.z) );

    float fHalfAABBExtentZ = 0.5f * (   Asura_Maths::Abs(xOri.e31 * xExtents.x)
                                      + Asura_Maths::Abs(xOri.e32 * xExtents.y)
                                      + Asura_Maths::Abs(xOri.e33 * xExtents.z) );

    MinX = -fHalfAABBExtentX;
    MaxX =  fHalfAABBExtentX;
    MinY = -fHalfAABBExtentY;
    MaxY =  fHalfAABBExtentY;
    MinZ = -fHalfAABBExtentZ;
    MaxZ =  fHalfAABBExtentZ;

	if(bRotateAboutMidpoint)
	{
		Displace( xCentre );
	}
	else
	{
		Displace( xCentre*xOri );
	}
}

//
// Asura_Random.
//

float Asura_RandomGenerator::GetNormalFloat( void )
{
	// We generate two at a time so cache the other one for the next call.
	// The maths below is pretty expensive so this is worth the effort.
	static bool bGotSpare = false;
	static float fSpare;

	if( bGotSpare )
	{
		bGotSpare = false;
		return fSpare;
	}

	// We use a Box-Muller transformation to get two normal distribution
	// random numbers from two uniform distribution ones. Given the latter,
	// u0 and u1, our output values are:
	//
	//     f0 = sqrt(-2 ln u0) * cos(2Pi * u1)
	//     f1 = sqrt(-2 ln u0) * sin(2Pi * u1)
	//
	// However, we can avoid the trigonometry by picking a point inside the
	// 2D unit circle, at the potential expense of having to generate extra
	// input uniform random numbers. See "Numerical Recipes In C" (page 289
	// in 2nd edition) for more details.

	float f0, f1, fMagSqd;
	do
	{
		f0 = 2.0f * GetFloat() - 1.0f;
		f1 = 2.0f * GetFloat() - 1.0f;
		fMagSqd = f0 * f0 + f1 * f1;
	}
	while( !fMagSqd || fMagSqd >= 1.0f );

	float fScale = Asura_Maths::Sqrt( -2.0f * logf(fMagSqd) / fMagSqd );

	fSpare = f1 * fScale;
	bGotSpare = true;

	return f0 * fScale;
}

//
// Asura_Vector_2.
//

//Choose a random direction within an angular deviation of this vector.
Asura_Vector_2 Asura_Vector_2::ChooseRandomDirectionInSegment(const float fMaxTheta, const float fMinTheta) const
{
	float fCosTheta, fSinTheta;

	if(x*x + y*y < 0.000001f)
	{
		Asura_Assert(false, "ChooseRandomDirectionInSegment() used with zero starting vector");
		return *this;
	}

	//Work out a random theta in the range
	// 0 <= theta <= fMaxTheta, and take its cosine.
	fCosTheta = Asura_Maths::Cosine((fMaxTheta-fMinTheta)*Asura_Random::GetFloat() + fMinTheta);

	//Randomly decide on which side of the vector the point should lie, and
	//derive the sine from the cosine, using a square root.
	if(Asura_Random::GetIntU()&0x40000000)
	{
		fSinTheta = Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
	}
	else
	{
		fSinTheta = -Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
	}

	//Rotate this through the angle specified
	Asura_Vector_2 xRet;

	xRet.x = fCosTheta * x - fSinTheta * y;
	xRet.y = fSinTheta * x + fCosTheta * y;

	//Choose a random length for the new vector, in the range of 0..1
	xRet.Normalise();

	return xRet;
}

//Choose a vector of random length within a specified angular devation of
//this vector, and with a length in the specified range.
Asura_Vector_2 Asura_Vector_2::ChooseRandomVectorInSegment(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
	return ChooseRandomDirectionInSegment(fMaxTheta, fMinTheta) 
		* ((fMaxLength - fMinLength) * Asura_Random::GetFloat() + fMinLength);
}

//Choose a random direction within an angular deviation of this vector.
Asura_Vector_2 Asura_Vector_2::ChooseRandomDirectionInSegmentSeeded(const float fMaxTheta, const float fMinTheta) const
{
	float fCosTheta, fSinTheta;

	if(x*x + y*y < 0.000001f)
	{
		Asura_Assert(false, "ChooseRandomDirectionInSegmentSeeded() used with zero starting vector");
		return *this;
	}

	//Work out a random theta in the range
	// 0 <= theta <= fMaxTheta, and take its cosine.
	fCosTheta = Asura_Maths::Cosine((fMaxTheta-fMinTheta)*Asura_Random::GetSeededFloat() + fMinTheta);

	//Randomly decide on which side of the vector the point should lie, and
	//derive the sine from the cosine, using a square root.
	if(Asura_Random::GetIntU()&0x40000000)
	{
		fSinTheta = Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
	}
	else
	{
		fSinTheta = -Asura_Maths::Sqrt(1.f - fCosTheta*fCosTheta);
	}

	//Rotate this through the angle specified
	Asura_Vector_2 xRet;

	xRet.x = fCosTheta * x - fSinTheta * y;
	xRet.y = fSinTheta * x + fCosTheta * y;

	//Choose a random length for the new vector, in the range of 0..1
	xRet.Normalise();

	return xRet;
}

//Choose a vector of random length within a specified angular devation of
//this vector, and with a length in the specified range.
Asura_Vector_2 Asura_Vector_2::ChooseRandomVectorInSegmentSeeded(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
	return ChooseRandomDirectionInSegmentSeeded(fMaxTheta, fMinTheta) 
		* ((fMaxLength - fMinLength) * Asura_Random::GetSeededFloat() + fMinLength);
}

//
// Asura_Vector_3.
//

//Choose a unit vector in a cone, with an angle subtended to this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomDirectionInCone(const float fMaxTheta, const float fMinTheta) const
{
	Asura_Vector_3 xDirection = *this;
	xDirection.Normalise();

	if(fMinTheta > fMaxTheta) return xDirection;

	//Now, we construct an arbitrary orthogonal basis about the axis.
	float fAbsX = Asura_Maths::Abs(xDirection.x);
	float fAbsY = Asura_Maths::Abs(xDirection.y);
	float fAbsZ = Asura_Maths::Abs(xDirection.z);

	//Asura_Vector_3 xContructionAxis(1.f, 0.f, 0.f);
	Asura_Vector_3 xNormal;

	//Work out the largest component of xDirection, and construct an appropriate normal,
	//using the unrolled cross product with that axis.
	if(fAbsX > fAbsY)
	{
		if(fAbsY > fAbsZ)
		{	
			//Cross with z-axis.
			xNormal.x = xDirection.y;
			xNormal.y = -xDirection.x;
			xNormal.z = 0.f;
		}
		else
		{
			//Cross with y-axis.
			xNormal.x = -xDirection.z;
			xNormal.y = 0.f;
			xNormal.z = xDirection.x;
		}
	}
	else if(fAbsX > fAbsZ)
	{
		//Cross with z-axis.
		xNormal.x = xDirection.y;
		xNormal.y = -xDirection.x;
		xNormal.z = 0.f;
	}
	else
	{
		//Cross with x-axis.
		xNormal.x = 0.f;
		xNormal.y = xDirection.z;
		xNormal.z = -xDirection.y;
	}

	xNormal.Normalise();

	//Now that we have the first normal, we can construct the second.
	Asura_Vector_3 xNormal2 = xDirection^xNormal;

	//Now, we choose a random 2-vector.
	Asura_Vector_2 xRandom;
	xRandom.RandomDirection();

	//Now we choose a random number between the cosines of the two angles.
	float fV1 = Asura_Maths::Cosine(fMinTheta);
	float fV2 = Asura_Maths::Cosine(fMaxTheta);

	float fRandomFloat = Asura_Random::GetFloat() * (fV1-fV2) + fV2;

	//Just check the normalisation, to be sure.
	if(fRandomFloat > 1.f) fRandomFloat = 1.f;
	else if(fRandomFloat < -1.f) fRandomFloat = -1.f;

	//Scale the primary component to the chosen length.
	xDirection *= fRandomFloat;

	//Work out the correct normalisation factor for the second component...
	//...and add the second component, scaled by that factor.
	xDirection += (xNormal * xRandom.x + xNormal2 * xRandom.y) * Asura_Maths::Sqrt(1.f - fRandomFloat*fRandomFloat);

	return xDirection;
}

//Choose a vector of length within the specified range, and with an angular deviation
//from this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomVectorInCone(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
	float fLength = Asura_Random::GetFloat()*(fMaxLength - fMinLength) + fMinLength;

	return ChooseRandomDirectionInCone(fMaxTheta, fMinTheta)*fLength;
}

//Choose a unit vector in a cone, with an angle subtended to this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomDirectionInConeSeeded(const float fMaxTheta, const float fMinTheta) const
{
	Asura_Vector_3 xDirection = *this;
	xDirection.Normalise();

	if(fMinTheta > fMaxTheta) return xDirection;

	//Now, we construct an arbitrary orthogonal basis about the axis.
	float fAbsX = Asura_Maths::Abs(xDirection.x);
	float fAbsY = Asura_Maths::Abs(xDirection.y);
	float fAbsZ = Asura_Maths::Abs(xDirection.z);

	//Asura_Vector_3 xContructionAxis(1.f, 0.f, 0.f);
	Asura_Vector_3 xNormal;

	//Work out the largest component of xDirection, and construct an appropriate normal,
	//using the unrolled cross product with that axis.
	if(fAbsX > fAbsY)
	{
		if(fAbsY > fAbsZ)
		{	
			//Cross with z-axis.
			xNormal.x = xDirection.y;
			xNormal.y = -xDirection.x;
			xNormal.z = 0.f;
		}
		else
		{
			//Cross with y-axis.
			xNormal.x = -xDirection.z;
			xNormal.y = 0.f;
			xNormal.z = xDirection.x;
		}
	}
	else if(fAbsX > fAbsZ)
	{
		//Cross with z-axis.
		xNormal.x = xDirection.y;
		xNormal.y = -xDirection.x;
		xNormal.z = 0.f;
	}
	else
	{
		//Cross with x-axis.
		xNormal.x = 0.f;
		xNormal.y = xDirection.z;
		xNormal.z = -xDirection.y;
	}

	xNormal.Normalise();

	//Now that we have the first normal, we can construct the second.
	Asura_Vector_3 xNormal2 = xDirection^xNormal;

	//Now, we choose a random 2-vector.
	Asura_Vector_2 xRandom;
	xRandom.RandomDirectionSeeded();

	//Now we choose a random number between the cosines of the two angles.
	float fV1 = Asura_Maths::Cosine(fMinTheta);
	float fV2 = Asura_Maths::Cosine(fMaxTheta);

	float fRandomFloat = Asura_Random::GetSeededFloat() * (fV1-fV2) + fV2;

	//Just check the normalisation, to be sure.
	if(fRandomFloat > 1.f) fRandomFloat = 1.f;
	else if(fRandomFloat < -1.f) fRandomFloat = -1.f;

	//Scale the primary component to the chosen length.
	xDirection *= fRandomFloat;

	//Work out the correct normalisation factor for the second component...
	//...and add the second component, scaled by that factor.
	xDirection += (xNormal * xRandom.x + xNormal2 * xRandom.y) * Asura_Maths::Sqrt(1.f - fRandomFloat*fRandomFloat);

	return xDirection;
}

//Choose a vector of length within the specified range, and with an angular deviation
//from this vector in the specified range.
Asura_Vector_3 Asura_Vector_3::ChooseRandomVectorInConeSeeded(const float fMaxTheta, const float fMinTheta, const float fMaxLength, const float fMinLength) const
{
	float fLength = Asura_Random::GetSeededFloat()*(fMaxLength - fMinLength) + fMinLength;

	return ChooseRandomDirectionInConeSeeded(fMaxTheta, fMinTheta)*fLength;
}

// F U N C T I O N   I M P L E M E N T A T I O N S ///////////////////////////

void Asura_Maths::OrientationFromDirection(Asura_Matrix_3x3 &Orientation, const Asura_Vector_3 &d)
{
	Asura_Vector_3 XVector;
	Asura_Vector_3 YVector;
	Asura_Vector_3 ZVector = d;

	ZVector.Normalise();


	if (ZVector.z==0.0f && ZVector.x==0.0f)
	{
		XVector.x = -ZVector.y;
		XVector.y = ZVector.x;
		XVector.z = 0.0f;

		XVector.Normalise();

		YVector = -XVector^ZVector;
	}
	else
	{
		XVector.x = ZVector.z;
		XVector.y = 0;
		XVector.z = -ZVector.x;

		XVector.Normalise();

		YVector = -XVector^ZVector;
	}

	Orientation.e11 = XVector.x;
	Orientation.e21 = XVector.y;
	Orientation.e31 = XVector.z;

	Orientation.e12 = YVector.x;
	Orientation.e22 = YVector.y;
	Orientation.e32 = YVector.z;

	Orientation.e13 = ZVector.x;
	Orientation.e23 = ZVector.y;
	Orientation.e33 = ZVector.z;
}
