#include <cassert>
#include <cstdlib>
#include <cmath>
#include <memory>
#include <cstring>
#include <iostream>
using namespace std;

#include <GL/glut.h>

#include <zpr.h>


/** A measure of zero for singularity test. */
GLdouble zprGLmatrix::zero = 1.0E-15;

/** Global singelton pointer initially set to zero. */
zpr* zpr::global = 0;

#define SHOW(x) #x << '=' << (x)


//
// From Mesa-2.2\src\glu\project.c
//

//
// Compute the inverse of a 4x4 matrix.  Contributed by scotter@lafn.org
//

void zprGLmatrix::invertMatrix(GLdouble *out, GLdouble const *m )
{

/* NB. OpenGL Matrices are COLUMN major. */
#define MAT(m,r,c) (m)[(c)*4+(r)]

// <TODO> Renaming the indexes??? Do they have a problem counting from 0? Why?
/* Here's some shorthand converting standard (row,column) to index. */
#define m11 MAT(m,0,0)
#define m12 MAT(m,0,1)
#define m13 MAT(m,0,2)
#define m14 MAT(m,0,3)
#define m21 MAT(m,1,0)
#define m22 MAT(m,1,1)
#define m23 MAT(m,1,2)
#define m24 MAT(m,1,3)
#define m31 MAT(m,2,0)
#define m32 MAT(m,2,1)
#define m33 MAT(m,2,2)
#define m34 MAT(m,2,3)
#define m41 MAT(m,3,0)
#define m42 MAT(m,3,1)
#define m43 MAT(m,3,2)
#define m44 MAT(m,3,3)

  static GLdouble temporary[16]; /* Allow out == in. */
  GLdouble * tmp = out;

  bool const outin(m==out);
  if (outin)
    tmp = & temporary[0];

  /* Inverse = adjoint / det. (See linear algebra texts.)*/

  /* pre-compute 2x2 dets for last two rows when computing */
  /* cofactors of first two rows. */
  GLdouble d12(m31*m42-m41*m32);
  GLdouble d13(m31*m43-m41*m33);
  GLdouble d23(m32*m43-m42*m33);
  GLdouble d24(m32*m44-m42*m34);
  GLdouble d34(m33*m44-m43*m34);
  GLdouble d41(m34*m41-m44*m31);

  tmp[0] =  (m22 * d34 - m23 * d24 + m24 * d23);
  tmp[1] = -(m21 * d34 + m23 * d41 + m24 * d13);
  tmp[2] =  (m21 * d24 + m22 * d41 + m24 * d12);
  tmp[3] = -(m21 * d23 - m22 * d13 + m23 * d12);

  /* Compute determinant as early as possible using these cofactors. */
  GLdouble det(m11 * tmp[0] + m12 * tmp[1] + m13 * tmp[2] + m14 * tmp[3]);

  /* Singularity test. */
  if (abs(det)>zero)
  {
    GLdouble invDet(1.0 / det);
    /* Compute rest of inverse. */
    tmp[0] *= invDet;
    tmp[1] *= invDet;
    tmp[2] *= invDet;
    tmp[3] *= invDet;

    tmp[4] = -(m12 * d34 - m13 * d24 + m14 * d23) * invDet;
    tmp[5] =  (m11 * d34 + m13 * d41 + m14 * d13) * invDet;
    tmp[6] = -(m11 * d24 + m12 * d41 + m14 * d12) * invDet;
    tmp[7] =  (m11 * d23 - m12 * d13 + m13 * d12) * invDet;

    /* Pre-compute 2x2 dets for first two rows when computing */
    /* cofactors of last two rows. */
    d12 = m11*m22-m21*m12;
    d13 = m11*m23-m21*m13;
    d23 = m12*m23-m22*m13;
    d24 = m12*m24-m22*m14;
    d34 = m13*m24-m23*m14;
    d41 = m14*m21-m24*m11;

    tmp[8] =  (m42 * d34 - m43 * d24 + m44 * d23) * invDet;
    tmp[9] = -(m41 * d34 + m43 * d41 + m44 * d13) * invDet;
    tmp[10] =  (m41 * d24 + m42 * d41 + m44 * d12) * invDet;
    tmp[11] = -(m41 * d23 - m42 * d13 + m43 * d12) * invDet;
    tmp[12] = -(m32 * d34 - m33 * d24 + m34 * d23) * invDet;
    tmp[13] =  (m31 * d34 + m33 * d41 + m34 * d13) * invDet;
    tmp[14] = -(m31 * d24 + m32 * d41 + m34 * d12) * invDet;
    tmp[15] =  (m31 * d23 - m32 * d13 + m33 * d12) * invDet;

    if (outin)
      memcpy(out, tmp, 16*sizeof(GLdouble));
  }

#undef m11
#undef m12
#undef m13
#undef m14
#undef m21
#undef m22
#undef m23
#undef m24
#undef m31
#undef m32
#undef m33
#undef m34
#undef m41
#undef m42
#undef m43
#undef m44
#undef MAT
}



