#include <assert.h>
#include <iostream.h>
#include <GL/gl.h>
#include "gprims/vector.H"
#include "glcanvas.H"


// map a 2D screen-space point to a ray through eye, world-space point
static int
map2d ( GLfloat *,                   // inverse of product of proj, model matrices
		int,                         // for lazy inversion of model matrix
		float vpx, float vpy,        // viewport coordinate of mouse event
		float *, float *, float *,   // world coordinate "near" point on ray
		float *, float *, float * ); // world coordinate "far" point on ray

// map a 3D homogeneous clip-space point into a homogeneous world-space point
static void 
map4d ( GLfloat *,
		float cx, float cy, float cz, float cw, // clip coords
		float *wX, float *wY, float *wZ, float *wW ); // world coords, homogeneous

// must be recalled whenever perspective or modeling matrix changes
static int    setup_map2d              ( GLfloat * );

static void   multmatrixMab            ( GLfloat *, GLfloat *, GLfloat * );
static int    invert4d                 ( GLfloat *, GLfloat *);


// changed for OpenGL, hardts
//static GLfloat mat[16];
static short _x1, _y1, _x2, _y2;
static float _xf1, _xf2, _yf1, _yf2;


// M = a b  (actually M = b a  according to OpenGL convention, column major)
// changed for OpenGL, hardts
static void
multmatrixMab( GLfloat * M, GLfloat * a, GLfloat * b )
{
  for( int i = 0; i < 4; i++ ){
    for( int k = 0; k < 4; k++ ){
      M[i*4+k] = 0;
      for( int j = 0; j < 4; j++ ) 
	M[i*4+k] += a[i*4+j] * b[j*4+k];
    }
  }
}


// hard gl selection stuff

