/*
 * math support
 * most of the more complicated code is taken from mesa.
 */

#include <cstdio> // ugly
#include <cstring>
#include <cmath>

#include <sys/time.h>
#include <time.h>

#include "opengl.h"

#include "util.h"
#include "entity.h"

#define DEG2RAD (M_PI / 180.)

void renormalize (sector &s, point &p)
{
  float i;

  p.x = modff (p.x, &i); s.x += (soffs)i;
  p.y = modff (p.y, &i); s.y += (soffs)i;
  p.z = modff (p.z, &i); s.z += (soffs)i;
}

/////////////////////////////////////////////////////////////////////////////

const vec3 normalize (const vec3 &v)
{
  GLfloat s = length (v);

  if (!s)
    return v;

  s = 1. / s;
  return vec3 (v.x * s, v.y * s, v.z * s);
}

const vec3 cross (const vec3 &a, const vec3 &b)
{
  return vec3 (
     a.y * b.z - a.z * b.y,
     a.z * b.x - a.x * b.z,
     a.x * b.y - a.y * b.x
  );
}

/////////////////////////////////////////////////////////////////////////////

plane::plane (GLfloat a, GLfloat b, GLfloat c, GLfloat d)
: n (vec3 (a,b,c))
{
  GLfloat s = 1. / length (n);

  n = n * s;
  this->d = d * s;
}

/////////////////////////////////////////////////////////////////////////////

void box::add (const box &o)
{
  a.x = min (a.x, o.a.x);
  a.y = min (a.y, o.a.y);
  a.z = min (a.z, o.a.z);
  b.x = max (b.x, o.b.x);
  b.y = max (b.y, o.b.y);
  b.z = max (b.z, o.b.z);
}

void box::add (const point &p)
{
  a.x = min (a.x, p.x);
  a.y = min (a.y, p.y);
  a.z = min (a.z, p.z);
  b.x = max (b.x, p.x);
  b.y = max (b.y, p.y);
  b.z = max (b.z, p.z);
}

/////////////////////////////////////////////////////////////////////////////

struct timer timer;
static double base;
double timer::now = 0.;
double timer::diff;
double timer::fps = 1.;
//double min_frame = 1. / 85.;
double min_frame = 1. / 1000.;
double vsync_start;
GLuint vsync_count;

extern "C" int glXGetVideoSyncSGI (GLuint *count);

void timer::frame ()
{
  struct timeval tv;
  double next;

  gettimeofday (&tv, 0);

  next = tv.tv_sec - base + tv.tv_usec / 1.e6;
  diff = next - now;

  fps = fps * 0.96 + (1. / diff) * 0.04;

  if (diff < min_frame)
    {
      SDL_Delay ((unsigned int)((min_frame - diff) * 1000.));

      gettimeofday (&tv, 0);

      next = tv.tv_sec - base + tv.tv_usec / 1.e6;
      diff = next - now;
    }

  now = next;

  // working code using GLX_SGI_video_sync. estimates the real vsync
  // frequency every 2 seconds. should use a better  statistical method.
  {
   static int fps = 0;
    GLuint frame;
    glXGetVideoSyncSGI (&frame);
    if (now - vsync_start > 2.)
      {
        if (vsync_start)
          {
            fps = int ((frame - vsync_count) / (now - vsync_start) + .5F);
// works, off for debugging
//            if (fps > 30)
//              min_frame = 1. / fps;
          }

        vsync_start = now;
        vsync_count = frame;
      }

    printf ("%d fps ", fps);
  }
}

timer::timer ()
{
  struct timeval tv;
  gettimeofday (&tv, 0);
  base = tv.tv_sec + tv.tv_usec / 1.e6;
}

void render_text (GLint x, GLint y, const char *str)
{
  glRasterPos2i (x, y);

#if 0
  while (!*str)
    glutBitmapCharacter (GLUT_BITMAP_HELVETICA_18, *str++);
#endif
}

namespace gl {

  void matrix::diagonal (GLfloat v)
  {
    for (int i = 4; i--; )
      for (int j = 4; j--; )
        data[i][j] = i == j ? v : 0.;
  }

  matrix operator *(const matrix &a, const matrix &b)
  {
    matrix r;

    // taken from mesa
    for (int i = 0; i < 4; i++)
      {
        const GLfloat ai0=a(i,0), ai1=a(i,1), ai2=a(i,2), ai3=a(i,3);

        r(i,0) = ai0 * b(0,0) + ai1 * b(1,0) + ai2 * b(2,0) + ai3 * b(3,0);
        r(i,1) = ai0 * b(0,1) + ai1 * b(1,1) + ai2 * b(2,1) + ai3 * b(3,1);
        r(i,2) = ai0 * b(0,2) + ai1 * b(1,2) + ai2 * b(2,2) + ai3 * b(3,2);
        r(i,3) = ai0 * b(0,3) + ai1 * b(1,3) + ai2 * b(2,3) + ai3 * b(3,3);
      }

    return r;
  }

