// nUtil - An utility Library for gnurbs
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

#include "nglu.h"

#include <cmath>
#include <cstring>

int _gluUnProject ( GLdouble winx, GLdouble winy, GLdouble winz,
                        const GLdouble modelMatrix[16],
                        const GLdouble projMatrix[16],
                        const GLint viewport[4],
                        GLdouble *objx, GLdouble *objy, GLdouble *objz, GLdouble Inv[4][4] )
{
  double finalMatrix[16];
  double in[4];
  double out[4];

  _gluMultMatricesd ( modelMatrix, projMatrix, finalMatrix );

  if ( !_gluInvertMatrixd ( finalMatrix, finalMatrix ) ) return ( GL_FALSE );

  in[0]=winx;
  in[1]=winy;
  in[2]=winz;
  in[3]=1.0;

  /* Map x and y from window coordinates */
  in[0] = ( in[0] - viewport[0] ) / viewport[2];
  in[1] = ( in[1] - viewport[1] ) / viewport[3];

  /* Map to range -1 to 1 */
  in[0] = in[0] * 2 - 1;
  in[1] = in[1] * 2 - 1;
  in[2] = in[2] * 2 - 1;

  _gluMultMatrixVecd ( finalMatrix, in, out );

  if ( out[3] == 0.0 ) return ( GL_FALSE );

  out[0] /= out[3];
  out[1] /= out[3];
  out[2] /= out[3];
  *objx = out[0];
  *objy = out[1];
  *objz = out[2];
  return ( GL_TRUE );
}

int _gluUnProjectFast ( GLdouble winx, GLdouble winy, GLdouble winz,
                            const GLdouble Inv[16],
                            const GLint viewport[4],
                            GLdouble *objx, GLdouble *objy, GLdouble *objz )
{
  double in[4];
  double out[4];
  in[0]=winx;  in[1]=winy;  in[2]=winz;  in[3]=1.0;

  /* Map x and y from window coordinates */
  in[0] = ( in[0] - viewport[0] ) / viewport[2];
  in[1] = ( in[1] - viewport[1] ) / viewport[3];

  /* Map to range -1 to 1 */
  in[0] = in[0] * 2 - 1;  in[1] = in[1] * 2 - 1;  in[2] = in[2] * 2 - 1;

  _gluMultMatrixVecd ( Inv, in, out );

  if ( out[3] == 0.0 ) return ( GL_FALSE );

  out[0] /= out[3];  out[1] /= out[3];  out[2] /= out[3];
  *objx = out[0];  *objy = out[1];  *objz = out[2];
  return ( GL_TRUE );
}

int _gluProjectFast (GLdouble objx, GLdouble objy, GLdouble objz,
                            const GLdouble Mat[16],
                            const GLint viewport[4],
                            GLdouble *winx, GLdouble *winy, GLdouble *winz)
{
  double in[4];
  double out[4];
  in[0]=objx;  in[1]=objy;  in[2]=objz;  in[3]=1.0;
  _gluMultMatrixVecd ( Mat, in, out );
  if ( out[3] == 0.0 ) return ( GL_FALSE );
  out[0] /= out[3];  out[1] /= out[3];  out[2] /= out[3];
  out[0] = (out[0]+1) /2;  out[1] = (out[1]+1) /2; out[2] = (out[2]+1) /2;
  out[0] = out[0]*viewport[2]+ viewport[0];
  out[1] = out[1]*viewport[3]+ viewport[1];
  *winx = out[0];  *winy = out[1];  *winz = out[2];
  return ( GL_TRUE );
  
}



void _gluMultMatricesd ( const GLdouble a[16], const GLdouble b[16],GLdouble r[16] )
{
  for (int i = 0; i < 4; i++ )
    for (int j = 0; j < 4; j++ )
      r[i*4+j] =  a[i*4+0]*b[0*4+j] +  a[i*4+1]*b[1*4+j] +  a[i*4+2]*b[2*4+j] +  a[i*4+3]*b[3*4+j];
}


