/*
matrix math
*/

#include <math.h>
#include <unistd.h>
#include "vector.h"
#include "matrix.h"


#define DEGRAD (M_PI / 180.0)
#define RADDEG (180.0 / M_PI)



/* do not use destination as an input! */
inline void matrix_mulmm(matrix r, matrix a, matrix b)
{
 int i, j, k;
 for(i = 0; i < 4; i++)
     for(j = 0; j < 4; j++)
        {
         r[i][j] = 0;
         for(k = 0; k < 4; k++)
             r[i][j] += a[i][k] * b[k][j];
        }
}
inline void matrix_mulmm_safe(matrix r, matrix a, matrix b)
{
 matrix t;
 matrix_mulmm(t, a, b);
 matrix_copy(r, t);
}


inline void matrix_mulmv(vector r, matrix a, vector b)
{
 int i;
 for(i = 0; i < 4; i++)
     r[i] = vector_dotprod_4d(a[i], b);
}
inline void matrix_mulmv_safe(vector r, matrix a, vector b)
{
 vector t;
 matrix_mulmv(t, a, b);
 vector_copy(r, t);
}





int maxlen = 15;

void print_strip(char e, char m)
{
 int j, k;
 fprintf(stderr, "%c", e);
 for(j = 0; j < 4; j++)
    {
     for(k = 0; k < maxlen; k++)
         fprintf(stderr, "-");
     fprintf(stderr, "%c", (j<4-1)?m:e);
    }
 fprintf(stderr, "\n");
}

void print_row(vector v)
{
 int i, k;
 char buf[1024];
 fprintf(stderr, "|");
 for(i = 0; i < 4; i++)
     {
      snprintf(buf, 1000, " %f ", v[i]);
      if(strlen(buf) > maxlen) maxlen = strlen(buf);
      for(k = strlen(buf) ; k < maxlen; k++)
          fprintf(stderr, " ");
      fprintf(stderr, "%s", buf);
      fprintf(stderr, "|");
     }
 fprintf(stderr, "\n");
}

/* pretty-print matricies */
void matrix_print(matrix m)
{
 int i;
 print_strip('.', '.');
 for(i = 0; i < 4; i++)
    {
     print_row(m[i]);
     if(i < 4-1)
         print_strip('|', '+');
    }
 print_strip('\'', '\'');
}

/* pretty-print vectors */
void vector_print(vector v)
{
 print_strip('.', '.');
 print_row(v);
 print_strip('\'', '\'');
}




inline void matrix_identity(matrix m)
{
 int i, j;
 for(i = 0; i < 4; i++)
     for(j = 0; j < 4; j++)
         if(i == j)
             m[i][j] = 1.0;
            else
             m[i][j] = 0.0;
}


inline void matrix_translate(matrix m, vectype x, vectype y, vectype z)
{
 matrix_identity(m);
 m[0][3] = x;
 m[1][3] = y;
 m[2][3] = z;
}
inline void matrix_translate_vector(matrix m, vector v)
{
 matrix_translate(m, v[0], v[1], v[2]);
}


inline void matrix_scale(matrix m, vectype x, vectype y, vectype z)
{
 matrix_identity(m);
 m[0][0] = x;
 m[1][1] = y;
 m[2][2] = z;
}
inline void matrix_scale_vector(matrix m, vector v)
{
 matrix_scale(m, v[0], v[1], v[2]);
}


inline void matrix_rotate_axis(matrix m, int axis, vectype a)
{
 matrix_identity(m);
 if(axis == 0)
    {
     m[1][1] = m[2][2] = cos(a * DEGRAD);
     m[1][2] = -(m[2][1] = sin(a * DEGRAD));
    }
 if(axis == 1)
    {
     m[0][0] = m[2][2] = cos(a * DEGRAD);
     m[2][0] = -(m[0][2] = sin(a * DEGRAD));
    }
 if(axis == 2)
    {
     m[0][0] = m[1][1] = cos(a * DEGRAD);
     m[0][1] = -(m[1][0] = sin(a * DEGRAD));
    }
}