void zpr::readModelView()
{
  glGetDoublev(GL_MODELVIEW_MATRIX,matrix);
  zprGLmatrix::invertMatrix(matrixI,matrix);
}

void zpr::reshape(int width_,int height_)
{
  assert(zpr::global!=0);
  global->reshapezpr(width_,height_);
}

void zpr::write()
{
  glViewport(0,0,width,height);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  assert(zNear>0.0);
  assert(zFar>zNear);
  glFrustum(left,right,bottom,top,zNear,zFar);
  glMatrixMode(GL_MODELVIEW);

  glerrordisplay();
}

void zpr::reshapezpr(intc width_,intc height_)
{
  width = width_;
  height = height_;

//cout << "width=" << width << endl;
//cout << "height=" << height << endl;

  //glViewport(0, 0, (GLsizei) width_, (GLsizei) height_);

  writeDefault();
}

void zpr::writeDefault()
{
  assert( height>0.0);
  assert( width>0.0);

  left=-1.0;
  right=1.0;

  double dy = (double) height / (double)width;
  bottom=-dy;
  top=dy;

  zNear=1.0;
  zFar=6.0;
  write();
}

void zpr::mouse(int button, int state, int x, int y)
{
  assert(global!=0);

  global->mousezpr(button,state,x,y);
}

void zpr::readMouse
(
  GLdouble *px,
  GLdouble *py,
  GLdouble *pz,
  intc x,
  intc y
) const 
{
//    Use the ortho projection and viewport information
//    to map from mouse co-ordinates back into world 
//    co-ordinates

  int viewport[4];
  glGetIntegerv(GL_VIEWPORT,viewport);

  *px = (GLdouble)(x-viewport[0])/(GLdouble)(viewport[2]);
  *py = (GLdouble)(y-viewport[1])/(GLdouble)(viewport[3]);

  *px = left + (*px)*(right-left);
  *py = top  + (*py)*(bottom-top);
  *pz = zNear;
}



void zpr::mousezpr(intc button, intc state, intc x, intc y)
{
  mouseX = x;
  mouseY = y;

  if (state==GLUT_UP)
  switch (button)
  {
    case GLUT_LEFT_BUTTON:   mouseLeft   = false; break;
    case GLUT_MIDDLE_BUTTON: mouseMiddle = false; break;
    case GLUT_RIGHT_BUTTON:  mouseRight  = false; break;
  }
  else
  switch (button)
  {
    case GLUT_LEFT_BUTTON:    mouseLeft   = true; break;
    case GLUT_MIDDLE_BUTTON:  mouseMiddle = true; break;
    case GLUT_RIGHT_BUTTON:   mouseRight  = true; break;
  }

  readMouse(&mouseXworld,&mouseYworld,&mouseZworld,x,y);

  //if (mousecallback)
  //  (*mousecallback)();
}

void zpr::motion(int x, int y)
{
  assert(global!=0);
  global->motionzpr(x,y);
}

void zpr::motionzpr(intc x, intc y)
{
  bool changed = false;

  intc dx = x - mouseX;
  intc dy = y - mouseY;

  if (dx==0 && dy==0)
    return;

  if (mouseMiddle || (mouseLeft && mouseRight))
  {
    GLdouble s = exp((GLdouble)dy*0.01);
    glScalef(s,s,s);
    changed = true;
  }
  else
  if (mouseLeft)
  {
    GLdouble ax,ay,az;
    GLdouble bx,by,bz;
    GLdouble angle;

    ax = dy;
    ay = dx;
    az = 0.0;
    angle = vlen(ax,ay,az)/(GLdouble)(width+1)*180.0;

    /* Use inverse matrix to determine local axis of rotation */

    bx = matrixI[0]*ax + matrixI[4]*ay + matrixI[8]*az;
    by = matrixI[1]*ax + matrixI[5]*ay + matrixI[9]*az;
    bz = matrixI[2]*ax + matrixI[6]*ay + matrixI[10]*az;

    glRotatef(angle,bx,by,bz);
	
    changed = true;
  }
  else
  if (mouseRight)
  {
    GLdouble px,py,pz;

    readMouse(&px,&py,&pz,x,y);

    glLoadIdentity();
    glTranslatef(px-mouseXworld,py-mouseYworld,pz-mouseZworld);
    glMultMatrixd(matrix);

    mouseXworld = px;
    mouseYworld = py;
    mouseZworld = pz;

    changed = true;
  }

  mouseX = x;
  mouseY = y;

  if (changed)
  {	
    readModelView();
    glutPostRedisplay();
  }
}