  matrix matrix::rotation (GLfloat angle, const vec3 &axis)
  {
    GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;

    s = (GLfloat) sinf (angle * DEG2RAD);
    c = (GLfloat) cosf (angle * DEG2RAD);

    const GLfloat mag = length (axis);

    if (mag <= 1.0e-4)
      return matrix (1);

    matrix m;
    const vec3 n = axis * (1. / mag);

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

    m(0,0) = (one_c * xx) + c;
    m(0,1) = (one_c * xy) - zs;
    m(0,2) = (one_c * zx) + ys;
    m(0,3) = 0;
    
    m(1,0) = (one_c * xy) + zs;
    m(1,1) = (one_c * yy) + c;
    m(1,2) = (one_c * yz) - xs;
    m(1,3) = 0;
    
    m(2,0) = (one_c * zx) - ys;
    m(2,1) = (one_c * yz) + xs;
    m(2,2) = (one_c * zz) + c;
    m(2,3) = 0;
    
    m(3,0) = 0;
    m(3,1) = 0;
    m(3,2) = 0;
    m(3,3) = 1;

    return m;
  }

  matrix matrix::translation (const vec3 &v)
  {
    matrix m(1);

    m(0,3) = v.x;
    m(1,3) = v.y;
    m(2,3) = v.z;

    return m;
  }

  matrix matrix::scaling (GLfloat sx, GLfloat sy, GLfloat sz, GLfloat sw)
  {
    matrix m (1.F);

    m(0,0) = sx;
    m(1,1) = sy;
    m(2,2) = sz;
    m(3,3) = sw;

    return m;
  }

  vec3 operator *(const matrix &a, const vec3 &v)
  {
    return vec3 (
      a(0,0) * v.x + a(0,1) * v.y + a(0,2) * v.z + a(0,3),
      a(1,0) * v.x + a(1,1) * v.y + a(1,2) * v.z + a(1,3),
      a(2,0) * v.x + a(2,1) * v.y + a(2,2) * v.z + a(2,3)
    );
  }

  void matrix::print ()
  {
    printf ("\n");
    printf ("[ %f, %f, %f, %f ]\n", data[0][0], data[1][0], data[2][0], data[3][0]);
    printf ("[ %f, %f, %f, %f ]\n", data[0][1], data[1][1], data[2][1], data[3][1]);
    printf ("[ %f, %f, %f, %f ]\n", data[0][2], data[1][2], data[2][2], data[3][2]);
    printf ("[ %f, %f, %f, %f ]\n", data[0][3], data[1][3], data[2][3], data[3][3]);
  }

  /////////////////////////////////////////////////////////////////////////////

  void draw_bbox (const sector &a, const sector &b)
  {
    static vertex_buffer vb;
    static index_buffer ib;

    vertex_v3f vd[] = {
       point (a.x, a.y, a.z),
       point (b.x, a.y, a.z),
       point (a.x, b.y, a.z),
       point (b.x, b.y, a.z),
       point (a.x, a.y, b.z),
       point (b.x, a.y, b.z),
       point (a.x, b.y, b.z),
       point (b.x, b.y, b.z),
    };

    if (!ib)
      {
        static GLushort verts[4*6] = {
           0, 4, 6, 2,  // -x
           1, 3, 7, 5,  // +x
           0, 1, 5, 4,  // -y
           7, 3, 2, 6,  // +y
           0, 2, 3, 1,  // -z
           4, 5, 7, 6,  // +z
        };

        ib.set (verts, 4*6, GL_STATIC_DRAW_ARB);
      }

    vb.set (vd, 8, GL_STREAM_DRAW_ARB);
    vb.bind ();
    ib.draw (GL_QUADS, 0, 4*6);
  }

  /////////////////////////////////////////////////////////////////////////////