inline void matrix_rotate_vector(matrix m, vector v, vectype a)
{
 vectype c = cos(a * DEGRAD), s = sin(a * DEGRAD);
 matrix_identity(m);
 m[0][0] = v[0]*v[0]*(1-c)+c;       m[0][1] = v[0]*v[1]*(1-c)-v[2]*s;  m[0][2] = v[0]*v[2]*(1-c)+v[1]*s;
 m[1][0] = v[1]*v[0]*(1-c)+v[2]*s;  m[1][1] = v[1]*v[1]*(1-c)+c;       m[1][2] = v[1]*v[2]*(1-c)-v[0]*s;
 m[2][0] = v[2]*v[0]*(1-c)-v[1]*s;  m[2][1] = v[2]*v[1]*(1-c)+v[0]*s;  m[2][2] = v[2]*v[2]*(1-c)+c;
}



/* creates a "lookat" matrix for a camera positioned at point 'eye', looking
towards the point at 'center', with up in the direction given by the 'up'
vector.  very, very similar to glulookat, since it's made from the opengl
docs...  provide a familiar interface and all that!  except z axis...  */
void matrix_lookat(matrix m, vector eye, vector center, vector up)
{
 vector f, upp, s, u, en;
 matrix t;

 vector_sub_3d(f, center, eye);
 vector_norm_3d(f, f);
 vector_norm_3d(upp, up);
 vector_crossprod_3d(s, f, upp);
 vector_norm_3d(s, s);
 vector_crossprod_3d(u, s, f);

 matrix_identity(m);
 vector_copy_3d(m[0], s);
 vector_copy_3d(m[1], u);
 vector_copy_3d(m[2], f);
/* vector_invert_3d(m[2], m[2]); */

 vector_invert_3d(en, eye); 
 matrix_translate_vector(t, en);
 matrix_mulmm_safe(m, m, t);
}


/* horizontal field of view in degrees, and aspect = horizontal / vertical. */
void matrix_perspective(matrix m, vectype fov, vectype aspect)
{
 vectype c = 1.0 / tan(fov * DEGRAD);
 matrix_identity(m);
 m[0][0] = c;
 m[1][1] = c * aspect;
 m[2][2] = 1;
 m[2][3] = 1;  /* give some Z information back */
 m[3][2] = 1;
 m[3][3] = 0;
}


/* lookat and perspective rolled together */
void matrix_camera(matrix m, vector eye, vector center, vector up, vectype fov, vectype aspect)
{
 matrix la;
 matrix pt;
 matrix_lookat(la, eye, center, up);
 matrix_perspective(pt, fov, aspect);
 matrix_mulmm(m, pt, la);
}

/* and compute actual pixels, too */
void matrix_camera_image(matrix m, vector eye, vector center, vector up, vectype fov, vectype width, vectype height)
{
 matrix t;
 matrix_camera(m, eye, center, up, fov, width / height);
 matrix_translate(t, 1.0, 1.0, 0.0);
 matrix_mulmm_safe(m, t, m);
 matrix_scale(t, (vectype)width / 2.0, -(vectype)height / 2.0, 1.0);
 matrix_mulmm_safe(m, t, m);
 matrix_translate(t, 0.0, (vectype)height, 0.0);
 matrix_mulmm_safe(m, t, m);
}

void matrix_camera_image_inverse(matrix m, vector eye, vector center, vector up, vectype fov, vectype width, vectype height)
{
 matrix t;
 matrix_camera_image(t, eye, center, up, fov, width, height);
 matrix_inverse(m, t);
 matrix_translate(t, -eye[0], -eye[1], -eye[2]);
 matrix_mulmm_safe(m, t, m);
}



void matrix_copy(matrix b, matrix a)
{
 int i, j;
 for(i = 0; i < 4; i++)
     for(j = 0; j < 4; j++)
        b[i][j] = a[i][j];
}

