using System;
using System.IO;
using System.Drawing;
using Microsoft.DirectX;


namespace DarkStrideToolbox
{
	//This class contains all the math that doesn't belong in its own class.  As an example all the matrix math will 
	//probabely be put into its own class.  But I don't need an "Angle" class so it all goes in here.  Although matrix
	//math is currently in here because I haven't created a matrix class yet.
	public class DSMath
	{
		public DSMath(){}


		//This function returns which direction to go for the shortest path from angle1 to angle2.
		public enum enumSHORTDISTTOANGLE
		{
			enumCCW = 0,
			enumCW = 1,
			enumSame = 2
		};		
		public static enumSHORTDISTTOANGLE ShortestDirectionToRadAngle( double nCurAngle,double nTargetAngle )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.ShortestDirectionToRadAngle";
			double nLenCW = 0;
			double nLenCCW = 0;
			enumSHORTDISTTOANGLE nWhichIsShortest = enumSHORTDISTTOANGLE.enumSame;

			try
			{
				//Get the length of the journey if we CCW
				nLenCCW = DistanceToRadAngleGoingCCW( nCurAngle,nTargetAngle );
				//Get the length of the journey if we CW
				nLenCW = DistanceToRadAngleGoingCW( nCurAngle,nTargetAngle );

				//Now determine which is shorter
				if( nLenCCW == nLenCW )
				{
					nWhichIsShortest = enumSHORTDISTTOANGLE.enumSame;
				}
				else if( nLenCCW < nLenCW )
				{
					nWhichIsShortest = enumSHORTDISTTOANGLE.enumCCW;
				}
				else
				{
					nWhichIsShortest = enumSHORTDISTTOANGLE.enumCW;
				}
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nWhichIsShortest );
		}


		//Returns the distance from Angle 1 to Angle 2 by going CW
		public static double DistanceToRadAngleGoingCW( double nCurAngle,double nTargetAngle )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.DistanceToRadAngleGoingCW";
			double nLenCW = 0;