  GLsizei format_stride (GLenum format)
  {
    switch (format)
      {
        case GL_V2F:			abort ();
        case GL_V3F:			return sizeof (vertex_v3f);
        case GL_C4UB_V2F:		abort ();
        case GL_C4UB_V3F:		abort ();
        case GL_C3F_V3F:		abort ();
        case GL_N3F_V3F:		abort ();
        case GL_C4F_N3F_V3F:		abort ();
        case GL_T2F_V3F:		abort ();
        case GL_T4F_V4F:		abort ();
        case GL_T2F_C4UB_V3F:		abort ();
        case GL_T2F_C3F_V3F:		abort ();
        case GL_T2F_N3F_V3F:		return sizeof (vertex_t2f_n3f_v3f);
        case GL_T2F_C4F_N3F_V3F:	abort ();
        case GL_T4F_C4F_N3F_V4F:	abort ();
        default:
          abort ();
      }
  }
 
  GLsizei format_offset_p (GLenum format)
  {
    switch (format)
      {
        case GL_V2F:			abort ();
        case GL_V3F:			{ vertex_v3f v; return (char *)&v.v - (char *)&v; }
        case GL_C4UB_V2F:		abort ();
        case GL_C4UB_V3F:		abort ();
        case GL_C3F_V3F:		abort ();
        case GL_N3F_V3F:		abort ();
        case GL_C4F_N3F_V3F:		abort ();
        case GL_T2F_V3F:		abort ();
        case GL_T4F_V4F:		abort ();
        case GL_T2F_C4UB_V3F:		abort ();
        case GL_T2F_C3F_V3F:		abort ();
        case GL_T2F_N3F_V3F:		{ vertex_t2f_n3f_v3f v; return (char *)&v.v - (char *)&v; }
        case GL_T2F_C4F_N3F_V3F:	abort ();
        case GL_T4F_C4F_N3F_V4F:	abort ();
        default:
          abort ();
      }
  }

  GLsizei format_offset_n (GLenum format)
  {
    switch (format)
      {
        case GL_V2F:			abort ();
        case GL_V3F:			abort ();
        case GL_C4UB_V2F:		abort ();
        case GL_C4UB_V3F:		abort ();
        case GL_C3F_V3F:		abort ();
        case GL_N3F_V3F:		abort ();
        case GL_C4F_N3F_V3F:		abort ();
        case GL_T2F_V3F:		abort ();
        case GL_T4F_V4F:		abort ();
        case GL_T2F_C4UB_V3F:		abort ();
        case GL_T2F_C3F_V3F:		abort ();
        case GL_T2F_N3F_V3F:		{ vertex_t2f_n3f_v3f v; return (char *)&v.n - (char *)&v; }
        case GL_T2F_C4F_N3F_V3F:	abort ();
        case GL_T4F_C4F_N3F_V4F:	abort ();
        default:
          abort ();
      }
  }

  GLsizei format_offset_t (GLenum format)
  {
    switch (format)
      {
        case GL_V2F:			abort ();
        case GL_V3F:			abort ();
        case GL_C4UB_V2F:		abort ();
        case GL_C4UB_V3F:		abort ();
        case GL_C3F_V3F:		abort ();
        case GL_N3F_V3F:		abort ();
        case GL_C4F_N3F_V3F:		abort ();
        case GL_T2F_V3F:		abort ();
        case GL_T4F_V4F:		abort ();
        case GL_T2F_C4UB_V3F:		abort ();
        case GL_T2F_C3F_V3F:		abort ();
        case GL_T2F_N3F_V3F:		{ vertex_t2f_n3f_v3f v; return (char *)&v.t - (char *)&v; }
        case GL_T2F_C4F_N3F_V3F:	abort ();
        case GL_T4F_C4F_N3F_V4F:	abort ();
        default:
          abort ();
      }
  }

  /////////////////////////////////////////////////////////////////////////////

  int nesting;

  void errchk (const char *name, const char *args, const char *file, int line)
  {
    static int inbegin;

    if (name[2] == 'B' && !strcmp (name, "glBegin"))
      inbegin = 1;
    else if (name[2] == 'E' && !strcmp (name, "glEnd"))
      inbegin = 0;

    if (inbegin)
      return;

    GLenum gl_derror = glGetError ();
    if (gl_derror != GL_NO_ERROR)
      {
        fprintf (stderr, "%s:%d [GLERROR %d,%s] %s(%s)\n",
                 file, line, gl_derror, gluErrorString (gl_derror), name, args);
        abort ();
      }
  }

}

template<typename idtype>
idpool<idtype>::idpool ()
: next (0)
{
}

template<typename idtype>
idtype idpool<idtype>::get ()
{
  idtype ret;

  if (free.size ())
    {
      ret = *(free.end () - 1);
      //freeid.pop_back ();
    }
  else
    ret = ++next;

  return ret;
}

template<typename idtype>
void idpool<idtype>::put (idtype &i)
{
  free.push_back (i);
}