/*---------------------------------------------------------------------*/
/* to = from.invert ; invert a matrix - to can be the same as from     */
/* lifted from sgi gl at some point	    			                   */
/*---------------------------------------------------------------------*/
static int
invert4d( GLfloat * to, GLfloat * from )
{
  // Matrix -> (GLfoat *) under OpenGL and m[i][j] -> m[i*4+j].  hardts.

  float wtemp[4][8];
  register float m0,m1,m2,m3,s;
  register float *r0,*r1,*r2,*r3, *rtemp;
  
  r0 = wtemp[0];
  r1 = wtemp[1];
  r2 = wtemp[2];
  r3 = wtemp[3];
  r0[0] = from[0*4+0];		/* build up [A][I]	*/
  r0[1] = from[0*4+1];
  r0[2] = from[0*4+2];
  r0[3] = from[0*4+3];
  r0[4] = 1.0;
  r0[5] = 0.0;
  r0[6] = 0.0;
  r0[7] = 0.0;
  r1[0] = from[1*4+0];
  r1[1] = from[1*4+1];
  r1[2] = from[1*4+2];
  r1[3] = from[1*4+3];
  r1[4] = 0.0;
  r1[5] = 1.0;
  r1[6] = 0.0;
  r1[7] = 0.0;
  r2[0] = from[2*4+0];
  r2[1] = from[2*4+1];
  r2[2] = from[2*4+2];
  r2[3] = from[2*4+3];
  r2[4] = 0.0;
  r2[5] = 0.0;
  r2[6] = 1.0;
  r2[7] = 0.0;
  r3[0] = from[3*4+0];
  r3[1] = from[3*4+1];
  r3[2] = from[3*4+2];
  r3[3] = from[3*4+3];
  r3[4] = 0.0;
  r3[5] = 0.0;
  r3[6] = 0.0;
  r3[7] = 1.0;
  
  if (r0[0] == 0.0) {		/* swap rows if needed		*/
    if (r1[0] == 0.0) {
      if (r2[0] == 0.0) {
	if (r3[0] == 0.0) goto singular;
	rtemp = r0; r0 = r3; r3 = rtemp;
      }
      else {rtemp = r0; r0 = r2; r2 = rtemp;}
    }
    else {rtemp = r0; r0 = r1; r1 = rtemp;}
  }
  m1 = r1[0]/r0[0];		/* eliminate first variable	*/
  m2 = r2[0]/r0[0];
  m3 = r3[0]/r0[0];
  s = r0[1];
  r1[1] = r1[1] - m1 * s;
  r2[1] = r2[1] - m2 * s;
  r3[1] = r3[1] - m3 * s;
  s = r0[2];
  r1[2] = r1[2] - m1 * s;
  r2[2] = r2[2] - m2 * s;
  r3[2] = r3[2] - m3 * s;
  s = r0[3];
  r1[3] = r1[3] - m1 * s;
  r2[3] = r2[3] - m2 * s;
  r3[3] = r3[3] - m3 * s;
  s = r0[4];
  if (s != 0.0) {
    r1[4] = r1[4] - m1 * s;
    r2[4] = r2[4] - m2 * s;
    r3[4] = r3[4] - m3 * s;
  }
  s = r0[5];
  if (s != 0.0) {
    r1[5] = r1[5] - m1 * s;
    r2[5] = r2[5] - m2 * s;
    r3[5] = r3[5] - m3 * s;
  }
  s = r0[6];
  if (s != 0.0) {
    r1[6] = r1[6] - m1 * s;
    r2[6] = r2[6] - m2 * s;
    r3[6] = r3[6] - m3 * s;
  }
  s = r0[7];
  if (s != 0.0) {
    r1[7] = r1[7] - m1 * s;
    r2[7] = r2[7] - m2 * s;
    r3[7] = r3[7] - m3 * s;
  }
  
  if (r1[1] == 0.0) {		/* swap rows if needed		*/
    if (r2[1] == 0.0) {
      if (r3[1] == 0.0) goto singular;
      rtemp = r1; r1 = r3; r3 = rtemp;
    }
    else {rtemp = r1; r1 = r2; r2 = rtemp;}
  }
  m2 = r2[1]/r1[1];		/* eliminate second variable	*/
  m3 = r3[1]/r1[1];
  r2[2] = r2[2] - m2 * r1[2];
  r3[2] = r3[2] - m3 * r1[2];
  r3[3] = r3[3] - m3 * r1[3];
  r2[3] = r2[3] - m2 * r1[3];
  s = r1[4];
  if (s != 0.0) {
    r2[4] = r2[4] - m2 * s;
    r3[4] = r3[4] - m3 * s;
  }
  s = r1[5];
  if (s != 0.0) {
    r2[5] = r2[5] - m2 * s;
    r3[5] = r3[5] - m3 * s;
  }
  s = r1[6];
  if (s != 0.0) {
    r2[6] = r2[6] - m2 * s;
    r3[6] = r3[6] - m3 * s;
  }
  s = r1[7];
  if (s != 0.0) {
    r2[7] = r2[7] - m2 * s;
    r3[7] = r3[7] - m3 * s;
  }
  
  if (r2[2] == 0.0) {		/* swap last 2 rows if needed	*/
    if (r3[2] == 0.0) goto singular;
    rtemp = r2; r2 = r3; r3 = rtemp;
  }
  m3 = r3[2]/r2[2];		/* eliminate third variable	*/
  r3[3] = r3[3] - m3 * r2[3];
  r3[4] = r3[4] - m3 * r2[4];
  r3[5] = r3[5] - m3 * r2[5];
  r3[6] = r3[6] - m3 * r2[6];
  r3[7] = r3[7] - m3 * r2[7];
  
  if (r3[3] == 0.0) goto singular;
  s = 1.0/r3[3];		/* now back substitute row 3	*/
  r3[4] = r3[4] * s;
  r3[5] = r3[5] * s;
  r3[6] = r3[6] * s;
  r3[7] = r3[7] * s;
  
  m2 = r2[3];			/* now back substitute row 2	*/
  s = 1.0/r2[2];
  r2[4] = s * (r2[4] - r3[4] * m2);
  r2[5] = s * (r2[5] - r3[5] * m2);
  r2[6] = s * (r2[6] - r3[6] * m2);
  r2[7] = s * (r2[7] - r3[7] * m2);
  m1 = r1[3];
  r1[4] = (r1[4] - r3[4] * m1);
  r1[5] = (r1[5] - r3[5] * m1);
  r1[6] = (r1[6] - r3[6] * m1);
  r1[7] = (r1[7] - r3[7] * m1);
  m0 = r0[3];
  r0[4] = (r0[4] - r3[4] * m0);
  r0[5] = (r0[5] - r3[5] * m0);
  r0[6] = (r0[6] - r3[6] * m0);
  r0[7] = (r0[7] - r3[7] * m0);
  
  m1 = r1[2];			/* now back substitute row 1	*/
  s = 1.0/r1[1];
  r1[4] = s * (r1[4] - r2[4] * m1);
  r1[5] = s * (r1[5] - r2[5] * m1);
  r1[6] = s * (r1[6] - r2[6] * m1);
  r1[7] = s * (r1[7] - r2[7] * m1);
  m0 = r0[2];
  r0[4] = (r0[4] - r2[4] * m0);
  r0[5] = (r0[5] - r2[5] * m0);
  r0[6] = (r0[6] - r2[6] * m0);
  r0[7] = (r0[7] - r2[7] * m0);
  
  m0 = r0[1];			/* now back substitute row 0	*/
  s = 1.0/r0[0];
  r0[4] = s * (r0[4] - r1[4] * m0);
  r0[5] = s * (r0[5] - r1[5] * m0);
  r0[6] = s * (r0[6] - r1[6] * m0);
  r0[7] = s * (r0[7] - r1[7] * m0);
  
  to[0*4+0] = r0[4];		/* copy results back		*/
  to[0*4+1] = r0[5];
  to[0*4+2] = r0[6];
  to[0*4+3] = r0[7];
  to[1*4+0] = r1[4];
  to[1*4+1] = r1[5];
  to[1*4+2] = r1[6];
  to[1*4+3] = r1[7];
  to[2*4+0] = r2[4];
  to[2*4+1] = r2[5];
  to[2*4+2] = r2[6];
  to[2*4+3] = r2[7];
  to[3*4+0] = r3[4];
  to[3*4+1] = r3[5];
  to[3*4+2] = r3[6];
  to[3*4+3] = r3[7];
  return 1;
  
 singular:
  cout << "ui::invert4d(): SINGULAR!" << endl;
  return 0;
}