void _gluMultMatrixVecd ( const GLdouble matrix[16], const GLdouble in[4],GLdouble out[4] )
{

  for (int i=0; i<4; i++ )
    out[i] =      in[0] * matrix[0*4+i] +      in[1] * matrix[1*4+i] +      in[2] * matrix[2*4+i] +      in[3] * matrix[3*4+i];
}

int _gluInvertMatrixd ( const GLdouble src[16], GLdouble inverse[16] )
{
  int i, j, k, swap;
  GLdouble t;
  GLdouble temp[4][4];

  for ( i=0; i<4; i++ )
    for ( j=0; j<4; j++ )
      temp[i][j] = src[i*4+j];

  _gluMakeIdentityd ( inverse );

  /*
  ** Look for largest element in column
  */
  for ( i = 0; i < 4; i++ )
  {
    swap = i;
    for ( j = i + 1; j < 4; j++ )
      if ( fabs ( temp[j][i] ) > fabs ( temp[i][i] ) )
        swap = j;

    /*
    ** Swap rows.
    */
    if ( swap != i )
    {
      for ( k = 0; k < 4; k++ )
      {
        t = temp[i][k];
        temp[i][k] = temp[swap][k];
        temp[swap][k] = t;

        t = inverse[i*4+k];
        inverse[i*4+k] = inverse[swap*4+k];
        inverse[swap*4+k] = t;
      }
    }

    /*
    ** No non-zero pivot. The matrix is singular, which shouldn't
    ** happen. This means the user gave us a bad matrix.
    */
    if ( temp[i][i] == 0 )
      return GL_FALSE;

    t = temp[i][i];

    for ( k = 0; k < 4; k++ )
    {
      temp[i][k] /= t;
      inverse[i*4+k] /= t;
    }

    for ( j = 0; j < 4; j++ )
    {
      if ( j != i )
      {
        t = temp[j][i];
        for ( k = 0; k < 4; k++ )
        {
          temp[j][k] -= temp[i][k]*t;
          inverse[j*4+k] -= inverse[i*4+k]*t;
        }
      }
    }
  }

  return GL_TRUE;
}

void _gluMakeIdentityd ( GLdouble m[16] )
{
  m[0+4*0] = 1.;  m[0+4*1] = 0.;  m[0+4*2] = 0.;  m[0+4*3] = 0.;
  m[1+4*0] = 0.;  m[1+4*1] = 1.;  m[1+4*2] = 0.;  m[1+4*3] = 0.;
  m[2+4*0] = 0.;  m[2+4*1] = 0.;  m[2+4*2] = 1.;  m[2+4*3] = 0.;
  m[3+4*0] = 0.;  m[3+4*1] = 0.;  m[3+4*2] = 0.;  m[3+4*3] = 1.;
}


//matrix will receive the calculated perspective matrix.
//You would have to upload to your shader
// or use glLoadMatrixf if you aren't using shaders.
void _gluPerspective ( GLdouble fovyInDegrees, GLdouble aspectRatio,
                           GLdouble znear, GLdouble zfar )
{
  GLdouble ymax, xmax;
  GLdouble temp, temp2, temp3, temp4;
  ymax = znear * tanf ( fovyInDegrees * M_PI / 360.0 );
  //ymin = -ymax;
  //xmin = -ymax * aspectRatio;
  xmax = ymax * aspectRatio;
//    gray_gluFrustum(matrix, -xmax, xmax, -ymax, ymax, znear, zfar);
  glFrustum ( -xmax, xmax, -ymax, ymax, znear, zfar );
}