zpr::zpr()
{
  global = this;

  mouseX = 0;
  mouseY = 0;
 
  mouseLeft   = false;
  mouseRight  = false;
  mouseMiddle = false;

  mouseXworld = 0.0;
  mouseYworld = 0.0;
  mouseZworld = 0.0;

  //mousecallback = 0;

  //glMatrixMode(GL_MODELVIEW);
  //glLoadIdentity();
 
  readScreenDimensions();

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  zNear=1.0;
  zFar=10.0;
  gluPerspective(30, (GLfloat) width/(GLfloat) height, zNear, zFar);

  glMatrixMode(GL_MODELVIEW);

  glLoadIdentity();
  gluLookAt
  (
    0.0, 0.0, 2.0,  // Eye  - creates a x: [-2,2] y: [-2,2] screen.
    0.0, 0.0, 0.0,  // Center
    0.0, 1.0, 0.0   // Up
  );


  glerrordisplay();
  update();

  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
}

void zpr::printInfo() const 
{
cout << SHOW(left) << endl;
cout << SHOW(right) << endl;
cout << SHOW(bottom) << endl;
cout << SHOW(top) << endl;
cout << SHOW(zNear) << endl;
cout << SHOW(zFar) << endl;
cout << SHOW(width) << endl;
cout << SHOW(height) << endl;
}

void zpr::writefromXaxis()
{
  GLdouble const xlen = right - left;
  GLdouble const ylen = xlen * (GLdouble) height / (GLdouble) width;
  top = bottom + ylen;

  write();
}

void zprGLmatrix::print() const
{
  unsigned int i;
  unsigned int k;
  unsigned int i2=0;
  for ( i=0; i<4; ++i)
  {
    for (k=0; k<4; ++k)
    {
      cout << matrix[i2] << " ";
      ++i2;
    }
    cout << endl;
  }
}

void zprGLmatrix::printTranspose() const
{
  unsigned int i;
  unsigned int k;
  for ( i=0; i<4; ++i)
  {
    for (k=0; k<4; ++k)
    {
      cout << access(i,k) << " ";
    }
    cout << endl;
  }
}

void zpr::readScreenDimensions()
{
  GLint viewport[4];
  glGetIntegerv( GL_VIEWPORT, viewport );

  glerrordisplay();

  width = viewport[2];
  height = viewport[3];
}

void zpr::readProjection()
{
  static GLdouble pmatrix[16];

  glerrordisplay();

  glGetDoublev(GL_PROJECTION_MATRIX,pmatrix);

  glerrordisplay();

  zprGLmatrix mat(pmatrix);

  GLdouble a[6];
  a[0] = mat.access(0,0);
  a[1] = mat.access(1,1);
  a[2] = mat.access(0,2);
  a[3] = mat.access(1,2);
  a[4] = mat.access(2,2);
  a[5] = mat.access(2,3);

  // This is essentially an inverse of a glFrustrum(...) call.
  // Here the input parameters to the glFrustrum call are
  // calculated from the projection matrix.
  GLdouble c0 = (1.0-a[4])/(1.0+a[4]);
  //GLdouble z0 = a[5]*(c0+1.0)*-0.5/c0;
  GLdouble z0 = a[5]*(1.0+1.0/c0)*-0.50;
  GLdouble z1 = -c0*z0;
  GLdouble x0 = z0*(a[2]-1.0)/a[0];
  GLdouble x1 = (z0*2.0+a[0]*x0)/a[0];
  GLdouble y0 = z0*(a[3]-1.0)/a[1];
  GLdouble y1 = (z0*2.0+a[1]*y0)/a[1];

  left = x0;
  right = x1;
  bottom = y0;
  top = y1;
  zNear = z0;
  zFar = z1;

  glerrordisplay();
}

void glerrordisplay()
{
  GLenum errval = glGetError();
  if (errval != GL_NO_ERROR)
     cout << gluErrorString( errval ) << endl;
}


GLfloat OpenGLinitialisation::light_ambient[] = 
  { 0.0, 0.0, 0.0, 1.0 };
GLfloat OpenGLinitialisation::light_diffuse[] = 
  { 1.0, 1.0, 1.0, 1.0 };
GLfloat OpenGLinitialisation::light_specular[] = 
  { 1.0, 1.0, 1.0, 1.0 };
GLfloat OpenGLinitialisation::light_position[] = 
  { 1.0, 1.0, 1.0, 0.0 };

GLfloat OpenGLinitialisation::mat_ambient[] = 
  { 0.7, 0.7, 0.7, 1.0 };
GLfloat OpenGLinitialisation::mat_diffuse[] = 
  { 0.8, 0.8, 0.8, 1.0 };
GLfloat OpenGLinitialisation::mat_specular[] = 
  { 1.0, 1.0, 1.0, 1.0 };
GLfloat OpenGLinitialisation::mat_shininess[] = 
  { 100.0 };

void OpenGLinitialisation::light0Init()
{
  glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
  glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
  glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
}

void OpenGLinitialisation::materialInit()
{
  glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
  glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
  glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
}

OpenGLinitialisation::OpenGLinitialisation()
{
  light0Init();
  materialInit();

  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glDepthFunc(GL_LESS);
  glEnable(GL_DEPTH_TEST);
  //glEnable(GL_NORMALIZE);
  glEnable(GL_COLOR_MATERIAL);
}