#pragma once
#include <vector>
#include "Vector.h"

namespace Aftr
{
   /**
      Householder reduction of real, symmetric matrix a[n][n].

      Input - real symmetric matrix a[n-1][n-1]
      Input - n, size of matrix
      Input - d[n-1], storage
      Input - e[n-1], storage

      Output - replaces a by the orthogonal matrix Q effecting the transformation.
      Output - d[n-1] contains diagonal elements
      Output - e[n-1] contains off-diagonal elements (e[0] = 1)

      taken from Numerical Recipes in C
   **/
   void HouseholderReduction( float** a, int n, float d[], float e[] );

   /**
      QL algorithm of a tri-diagonal matrix a[n-1][n-1].

      Input - z, Identity Matrix, or tridiagonal matrix
      Input - d[n-1], main diagonal of symmetric tridiagonal matrix
      Input - e[n-1], off diagnonal of symmetric tridiagonal matrix

      Output - z the eigenvectors with column k corresponding to eigenvalue d[k]
      Output - d the eigenvalues of a
   **/
   void QL( float d[], float e[], int n, float** z );

   /**
      Will generate a 4x4 matrix containing:
      x-basis vector = x
      z-basis vector = the vector in the plane formed by the x and z vectors, orthogonal to x, and in the same direction of z
      y-basis vector = z-basis vector cross x-basis vector
   **/
   void generateMatrixAbsoluteLookGivenUp( Vector x, Vector z, float* m );

   /**
      Will generate a 4x4 matrix containing:
      z-basis vector = z
      x-basis vector = the vector in the plane formed by the x and z vectors, orthogonal to z, and in the same direction of x
      y-basis vector = z-basis vector cross x-basis vector
   **/
   void generateMatrixAbsoluteUpGivenLook( Vector z, Vector x, float* m );

   /**
      This method is an exact reimplementation of gluLookAt. This exists because Open GL 1.1
      ES does not have a glu library implemented. Hence the Aftr class re implements this method.
      Usage of this function should be used identically to gluLookAt. That is, one can simply
      replace all calls to GLU's gluLookAt with aftrGluLookAt, of course, the eye, center, and
      up parameters use the Aftr Engine's Vector class instead of passing 9 floats.


      gluLookAt creates a viewing matrix derived from an eye point, a reference point indicating the
      center of the scene, and an UP vector.

      The matrix maps the reference point to the negative z axis and the eye point to the origin. When
      a typical projection matrix is used, the center of the scene therefore maps to the center of the viewport. Similarly,
      the direction described by the UP vector projected onto the viewing plane is mapped to the
      positive y axis so that it points upward in the viewport. The UP vector
      must not be parallel to the line of sight from the eye point to the reference point.
   */
   void aftrGluLookAt( const Vector& eye, const Vector& center, const Vector& up );

   /**
      Helper method called by aftrGluLookAt. Populates the output 4x4 matrix to the current
      Open GL View Matrix. This method does not modify any OpenGL state.
   */
   void aftrComputeViewMatrix( const Vector& eye, const Vector& center, const Vector& up, float* out4x4ViewMatrix );

   /**
      This method is an exact reimplementation of gluPerspective. This exists because Open GL 1.1
      ES does not have a glu library implemented. Hence the Aftr reimplements this method.
      Usage of this function should be used identically to gluPerspective. That is, one can simply
      replace all calls to GLU's gluPerspective with aftrGluPerspective.

      This will accumulate the calculated projection matrix into the current matrix. Typically, the
      user would do something like:
      glMatrixMode( GL_PROJECTION );
      glLoadIdentity();
      aftrGluPerspective( fovyInDegrees, aspect, zNear, zFar );
      glMatrixMode( GL_MODELVIEW );
      ...

      gluPerspective specifies a viewing frustum into the world coordinate system.
      In general, the aspect ratio in gluPerspective should match the aspect ratio
      of the associated viewport. For example, aspect = 2.0 means  the viewer's
      angle of view is twice as wide in x as it is in y.
      If the viewport is twice as wide as it is tall, it displays the image without distortion.

      The matrix generated by gluPerspective is multiplied by the current matrix,
      just as if glMultMatrix were called with the generated matrix.
      To load the perspective matrix onto the current matrix stack instead,
      precede the call to gluPerspective with a call to glLoadIdentity.
   */
   void aftrGluPerspective( float fovyInDegrees, float aspect, float zNear, float zFar );

   /**
      Identical to aftrGluPerspective with one exception - instead of manipulating the current OpenGL
      state (via glMultMatrixf(projMat)), the computed perspective projection matrix is returned
      via the outProjectionMatrix parameter.

      This method is used by the camera to update its perspective projection matrix without changing
      any OpenGL state.
   */
   void aftrGluPerspectiveNoGLStateModification( float fovyInDegrees, float aspect, float zNear, float zFar, float outProjectionMatrix[] );