void matrix_random(matrix a)
{
 int i, j;
 for(i = 0; i < 4; i++)
     for(j = 0; j < 4; j++)
        a[i][j] = drandn();
}
void matrix_random_int(matrix a)
{
 int i, j;
 for(i = 0; i < 4; i++)
     for(j = 0; j < 4; j++)
        a[i][j] = rand() % 201 - 100;
}




void matrix_inverse(matrix b, matrix a)
{
 int i, j, i2, pr;
 vector r;
 matrix aa;
 int used[4] = {-1, -1, -1, -1};

 matrix_copy(aa, a);
 matrix_identity(b);

 for(j = 0; j < 4; j++)
    {
     pr = -1;
     for(i = 0; i < 4; i++)
         if(used[i] < 0 && aa[i][j] != 0.0)
            {
             pr = i;
             break;
            }
     if(pr < 0) { fprintf(stderr, "No row found without a zero in column %d!\n", j); return; }
     used[pr] = j;
     for(i = 0; i < 4; i++)
        {
         if(i == pr) continue;
         vector_scale(r, b[pr], -aa[i][j]/aa[pr][j]);
         vector_add(b[i], b[i], r);
         vector_scale(r, aa[pr], -aa[i][j]/aa[pr][j]);
         vector_add(aa[i], aa[i], r);
        }
     /* matrix_print(aa); */
    }

 for(i = 0; i < 4; i++)
    {
     for(i2 = i+1; i2 < 4; i2++)
         if(used[i2] == i)
            {
             vector_swap(aa[i], aa[i2]);
             vector_swap(b[i], b[i2]);
             used[i2] = used[i];
            }
     vector_scale(b[i], b[i], 1.0 / aa[i][i]);
     vector_scale(aa[i], aa[i], 1.0 / aa[i][i]);
    }
}






int matrix_test3(void)
{
 matrix tr, tri;
 vector eye = {0, 0, -10};
 vector center = {0, 0, 0};
 vector up = {0, 1, 0};
 vector p, r, p1 = {5,4,0,1}/*, p2 = {1,0,0,1}, p3 = {1,0,1,1}*/;
 vector d;

 matrix_camera(tr, eye, center, up, 60, 1.0);
 matrix_print(tr);

 matrix_inverse(tri, tr);
 matrix_print(tri);


 vector_print(p1);
 vector_sub_3d(d, p1, eye);
 vector_norm_3d(d, d);
 vector_print(d);
 matrix_mulmv(p, tr, p1);
 vector_print(p);
 vector_norm_w(p, p);
 vector_print(p);
 p[2] = 17.0;
 vector_print(p); 
 matrix_mulmv(r, tri, p);
 vector_print(r);
 vector_norm_w(r, r);
 vector_print(r);
 vector_sub_3d(d, r, eye);
 vector_norm_3d(d, d);
 vector_print(d);
 fprintf(stderr, "\n");


/*
 vector_print(p2);
 matrix_mulmv(p, tr, p2);
 vector_print(p);
 vector_norm_w(p, p);
 vector_print(p);
 matrix_mulmv(r, tri, p);
 vector_norm_w(r, r);
 vector_print(r);
 fprintf(stderr, "\n");

 vector_print(p3);
 matrix_mulmv(p, tr, p3);
 vector_print(p);
 vector_norm_w(p, p);
 vector_print(p);
 matrix_mulmv(r, tri, p);
 vector_norm_w(r, r);
 vector_print(r);
 fprintf(stderr, "\n");
*/


 return 0;
}








int matrix_test2(void)
{
/* matrix a = { {0,2,3,4}, {4,2,1,3}, {5,1,3,2}, {3,7,4,1} }; */
 matrix a = { {1,2,3,4}, {5,6,7,8}, {9,10,11,12}, {13,14,15,16} };
 matrix b, c;
 int i;

 for(i = 0; i < 10; i++)
    {
     matrix_random_int(a); 
/*     matrix_print(a); */
     matrix_inverse(b, a);
/*     matrix_print(b); */
     matrix_mulmm(c, a, b);
     matrix_print(c);
     fprintf(stderr, "\n\n");
    }

 return 0;
}