			try
			{
				//Get the length of the journey if we CCW
				if( nCurAngle < nTargetAngle )
				{
					nLenCW = nTargetAngle-nCurAngle;
				}
				else if( nCurAngle > nTargetAngle )
				{
					nLenCW = Math.PI*2-nCurAngle + nTargetAngle;
				}
				else
				{
					nLenCW = 0;
				}
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nLenCW );
		}
		//Returns the distance from Angle 1 to Angle 2 by going CW
		public static double DistanceToRadAngleGoingCCW( double nCurAngle,double nTargetAngle )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.DistanceToRadAngleGoingCCW";
			double nLenCCW = 0;

			try
			{
				//Get the length of the journey if we CW
				if( nCurAngle < nTargetAngle )
				{
					nLenCCW = nCurAngle + Math.PI*2 - nTargetAngle;
				}
				else if( nCurAngle > nTargetAngle )
				{
					nLenCCW = nCurAngle - nTargetAngle;
				}
				else
				{
					nLenCCW = 0;
				}		
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nLenCCW );
		}

		//Calculate the difference between some angles
		public static double AngleDegDiff( double nAngle1,double nAngle2 )
		{
			double nNormAngle1 = 0;
			double nNormAngle2 = 0;
			double nRetVal = 0;

			nNormAngle1 = DSMath.NormalizeRadAngle( nAngle1 );
			nNormAngle2 = DSMath.NormalizeRadAngle( nAngle2 );

			nRetVal = DSMisc.Min( DSMath.DistanceToRadAngleGoingCCW( nNormAngle1,nNormAngle2 ),
				DSMath.DistanceToRadAngleGoingCW( nNormAngle1,nNormAngle2 ) );

			return( nRetVal );
		}
		public static double Angle90DegDiff( double nAngle1,double nAngle2 )
		{
			double[] nNormAngle1 = new double[ 2 ];
			double[] nNormAngle2 = new double[ 2 ];
			double nRetVal = Math.PI * 2.0;


			nNormAngle1[ 0 ] = DSMath.NormalizeRadAngle( nAngle1 );
			nNormAngle2[ 0 ] = DSMath.NormalizeRadAngle( nAngle2 );
			if( nNormAngle1[ 0 ] < Math.PI )
			{
				nNormAngle1[ 1 ] = DSMath.NormalizeRadAngle( nAngle1 + Math.PI );
			}
			else
			{
				nNormAngle1[ 1 ] = DSMath.NormalizeRadAngle( nAngle1 - Math.PI );
			}
			if( nNormAngle2[ 0 ] < Math.PI )
			{
				nNormAngle2[ 1 ] = DSMath.NormalizeRadAngle( nAngle2 + Math.PI );
			}
			else
			{			
				nNormAngle2[ 1 ] = DSMath.NormalizeRadAngle( nAngle2 - Math.PI );
			}

			for( int n1Ndx=0 ; n1Ndx<2 ; n1Ndx++ )
			{
				for( int n2Ndx=0 ; n2Ndx<2 ; n2Ndx++ )
				{
					nRetVal = DSMisc.Min( nRetVal,DSMath.DistanceToRadAngleGoingCCW( nNormAngle1[ n1Ndx ],nNormAngle2[ n2Ndx ] ) );
					nRetVal = DSMisc.Min( nRetVal,DSMath.DistanceToRadAngleGoingCW( nNormAngle1[ n1Ndx ],nNormAngle2[ n2Ndx ] ) );
				}
			}

			
			return( nRetVal );
		}


		//This function moves number x towords number y by an interval of change.  If it would
		//pass this number then it sets it equal to the number.
		public static double MoveTowords( double nTarget,double nStart,double nChange )
		{
			double nRetVal = 0;


			if( nTarget < nStart )
			{
				if( nStart + Math.Abs( nChange ) * -1 < nTarget )
				{
					nRetVal = nTarget;
				}
				else
				{
					nRetVal = nStart + Math.Abs( nChange ) * -1;
				}
			}
			else
			{
				if( nStart + Math.Abs( nChange ) > nTarget )
				{
					nRetVal = nTarget;
				}
				else
				{
					nRetVal = nStart + Math.Abs( nChange );
				}
			}


			return( nRetVal );			
		}
		

		//Move a current angle twords a target angle by the fastest route possible
		public static double MoveRadAngleTwordsRadAngle( double nCurAngle,double nChangeAmount,double nTargetAngle )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.MoveRadAngleTwordsAngle";
			double nLenCW = 0;
			double nLenCCW = 0;
			double nNewAngle = 0;

			try
			{
				//Find out which path is shorter
				nLenCCW = DistanceToRadAngleGoingCCW( nCurAngle,nTargetAngle );
				//Get the length of the journey if we CW
				nLenCW = DistanceToRadAngleGoingCW( nCurAngle,nTargetAngle );

				//Now which path is shorter
				if( nLenCW == 0 && nLenCCW == 0 )
				{
					nNewAngle = nCurAngle;
				}
				else if( nLenCCW < nLenCW )
				{
					if( nLenCCW > nChangeAmount )
					{
						nNewAngle = nCurAngle - nChangeAmount;
					}
					else
					{
						nNewAngle = nTargetAngle;
					}
				}
				else //if( nLenCW < nLenCCW )
				{
					if( nLenCW > nChangeAmount )
					{
						nNewAngle = nCurAngle + nChangeAmount;
					}
					else
					{
						nNewAngle = nTargetAngle;
					}
				}

				//Now normalize the new angle
				nNewAngle = NormalizeRadAngle( nNewAngle );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nNewAngle );
		}


		//This function takes an angle and puts it between 0 and 2*PI
		public static double NormalizeRadAngle( double nAngle )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.NormalizeRadAngle";
			double nNewAngle = nAngle;
			long nTemp = 0;

			try
			{
				if( nNewAngle > Math.PI*2 )
				{
					nTemp = (long)( nNewAngle / (Math.PI*2) );
					nNewAngle -= ( nTemp * Math.PI*2 );
				}
				else if( nNewAngle < 0 )
				{
					nTemp = (long)( -nNewAngle / (Math.PI*2) ) + 1;
					nNewAngle += Math.PI*2 * nTemp;
				}
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nNewAngle );
		}


		//11/25/2005 Chris Hill  Modified this function to use the standard trig quadrants.
		//05/20/2003 Chris Hill  This function takes one vector and returns the angle the vector is at.
		//      90
		//      |
		//180 -- -- 0 
		//      |
		//     270
		public static double CalculateRadAngle( double nX1,double nY1,double nX2,double nY2 )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.CalculateRadAngle";
			double nRetVal = 0;
			double nDeltaX = nX2 - nX1;	
			double nDeltaY = nY2 - nY1;
			
			try
			{
				if( nDeltaX == 0 )
				{
					if( nDeltaY < 0 )
					{
						nRetVal = ( Math.PI / 2 ) * 3;
					}
					else
					{
						nRetVal = Math.PI / 2;
					}
				}
				else if( nDeltaY == 0 )
				{
					if( nDeltaX > 0 )
					{
						nRetVal = 0;
					}
					else
					{
						nRetVal = Math.PI;
					}
				}
				//Quadrent 1
				else if( nDeltaX > 0 && nDeltaY > 0 )
				{
					nRetVal = Math.Atan( nDeltaY / nDeltaX );
				}
				//Quadrent 2
				else if( nDeltaX < 0 && nDeltaY > 0 )
				{
					nRetVal = Math.Atan( Math.Abs( nDeltaX ) / nDeltaY ) + (Math.PI/2);
				}
				//Quadrent 3
				else if( nDeltaX < 0 && nDeltaY < 0 )
				{
					nRetVal = Math.Atan( nDeltaY / nDeltaX ) + Math.PI;
				}
				//Quadrent 4
				else if( nDeltaX > 0 && nDeltaY < 0 )
				{
					nRetVal = Math.Atan( nDeltaX / Math.Abs( nDeltaY ) ) + (Math.PI/2)*3;
				}
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nRetVal );
		}


		//07/11/2003 Chris Hill  This function converts a number from radians to degrees.
		public static double DegToRad( double nDeg )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.DegToRad";
			double nRad = 0;

			try
			{
				nRad = ( nDeg / 360.0 ) * ( Math.PI * 2 );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nRad );
		}
		//07/11/2003 Chris Hill  This function converts a number from degrees to radians.
		public static double RadToDeg( double nRad )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.RadToDeg";
			double nDeg = 0;

			try
			{
				nDeg = ( nRad / ( Math.PI * 2 ) ) * 360.0;
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDeg );
		}


		//This function returns the distance between two points
		//Define your two points. Point 1 at (x1, y1) and Point 2 at (x2, y2).
		//xd = x2-x1
		//yd = y2-y1
		//Distance = SquareRoot(xd*xd + yd*yd)
		/*
		 * Excerpt From "http://freespace.virgin.net/hugo.elias/routines/r_dist.htm"
		 * As you can see, this requires that you perform a square root. Square roots should be avoided like the plague if you want to write fast code. Only perform a Square Root if you really need to.
		 * Ways to avoid Square Roots:
		 *   If you don't need a very accurate distance, you can use a lookup table to calculate it.
		 *   If, for example, you are performing collision detection between spheres, and all you want to know is whether or not two have collided, then you do not need to use a square root. Simply change the piece of code from: 
		 *   if SquareRoot(xd*xd + yd*yd) < Diameter
		 *								   to: 
		 *   if (xd*xd + yd*yd) < (Diameter*Diameter)
		 * */
		public static double Distance( double dX1,double dY1,double dX2,double dY2)
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.Distance";
			double nDistance = 0;
			
			try
			{
				//nDistance = ( Math.Sqrt( dX1 - dX2 ) + Math.Sqrt( dY1 - dY2 ) ) *
				//			( Math.Sqrt( dX1 - dX2 ) + Math.Sqrt( dY1 - dY2 ) );

				nDistance = Math.Sqrt( ( (dX2-dX1)*(dX2-dX1) ) + 
										( (dY2-dY1)*(dY2-dY1) ) );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDistance );
		}
		//Define your two points. Point 1 at (x1, y1, z1) and Point 2 at (x2, y2, z2).
		//xd = x2-x1
		//yd = y2-y1
		//zd = z2-z1
		//Distance = SquareRoot(xd*xd + yd*yd + zd*zd)
		public static double Distance( double dX1,double dY1,double dZ1,double dX2,double dY2,double dZ2 )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.Distance";
			double nDistance = 0;
			
			try
			{
				nDistance = Math.Sqrt( ( (dX2-dX1)*(dX2-dX1) ) + 
										( (dY2-dY1)*(dY2-dY1) ) +
										( (dZ2-dZ1)*(dZ2-dZ1) ) );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDistance );
		}
		public static double Distance( Vector3 vPoint1,Vector3 vPoint2 )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.Distance";
			double nDistance = 0;
			
			try
			{
				nDistance = DSMath.Distance( vPoint1.X,vPoint1.Y,vPoint1.Z,
											   vPoint2.X,vPoint2.Y,vPoint2.Z );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDistance );
		}
		public static double Distance( Vector2 vPoint1,Vector2 vPoint2 )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.Distance";
			double nDistance = 0;
			
			try
			{
				nDistance = DSMath.Distance( vPoint1.X,vPoint1.Y,0,
											   vPoint2.X,vPoint2.Y,0 );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDistance );
		}
		public static double Distance( Point oPoint1,Point oPoint2 )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.Distance";
			double nDistance = 0;
			
			try
			{
				nDistance = DSMath.Distance( oPoint1.X,oPoint1.Y,0,oPoint2.X,oPoint2.Y,0 );
			}
			catch( System.Exception oEx )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEx );
			}
			return( nDistance );
		}


		//These functons multiply a matrix and a vector.  Turns out they are order of operations important... so you 
		//have to know if you want to multiply a vector by a matrix or a matrix by a vector. 
		public static Vector3 VectorMatrixMultiply( Matrix oMatrix,Vector3 oVector )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.VectorMatrixMultiply";
			Vector3 oRetVal;
			float nX1 = 0,nX2 = 0,nX3 = 0;

			try
			{
				//Why a *1 on the end you ask?  When donig any kind of 3D work you really are supposed to use Vector4
				//not Vector3.  In that Vector4 the last bit will always be a one.  Why is that you ask?  I have no clue.
				//I assume it has something to do with getting the transpose numbers from a 4x4 matrix.
				nX1 = oMatrix.M11 * oVector.X  +  oMatrix.M12 * oVector.Y  +  oMatrix.M13 * oVector.Z  +  oMatrix.M14 * 1;
				nX2 = oMatrix.M21 * oVector.X  +  oMatrix.M22 * oVector.Y  +  oMatrix.M23 * oVector.Z  +  oMatrix.M24 * 1;
				nX3 = oMatrix.M31 * oVector.X  +  oMatrix.M32 * oVector.Y  +  oMatrix.M33 * oVector.Z  +  oMatrix.M34 * 1;

				oRetVal = new Vector3(nX1,nX2,nX3);
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEnX );
			}
			return( oRetVal );
		}
		public static Vector4 VectorMatrixMultiply( Matrix oMatrix,Vector4 oVector )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.VectorMatrixMultiply";
			Vector4 oRetVal;
			float nX1 = 0,nX2 = 0,nX3 = 0,nX4 = 0;

			try
			{
				nX1 = oMatrix.M11 * oVector.X  +  oMatrix.M12 * oVector.Y  +  oMatrix.M13 * oVector.Z  +  oMatrix.M14 * oVector.W;
				nX2 = oMatrix.M21 * oVector.X  +  oMatrix.M22 * oVector.Y  +  oMatrix.M23 * oVector.Z  +  oMatrix.M24 * oVector.W;
				nX3 = oMatrix.M31 * oVector.X  +  oMatrix.M32 * oVector.Y  +  oMatrix.M33 * oVector.Z  +  oMatrix.M34 * oVector.W;
				nX4 = oMatrix.M41 * oVector.X  +  oMatrix.M42 * oVector.Y  +  oMatrix.M43 * oVector.Z  +  oMatrix.M44 * oVector.W;

				oRetVal = new Vector4(nX1,nX2,nX3,nX4);
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEnX );
			}
			return( oRetVal );
		}

		public static Vector3 MatrixVectorMultiply( Matrix oMatrix,Vector3 oVector )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.VectorMatrixMultiply";
			Vector3 oRetVal;
			float nX1 = 0,nX2 = 0,nX3 = 0;

			try
			{
				//Why a *1 on the end you ask?  When donig any kind of 3D work you really are supposed to use Vector4
				//not Vector3.  In that Vector4 the last bit will always be a one.  Why is that you ask?  I have no clue.
				//I assume it has something to do with getting the transpose numbers from a 4x4 matrix.
				nX1 = oMatrix.M11 * oVector.X  +  oMatrix.M21 * oVector.Y  +  oMatrix.M31 * oVector.Z  +  oMatrix.M41 * 1;
				nX2 = oMatrix.M12 * oVector.X  +  oMatrix.M22 * oVector.Y  +  oMatrix.M32 * oVector.Z  +  oMatrix.M42 * 1;
				nX3 = oMatrix.M13 * oVector.X  +  oMatrix.M23 * oVector.Y  +  oMatrix.M33 * oVector.Z  +  oMatrix.M43 * 1;

				oRetVal = new Vector3(nX1,nX2,nX3);
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName  +  " Failed.",oEnX );
			}
			return( oRetVal );
		}
		public static Vector4 MatrixVectorMultiply( Matrix oMatrix,Vector4 oVector )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.VectorMatrixMultiply";
			Vector4 oRetVal;
			float nX1 = 0,nX2 = 0,nX3 = 0,nX4 = 0;

			try
			{
				nX1 = oMatrix.M11 * oVector.X  +  oMatrix.M21 * oVector.Y  +  oMatrix.M31 * oVector.Z  +  oMatrix.M41 * oVector.W;
				nX2 = oMatrix.M12 * oVector.X  +  oMatrix.M22 * oVector.Y  +  oMatrix.M32 * oVector.Z  +  oMatrix.M42 * oVector.W;
				nX3 = oMatrix.M13 * oVector.X  +  oMatrix.M23 * oVector.Y  +  oMatrix.M33 * oVector.Z  +  oMatrix.M43 * oVector.W;
				nX4 = oMatrix.M14 * oVector.X  +  oMatrix.M24 * oVector.Y  +  oMatrix.M34 * oVector.Z  +  oMatrix.M44 * oVector.W;

				oRetVal = new Vector4(nX1,nX2,nX3,nX4);
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName  +  " Failed.",oEnX );
			}
			return( oRetVal );
		}


		//11/07/2003 Chris Hill  This function takes a left-handed view matrix (The kind you make to pass into the 
		//device view matrix) and pulls out the from vector you originally built it from!
		public static Vector3 FromVectorFromALHViewMatrix( Matrix matLHView )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.FromVectorFromALHViewMatrix";
			Matrix matRotFromView;
			Vector3 oFromVector;
			Vector3 oTranslation;

			try
			{
				//First get a rotation matrix... this means stripping off the translation components.
				matRotFromView = matLHView;
				matRotFromView.M41 = 0;
				matRotFromView.M42 = 0;
				matRotFromView.M43 = 0;

				//Now make our from Vector, don't forget to switch the signs.
				oTranslation = new Vector3( -matLHView.M41,-matLHView.M42,-matLHView.M43 );
				oFromVector = DSMath.VectorMatrixMultiply( matRotFromView,oTranslation );
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName + " Failed.",oEnX );
			}
			return( oFromVector );
		}


		//11/03/2003 Chris Hill  This function returns a matrix that will billboard a primitive to face the camera on
		//all three dimensions.  
		public static Matrix BillBoardXYZ( Matrix matView,Matrix matTransRotate )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.BillBoardXYZ";
			Matrix matCopyOfView;
			Matrix matBillboardView;

			try
			{
				//Obtain the view matrix from D3D.				
				matCopyOfView = matView;

				//Elements 41, 42 and 43 (row 4, columns 1, 2 and 3) in the matrix represent the translation, so by
				//setting them to 0 we ensure no translation will occur.
				matCopyOfView.M41 = 0.0f;
				matCopyOfView.M42 = 0.0f;
				matCopyOfView.M43 = 0.0f;

				//Transpose the matrix (basically, switch the rows & columns of the matrix)
				matBillboardView = Matrix.Identity;
				matBillboardView.Transpose( matCopyOfView );

				// Multiply our billboarded view matrix by the world matrix. This ensures that we always face the
				// camera, but still perform world transformations to move objects to the right place.
				matBillboardView *= matTransRotate;
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName  +  " Failed.",oEnX );
			}
			return( matBillboardView );
		}

		public static Matrix BillBoardXYZ( Matrix matView )
		{
			const string sRoutineName = "DarkStrideToolbox.DSMath.BillBoardXYZ";
			Matrix matRetVal;

			try
			{
				matRetVal = BillBoardXYZ( matView,Matrix.Identity );
			}
			catch( System.Exception oEnX )
			{
				throw new System.Exception( sRoutineName  +  " Failed.",oEnX );
			}
			return( matRetVal );
		}

	}
}