   /**
      Inverts a 4x4 matrix by pivoting on largest element in column, returns false if matrix is singular.
   */
   template< typename T >
   bool aftrGluInvertMatrix( const T src[16], T inverse[16] );
   //template bool aftrGluInvertMatrix< float >( const float src[16], float inverse[16] );
   //template bool aftrGluInvertMatrix< double >( const double src[16], double inverse[16] );

   /**
      Creates a projection matrix that can be used to restrict drawing to a small region of the viewport.
      This is typically useful to determine what objects are being drawn near the cursor.
      x,y - Specify the center of a picking region in window coordinates.
      width, height - Specify the width and height, respectively, of the picking region in window coordinates.
      viewport - Specifies the current viewport.
         viewport[0,1]=x,y - Specify the lower left corner of the viewport rectangle, in pixels.
         viewport[2,3]=width,height - Specify the width and height of the viewport, in pixels.

   */
   template< typename T > Mat4T<T> aftrGluPickMatrix( T x, T y, T width, T height, int viewport[4] );
   //template Mat4T<float> aftrGluPickMatrix<float>( float x, float y, float width, float height, int viewport[4] ); //explicit template instantiation
   //template Mat4T<double> aftrGluPickMatrix<double>( double x, double y, double width, double height, int viewport[4] ); //explicit template instantiation

   template< typename T >
   bool aftrGluUnProject( T winx, T winy, T winz, const T modelViewMatrix[16], const T projMatrix[16], const int viewport[4],
      T* objSpaceX, T* objSpaceY, T* objSpaceZ );
   //template bool aftrGluUnProject<float>( float winx, float winy, float winz, const float modelViewMatrix[16], const float projMatrix[16], const int viewport[4], float* objSpaceX, float* objSpaceY, float* objSpaceZ );
   //template bool aftrGluUnProject< double >( double winx, double winy, double winz, const double modelViewMatrix[16], const double projMatrix[16], const int viewport[4], double* objSpaceX, double* objSpaceY, double* objSpaceZ );

   void aftrGluOrtho2D( float left, float right, float bottom, float top );

   /**
      Identical to aftrGluOrtho2D with one exception - instead of manipulating the current OpenGL
      state (via glMultMatrixf(projMat)), the computed perspective projection matrix is returned
      via the outProjectionMatrix parameter.

      This method is used by the camera to update its perspective projection matrix without changing
      any OpenGL state.

      A common set of input parameters would be left=0, right=1, bottom=0, top=1, near=-1, far=1.
   */
   void aftrGluOrtho2DNoGLStateModification( float left, float right, float bottom, float top, float near, float far, float outProjectionMatrix[] );

   /**
      Perform Gauss-Jordan elimination with row-pivoting to obtain the solution to
      the system of linear equations. Given A and B, solve for X. All matrices are
      the same dimensionality.

      A X = B

      Arguments:
      lhs      - left-hand side of the equation, matrix A
      rhs      - right-hand side of the equation, matrix B
      nrows    - number of rows in the arrays lhs and rhs
      ncolsrhs - number of columns in the array rhs

      The function uses Gauss-Jordan elimination with pivoting.  The solution X to
      the linear system winds up stored in the array rhs; create a copy to pass to
      the function if you wish to retain the original RHS array.

      Passing the identity matrix as the rhs argument results in the inverse of
      matrix A, if it exists.

      Code originally from http://www.dreamincode.net/code/snippet1312.htm
*/
   template< typename T >
   bool gaussianJordanElimination( T** lhs, T** rhs, int nrows, int ncolsrhs );

   /// Helper function called by gaussianJordanElimination
   template< typename T >
   void gaussianJordanEliminationSwapRows( T** arr, int row0, int row1 );

   /*******************************
   Matrix Methods
   *******************************/

/**
   This method multiplies two column major matrices such that a := a * b. That is,
   "a" accumulates the result of "a" post-multiplied onto "b". This is used
   as multiplication for rotation and scaling manipulations. The two
   matrices MUST BOTH be 4x4 in size. This is used instead of OpenGL's
   internal methods so this code is ANSI C++ and multi-platform.

   This performs post multiplication using column major indexing.
*/
   template< typename T >
   void multiply4x4Matrix( T a[16], const T b[16] );
   //template void multiply4x4Matrix< float >( float a[16], const float b[16] ); //explicit template instantiation
   //template void multiply4x4Matrix< double >( double a[16], const double b[16] ); //explicit template instantiation

   /**
      This method multiplies two matrices such that result := a * b.
      This performs pre multiplication.

      If you'd like to perform the accumulation of two column major
      4x4 matrices (like OpenGL matrices) do the following:
      PV <- P * V, then call (post multiplication of column-major matrices)
      multiply4x4Matrix( V, P, PV ); // PV now stores P * V (post multiplied),
                                     // which is the same as V * P (pre multiplied)
   */
   template< typename T >
   void multiply4x4Matrix( const T a[16], const T b[16], T result[16] );
   //template void multiply4x4Matrix<float>( const float a[16], const float b[16], float result[16] );
   //template void multiply4x4Matrix<double>( const double a[16], const double b[16], double result[16] );