#define MT_POINTS 5
#define MT_HEIGHT 50
#define MT_WIDTH 150
#define MT_CLOUD 8
int matrix_test(void)
{
 char buf[MT_WIDTH][MT_HEIGHT];
 matrix tr;
 matrix rot;

 vector eye = {0, 0, -10};
 vector center = {0, 0, 0};
 vector up = {0, 1, 0};

 vector points[MT_POINTS];
 char chars[MT_POINTS];
 vector l, p;
 int i, t, u, m = 0;
 vectype j;
 vector rotv = {0, 1, 0};

 srand(time(NULL));

 for(i = 0; i < MT_POINTS; i++)
    {
     points[i][0] = drandn() * MT_CLOUD;
     points[i][1] = drandn() * MT_CLOUD;
     points[i][2] = drandn() * MT_CLOUD;
     points[i][3] = 1;
     /* chars[i] = 'A' + i; */
     chars[i] = ' ' + 1 + (rand() % (127 - 33));
    }

 while(1)
    {
     memset(buf, ' ', MT_WIDTH * MT_HEIGHT);

     if(!(rand() % 10)) m = rand() % 3; 
     matrix_rotate_axis(rot, m, 5);
     matrix_mulmv_safe(rotv, rot, rotv);
     matrix_rotate_vector(rot, rotv, 5);

     matrix_mulmv_safe(eye, rot, eye);
     matrix_mulmv_safe(up, rot, up);

     matrix_camera_image(tr, eye, center, up, 60.0, MT_WIDTH, MT_HEIGHT);

     for(i = 0; i < MT_POINTS; i++)
        {
         matrix_mulmv(l, tr, points[i]);
         vector_norm_w(l, l);
vector_print(l);
         t = l[0];  if(t < 0) t = 0;  if(t >= MT_WIDTH) t = MT_WIDTH-1;
         u = l[1];  if(u < 0) u = 0;  if(u >= MT_HEIGHT) u = MT_HEIGHT-1;
         buf[t][u] = chars[i];
        }

     for(j = -10; j < 10; j += 0.5)
        {
         p[0] = j;  p[1] = 0;  p[2] = 0;  p[3] = 1;
         matrix_mulmv(l, tr, p);
         vector_norm_w(l, l);
         t = l[0];  if(t < 0) t = 0;  if(t >= MT_WIDTH) t = MT_WIDTH-1;
         u = l[1];  if(u < 0) u = 0;  if(u >= MT_HEIGHT) u = MT_HEIGHT-1;
         buf[t][u] = 'X';
        }
     for(j = -10; j < 10; j += 0.5)
        {
         p[0] = 0;  p[1] = j;  p[2] = 0;  p[3] = 1;
         matrix_mulmv(l, tr, p);
         vector_norm_w(l, l);
         t = l[0];  if(t < 0) t = 0;  if(t >= MT_WIDTH) t = MT_WIDTH-1;
         u = l[1];  if(u < 0) u = 0;  if(u >= MT_HEIGHT) u = MT_HEIGHT-1;
         buf[t][u] = 'Y';
        }
     for(j = -10; j < 10; j += 0.5)
        {
         p[0] = 0;  p[1] = 0;  p[2] = j;  p[3] = 1;
         matrix_mulmv(l, tr, p);
         vector_norm_w(l, l);
         t = l[0];  if(t < 0) t = 0;  if(t >= MT_WIDTH) t = MT_WIDTH-1;
         u = l[1];  if(u < 0) u = 0;  if(u >= MT_HEIGHT) u = MT_HEIGHT-1;
         buf[t][u] = 'Z';
        }

     for(u = 0; u < MT_HEIGHT; u++)
        {
         for(t = 0; t < MT_WIDTH; t++)
            {
             fprintf(stderr, "%c", buf[t][u]);
            }
         fprintf(stderr, "\n");
        }
     fprintf(stderr, "\n");

 
     usleep(50000);
     /* fprintf(stdout, "\0x1B[1;1H\n"); */
     system("clear");
    }

 return 0;
}



/*
int main(void)
{
 return matrix_test();
}
*/