// capture current transform [in mat],
// current viewport [in _x*, _y*]
// this should be called lazily whenever
// gl's modeling/persp matrix changes
static int
setup_map2d( GLfloat *mat )
{
  // changed for OpenGL, hardts
  GLfloat mp[16], mv[16], mpv[16];
  
  // get proj matrix
  glGetFloatv( GL_PROJECTION_MATRIX,  mp );
  
  // get model matrix
  glGetFloatv( GL_MODELVIEW_MATRIX,  mv );
  
#if 0
  cout<<"\nprojection....\n";
  for( int i=0; i < 4; ++i ) {
    for( int j=0; j < 4; ++j ) { 
      cout<< mp[ (i*4)+ j] << " ";
    }
    cout<<endl;cout.flush();
  }

  cout<<"\nmodelview....\n";
  for( i=0; i < 4; ++i ) {
    for( int j=0; j < 4; ++j ) {
      cout<< mv[ (i*4)+ j] << " ";
    }
    cout<<endl;cout.flush();
  }

  cout<<"\n\n";cout.flush();
#endif

  // product mpv = p.v
  multmatrixMab( mpv, mv, mp );

  // invert mpv into mat, to be stored by caller
  if( !invert4d( mat, mpv ) )
    { cout << "singular matrix!" << endl; return 0; }

  GLint vPort[4]; // x, y, w, h
  glGetIntegerv( GL_VIEWPORT, vPort ); 

  // center of pixel at lower left
  _x1 = vPort[0];                // x1
  _y1 = vPort[1];                // y1
  // center of pixel at upper right
  _x2 = vPort[0] + vPort[2] - 1; // x2
  _y2 = vPort[1] + vPort[3] - 1; // y2

  // floating coords of lower-left corner of lower-left pixel
  _xf1 = _x1 - 0.5f;
  _yf1 = _y1 - 0.5f;
  // floating coords of upper-right corner of upper-right pixel
  _xf2 = _x2 + 0.5f;
  _yf2 = _y2 + 0.5f;

  if( _xf1 >= _xf2 ){ cout << "ZERO x VWPT!" << endl; return 0; }
  if( _yf1 >= _yf2 ){ cout << "ZERO y VWPT!" << endl; return 0; }

  return 1;
}