   /**
      Multiplies 4x1 point v by 4x4 matrix a storing the result in 4x1 point out.
   */
   template< typename T >
   void multiply4x4MatrixBy4x1Vector( const T a[16], const T v[4], T out[4] );
   //template void multiply4x4MatrixBy4x1Vector<float>( const float a[16], const float v[4], float out[4] );
   //template void multiply4x4MatrixBy4x1Vector<double>( const double a[16], const double v[4], double out[4] );

   /**
      Returns a column major 4x4 matrix where columns 0, 1, 2 correspond to xdir, ydir, zdir, respectively.
      The 4th column and row are set to 0,0,0,1.
   */
   void make4x4MatrixFrom3Vectors( const Vector& xdir, const Vector& ydir, const Vector& zdir, float m[16] );
   void make3x3MatrixFrom3Vectors( const Vector& xdir, const Vector& ydir, const Vector& zdir, float m[9] );

   /**
      This method finds the 4x4 matrix such that the upper left 3x3 matrix
      is the proper rotation matrix to rotate a point at displacement a from
      the origin to displacement b from the origin and stores it in m. Upper
      left 3x3 described above, m[15] == 0, all remaining elements are 0.
   */
   void determineRotationMatrix( Vector source, Vector destination, float r[16] );

   /**
      This method populates a float[16] from a Vector containing the roll (positive rotation about the X axis),
      pitch (positive rotation about the Y axis), and yaw (positive rotation about the Z axis)
      in the x,y,z components of the Vector, respectively.
   */
   void eulerAnglesToDirectionCosineMatrix( Vector euler, float displayMat[16] );

   /**
      Vector = original * matrix.
      Where Vector is a 3D Aftr Vector, original is the Aftr Vector to be transformed
      by the 4x4 matrix.
   */
   Vector transformVectorThrough4x4Matrix( const Vector& original, const float m[16] );

   /**
      outVec4D = inVec4D * m.
      Where outVec4D and inVec4D correspond to an array of 4 floats. inVec4D is transformed
      by matrix and the result is stored in outVec4D.
   */
   template< typename T >
   void transformVector4DThrough4x4Matrix( const T inVec4D[4], T outVec4D[4], const T m[16] );
   //template void transformVector4DThrough4x4Matrix< float >( const float inVec4D[4], float outVec4D[4], const float m[16] );
   //template void transformVector4DThrough4x4Matrix< double >( const double inVec4D[4], double outVec4D[4], const double m[16] );

    /**
      outVec3D[0..2] = [inVec3D,1] * m.
      Where outVec4D and inVec4D correspond to an array of 4 floats. inVec4D is transformed
      by matrix and the result is stored in outVec4D.
   */
   void transformVector3DThrough4x4Matrix( const float inVec3D[3], float outVec3D[3], const float m[16] );


   /**
      Note: Transposes the elements of the two in passed 4x4 matrices. The inMatrix is
      internally copied to a temporary matrix, so it is completely correct to pass the
      same matrix in as both the in and out matrix - it will still be transposed correctly.
   */
   template< typename T>
   void transpose4x4Matrix( const T inMatrix[], T outMatrix[] );
   //template void transpose4x4Matrix<float>( const float inMatrix[], float outMatrix[] );
   //template void transpose4x4Matrix<double>( const double inMatrix[], double outMatrix[] );

   /**
      This method initializes the in passed display matrix to the identity
      matrix.
   */
   template< typename T >
   void aftrSetIdentityMatrix( T matrix[16] );
   //template void aftrSetIdentityMatrix<float>( float matrix[16] );
   //template void aftrSetIdentityMatrix<double>( double matrix[16] );

   std::string print4x4MatrixColumn( const float* m );
   std::string print4x4MatrixColumn( const double* m );
   std::string print4x4MatrixRow( const float* m );
   std::string printMxNMatrixRow( const float* matrix, int m, int n );
   std::string printMxNMatrixRow( const float** matrix, int m, int n );

   std::string print4x4MatrixColumnWolframAlpha( const double* m );
   std::string print4x4MatrixColumnMatlab( const double* m );

   /// Returns the current MODELVIEW_MATRIX on the OpenGL Stack.
   /// This sets the OpenGL MatrixMode to GL_MODELVIEW upon exit.
   std::string printOpenGLMODELVIEWMatrix();

   /// Returns the current PROJECTION_MATRIX on the OpenGL Stack.
   /// This sets the OpenGL MatrixMode to GL_MODELVIEW upon exit.
   std::string printOpenGLPROJECTIONMatrix();


   /*************************************
   End Matrix Methods
   *************************************/

} //namespace Aftr