#include <cstdio> // ugly

#include <cmath>

#include <blitz/tinymat.h>

using namespace blitz;

#include "util.h"

const vec3 normalize (const vec3 &v)
{
  GLfloat s = 1. / sqrtf (v.x * v.x + v.y * v.y + v.z * v.z);

  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
         );
}

GLfloat dot (const vec3 &a, const vec3 &b)
{
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

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

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

  int i, j, k;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      {
        GLfloat s = 0.;
        for (k = 0; k < 4; k++)
          s += a(i,k) * b(k,j);

        r(i,j) = s;
      }

  return r;
}

void gl_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 gl_matrix::translate (const vec3 &v)
{
  gl_matrix m(1);

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

  (*this) = (*this) * m;
}

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);
}