// map mouse position to a line in world coordinates
// returns 1 if matrix is non-singular; 0 otherwise
// wx1,wy1,wz1 is returned as world-space eye position
// wx2,wy2,wz2 is returned as a point on eye-mouse line
static int
map2d( GLfloat *mat,
       int viewChange,
       float vpx, float vpy, // viewport relative
       float *wx1, float *wy1, float *wz1,
       float *wx2, float *wy2, float *wz2 )
{
  if( viewChange )
    if ( !setup_map2d( mat ) )
      return 0;

  // invert point [ 0 0 -1 0 ], i.e., z = -infinity,
  // to find the eyepoint in world coordinates
  float w1, w2;
  map4d ( mat, 0, 0, -1, 0, wx1, wy1, wz1, &w1 );

  // convert to clip coordinates, i.e., [ -1 <= clipx,y <= +1 ]
  // invert [ clipx clipy 0 1 ] to find "far" point on line
  map4d ( mat,
	  ( 2 * vpx - _xf1 - _xf2 ) / ( _xf2 - _xf1 ),	// clipx
	  ( 2 * vpy - _yf1 - _yf2 ) / ( _yf2 - _yf1 ),	// clipy
		  -0.5,						                // clipz == 0.5 
		  1,                                        // clipw == 1
	  wx2, wy2, wz2, &w2 );	// world coords

  // project far point (should not be ideal)
  if ( w2 != 0 ) {
	*wx2 /= w2;
	*wy2 /= w2;
	*wz2 /= w2;
    }
  else {
    cout << "map2d(): far point is ideal (w == 0.0)!" << endl;
	return 0;
    }

  // if eyepoint is local, project it to a 3D point
  if ( w1 != 0 ) {
	*wx1 /= w1;
	*wy1 /= w1;
	*wz1 /= w1;
    }
  else { // otherwise, eyepoint is ideal (i.e., at infinity)
	// interpret eye as direction d; generate new eye as "far + d"
	*wx1 = *wx2 + *wx1;
	*wy1 = *wy2 + *wy1;
	*wz1 = *wz2 + *wz1;
    }

  // success
  return 1;
}


// transform a homogeneous point from clip coordinates to world coordinates
static void
map4d( GLfloat *mat,
       float cx, float cy, float cz, float cw,        // clip coords
       float *wX, float *wY, float *wZ, float *wW )   // world coords
{
  // With OpenGL, mat[i][j] -> mat[i*4+j].  hardts.

  *wX = (cx * mat[0*4+0] + cy * mat[1*4+0] + cz * mat[2*4+0] + cw * mat[3*4+0]);
  *wY = (cx * mat[0*4+1] + cy * mat[1*4+1] + cz * mat[2*4+1] + cw * mat[3*4+1]);
  *wZ = (cx * mat[0*4+2] + cy * mat[1*4+2] + cz * mat[2*4+2] + cw * mat[3*4+2]);
  *wW = (cx * mat[0*4+3] + cy * mat[1*4+3] + cz * mat[2*4+3] + cw * mat[3*4+3]);
}

// these are in eyemouse.C for convenience.  rayAction() maps screen-space
// mouse events to world-space rays, and passes them back to the user.
//
// return 1 iff redraw is requested by user call
int
DisplayGLCanvas::rayAction( PMOUSEINFO pMI, PHANDLE pHandle ) { 
  
  float eye[3]; 
  float far[3];

  // map 2D screen-space point into 3D world-space ray
  if (! map2d( _MPinv, 
	       _viewChange,
	       float( pMI->curX ), float( pMI->curY ),
	       &eye[0], &eye[1], &eye[2],
	       &far[0], &far[1], &far[2] ) ) 
	return 0;

  _viewChange = 0;

  // call user action for this event; supply eye-far ray 
  int retval = UserAction ( Point3(&eye[0]), 
			    Point3(&far[0]), 
			    pMI, 
			    pHandle );
  return  ( retval );
}

// virtual, to be overloaded by user
PHANDLE    
DisplayGLCanvas::UserHandle  ( void  )  
{ 
  return NULL; 
}

// virtual, to be overloaded by user 
int 
DisplayGLCanvas::UserAction( const Point3&, const Point3&, 
			     PMOUSEINFO, PHANDLE )
{
  return 0;
}