void  _gluFrustum ( GLdouble *matrix, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top,
                        GLdouble znear, GLdouble zfar )
{
  GLdouble temp, temp2, temp3, temp4;
  temp = 2.0 * znear;
  temp2 = right - left;
  temp3 = top - bottom;
  temp4 = zfar - znear;
  matrix[0] = temp/temp2;               matrix[1] = 0.0;                matrix[2] = 0.0;                        matrix[3] = 0.0;
  matrix[4] = 0.0;                      matrix[5] = temp / temp3;       matrix[6] = 0.0;                        matrix[7] = 0.0;
  matrix[8] = (right+left)/temp2;       matrix[9] = (top+bottom)/temp3; matrix[10] = (-zfar-znear)/temp4;       matrix[11] = -1.0;
  matrix[12] = 0.0;                     matrix[13] = 0.0;               matrix[14] = (-temp*zfar)/temp4;        matrix[15] = 0.0;
}

inline void ComputeNormalOfPlane ( GLdouble *normal, const GLdouble *pvector1, const GLdouble *pvector2 )
{
  normal[0]= ( pvector1[1]*pvector2[2] )- ( pvector1[2]*pvector2[1] );
  normal[1]= ( pvector1[2]*pvector2[0] )- ( pvector1[0]*pvector2[2] );
  normal[2]= ( pvector1[0]*pvector2[1] )- ( pvector1[1]*pvector2[0] );
}

inline void NormalizeVector ( GLdouble *pvector )
{
  GLdouble normalizingConstant;
  normalizingConstant=1.0/sqrt ( pvector[0]*pvector[0]+pvector[1]*pvector[1]+pvector[2]*pvector[2] );
  pvector[0]*=normalizingConstant;
  pvector[1]*=normalizingConstant;
  pvector[2]*=normalizingConstant;
}

void glhTranslated2 ( GLdouble *matrix, GLdouble x, GLdouble y, GLdouble z )
{
  matrix[12]=matrix[0]*x+matrix[4]*y+matrix[8]*z+matrix[12];
  matrix[13]=matrix[1]*x+matrix[5]*y+matrix[9]*z+matrix[13];
  matrix[14]=matrix[2]*x+matrix[6]*y+matrix[10]*z+matrix[14];
  matrix[15]=matrix[3]*x+matrix[7]*y+matrix[11]*z+matrix[15];
}

void _gluLookAt ( GLdouble *eyePosition3D,
                      GLdouble *center3D, GLdouble *upVector3D )
{
  GLdouble matrix[16];
  GLdouble forward[3], side[3], up[3];
  GLdouble matrix2[16], resultMatrix[16];
  _gluMakeIdentityd ( matrix );
  //------------------
  forward[0] = center3D[0] - eyePosition3D[0];
  forward[1] = center3D[1] - eyePosition3D[1];
  forward[2] = center3D[2] - eyePosition3D[2];
  NormalizeVector ( forward );
  //------------------
  //Side = forward x up
  ComputeNormalOfPlane ( side, forward, upVector3D );
  NormalizeVector ( side );
  //------------------
  //Recompute up as: up = side x forward
  ComputeNormalOfPlane ( up, side, forward );
  //------------------
  matrix2[0] = side[0]; matrix2[4] = side[1];  matrix2[8] = side[2];  matrix2[12] = 0.0;
  //------------------
  matrix2[1] = up[0];   matrix2[5] = up[1];  matrix2[9] = up[2];  matrix2[13] = 0.0;
  //------------------
  matrix2[2] = -forward[0];  matrix2[6] = -forward[1];  matrix2[10] = -forward[2];  matrix2[14] = 0.0;
  //------------------
  matrix2[3] = matrix2[7] = matrix2[11] = 0.0;  matrix2[15] = 1.0;
  //------------------
  _gluMultMatricesd ( matrix, matrix2,resultMatrix );
  glhTranslated2 ( resultMatrix, -eyePosition3D[0], -eyePosition3D[1], -eyePosition3D[2] );
  //------------------
  memcpy ( matrix, resultMatrix, 16*sizeof ( GLdouble ) );
  glLoadMatrixd ( matrix );
}