/* 
 * "GLmatrix" class template and inlines
 *
 * GLmatrix is objective implementation of OpenGL style matrices.
 */

#ifndef __OGL2_GLMATRIX__
#define __OGL2_GLMATRIX__

#include <iostream>
#include "types.h"

#ifndef PI
#define PI 3.14159265358979324
#endif

extern "C++" {

   
/* Matrix multiplication
 */
template <class __type> inline void
__glmatrix_multiply (__type* a, __type* b, __type *c) {
  __type b1, b2, b3, b4;
  for(int i=0; i<4; i++) {
    b1 = b[i<<2]; b2 = b[1+(i<<2)]; b3 = b[2+(i<<2)]; b4 = b[3+(i<<2)];
    c[(i<<2)]   = a[0]*b1 + a[4]*b2 +  a[8]*b3 + a[12]*b4;
    c[(i<<2)+1] = a[1]*b1 + a[5]*b2 +  a[9]*b3 + a[13]*b4;
    c[(i<<2)+2] = a[2]*b1 + a[6]*b2 + a[10]*b3 + a[14]*b4;
    c[(i<<2)+3] = a[3]*b1 + a[7]*b2 + a[11]*b3 + a[15]*b4;
  }
}


/* Rotation matrix generation template, adapted from Mesa3D, the original
 * author of the function is Erich Boleyn (erich@uruk.org).
 */
template <class __type> inline void
__glmatrix_rotation_matrix (__type angle, __type x, __type y, __type z, __type* m) {

  __type mag, s, c;
  __type xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

  s = (__type)sin( angle * PI/180.0 );
  c = (__type)cos( angle * PI/180.0 );

  mag = (__type)sqrt(x*x + y*y + z*z);

  if (mag == 0.0) {
     __type *p = m;
     for(int i=16; i; i--)
       *p++ = 0;
     m[0] = 1; m[5] = 1; m[10] = 1; m[15] = 1;
     return;
  }

  x /= mag;
  y /= mag;
  z /= mag;

  xx = x * x;
  yy = y * y;
  zz = z * z;
  xy = x * y;
  yz = y * z;
  zx = z * x;
  xs = x * s;
  ys = y * s;
  zs = z * s;
  one_c = 1.0 - c;

  m[0] = (one_c * xx) + c;
  m[4] = (one_c * xy) - zs;
  m[8] = (one_c * zx) + ys;
  m[12] = 0.0;

  m[1] = (one_c * xy) + zs;
  m[5] = (one_c * yy) + c;
  m[9] = (one_c * yz) - xs;
  m[13] = 0.0;

  m[2] = (one_c * zx) - ys;
  m[6] = (one_c * yz) + xs;
  m[10] = (one_c * zz) + c;
  m[14] = 0.0;

  m[3] = 0.0;
  m[7] = 0.0;
  m[11]= 0.0;
  m[15]= 1.0;
}


template <class __type>
struct GLmatrix_template
{
  mutable __type M[16];

  GLmatrix_template () {
    __type *p = M;
    for(int i=0; i<16; i++)
      *p++ = 0;
  }
  GLmatrix_template (__type *p) {
    __type *d = M;
    for(int i=16; i; i--)
      *d++ = *p++;
  }

  __type* operator() () const {
    return M;
  }
  __type& operator[] (int i) const {
    return M[i];
  }

  GLmatrix_template& Load (__type *p) {
    __type *d = M;
    for(int i=16; i; i--)
      *d++ = *p++;
    return *this;
  }
  GLmatrix_template& Zero () {
    __type *p = M;
    for(int i=16; i; i--)
      *p++ = 0;
    return *this;
  }
  GLmatrix_template& Identity () {
    __type *p = M;
    for(int i=16; i; i--)
      *p++ = 0;
    M[0] = 1; M[5] = 1; M[10] = 1; M[15] = 1;
    return *this;
  }
  GLmatrix_template& LoadIdentity () {
    return Identity();
  }
  GLmatrix_template& Transpose () {
    __type *u, *v;
    __type a;
    for(int i=0; i<4; i++) {
      u = v = M+i+(i<<2);
      for(int j=3-i; j; j--) {
        u++;
        v+=4;
        a = *u;
        *u = *v;
        *v = a;
      }
    }  
    return *this;
  }
  GLmatrix_template& OrthoInverse () {
    __type a;
    __type x=M[12];
    __type y=M[13];
    __type z=M[14];
    a=M[1]; M[1]=M[4]; M[4]=a;
    a=M[2]; M[2]=M[8]; M[8]=a;
    a=M[6]; M[6]=M[9]; M[9]=a;
    M[12] = -(x*M[0] + y*M[4] + z*M[8]);
    M[13] = -(x*M[1] + y*M[5] + z*M[9]);
    M[14] = -(x*M[2] + y*M[6] + z*M[10]);
    return *this;
  }
  GLmatrix_template& Translate (__type x, __type y, __type z) {
    M[12] = M[0] * x + M[4] * y + M[8]  * z + M[12];
    M[13] = M[1] * x + M[5] * y + M[9]  * z + M[13];
    M[14] = M[2] * x + M[6] * y + M[10] * z + M[14];
    M[15] = M[3] * x + M[7] * y + M[11] * z + M[15];
    return *this;
  }
  GLmatrix_template& Scale (__type x, __type y, __type z) {
    M[0]*=x; M[4]*=y;  M[8]*=z;
    M[1]*=x; M[5]*=y;  M[9]*=z;
    M[2]*=x; M[6]*=y; M[10]*=z;
    M[3]*=x; M[7]*=y; M[11]*=z;
    return *this;
  }
  GLmatrix_template& Rotate (__type angle, __type x, __type y, __type z) {
    __type b[16];
    __type c[16];
    __glmatrix_rotation_matrix(angle, x, y, z, b);
    __glmatrix_multiply(this->M, b, c);
    return Load(c);
  }
  
  GLmatrix_template& operator *=(const GLmatrix_template& B) {
    __type c[16];
    __glmatrix_multiply(this->M, B(), c);
    return Load(c);
  }
  GLmatrix_template& operator +=(const GLmatrix_template& B) {
    __type *d = M;
    const __type *s = B.M;
    for(int i=16; i; i--, d++, s++)
      *d += *s;
    return *this;
  }
  GLmatrix_template& operator -=(const GLmatrix_template& B) {
    __type *d = M;
    const __type *s = B.M;
    for(int i=16; i; i--, d++, s++)
      *d -= *s;
    return *this;
  }
  GLmatrix_template& operator *=(__type scalar) {
    __type *d = M;
    for(int i=16; i; i--, d++)
      *d *= scalar;
    return *this;
  }
  GLmatrix_template& operator /=(__type scalar) {
    __type *d = M;
    for(int i=16; i; i--, d++)
      *d *= scalar;
    return *this;
  }
  void MultVertex4f (__type& _x, __type& _y, __type& _z, __type& _w)
  {
    __type x = _x*M[0] + _y*M[4] + _z*M[8] +  _w*M[12];
    __type y = _x*M[1] + _y*M[5] + _z*M[9] +  _w*M[13];
    __type z = _x*M[2] + _y*M[6] + _z*M[10] + _w*M[14];
    __type w = _x*M[3] + _y*M[7] + _z*M[11] + _w*M[15];
    _x=x; _y=y; _z=z; _w=w;
  }

};

template class GLmatrix_template<GLfloat>;
template class GLmatrix_template<GLdouble>;
typedef GLmatrix_template<GLfloat> GLmatrix;
typedef GLmatrix_template<GLdouble> GLmatrix_double;

// inlines

template <class __type> inline ostream&
operator<<(ostream& s, const GLmatrix_template<__type>& A)
{
  __type *p = A();
  for(int i=4; i; i--) {
    for(int j=4; j; j--, p+=4)
      s << (float)(*p) << " ";
    p-=15;
    s << endl;
  }
  return s;
}

template <class __type> inline GLmatrix_template<__type>
operator + (const GLmatrix_template<__type>& A)
{
  return A;
}

template <class __type> inline GLmatrix_template<__type>
operator - (const GLmatrix_template<__type>& A)
{
  GLmatrix_template<__type> nA;
  __type *d = nA();
  __type *s = A();
  for(int i=16; i; i--)
    *d++ = -(*s++);
  return nA;
}

template <class __type> inline GLmatrix_template<__type>
operator + (const GLmatrix_template<__type>& A, const GLmatrix_template<__type>& B)
{
  GLmatrix_template<__type> C;
  __type *d = C();
  __type *sA = A();
  __type *sB = B();
  for(int i=16; i; i--, d++, sA++, sB++)
    *d = *sA + *sB;
  return C;
}

template <class __type> inline GLmatrix_template<__type>
operator - (const GLmatrix_template<__type>& A, const GLmatrix_template<__type>& B)
{
  GLmatrix_template<__type> C;
  __type *d = C();
  __type *sA = A();
  __type *sB = B();
  for(int i=16; i; i--, d++, sA++, sB++)
    *d = *sA - *sB;
  return C;
}

template <class __type> inline GLmatrix_template<__type>
operator * (const GLmatrix_template<__type>& A, const GLmatrix_template<__type>& B)
{
  GLmatrix_template<__type> C;
  __glmatrix_multiply(A(), B(), C());
  return C;
}

template <class __type> inline GLmatrix_template<__type>
operator * (const GLmatrix_template<__type>& A, const __type& scalar)
{
  GLmatrix_template<__type> C;
  __type *d = C();
  __type *s = A();
  for(int i=16; i; i--, d++, s++)
    *d = (*s)*scalar;
  return C;
}

template <class __type> inline GLmatrix_template<__type>
operator * (const __type& scalar, const GLmatrix_template<__type>& A)
{
  GLmatrix_template<__type> C;
  __type *d = C();
  __type *s = A();
  for(int i=16; i; i--, d++, s++)
    *d = scalar*(*s);
  return C;
}

template <class __type> inline GLmatrix_template<__type>
operator / (const GLmatrix_template<__type>& A, const __type& scalar)
{
  GLmatrix_template<__type> C;
  __type *d = C();
  __type *s = A();
  for(int i=16; i; i--, d++, s++)
    *d = (*s)/scalar;
  return C;
}

template <class __type> inline __type
norm (const GLmatrix_template<__type>& A)
{
  __type acc = 0;
  __type *s = A();
  for(int i=16; i; i--, s++)
    acc += (*s)*(*s);
  return sqrt(acc);
}

} // extern "C++"

#endif
