/*
 *  matrix.c -- matrix efuns.
 *              2-93 : Dwayne Fontenot : original coding.
 */

#include <math.h>

#ifdef LATTICE
#include "/lpc_incl.h"
#else
#include "../lpc_incl.h"
#include "../efun_protos.h"
#endif

#include "matrix.h"

static Matrix identity =
{1., 0., 0., 0.,
 0., 1., 0., 0.,
 0., 0., 1., 0.,
 0., 0., 0., 1.};

static void print_matrix (Matrix, char *);
static void print_array (Vector *, char *);
static Vector *normalize_array (Vector *);
static Vector *cross_product (Vector *, Vector *, Vector *);
static Vector *points_to_array (Vector *, Vector *, Vector *);

void f_id_matrix (void)
{
    array_t *matrix;
    int i;

    matrix = allocate_empty_array(16);
    for (i = 0; i < 16; i++) {
        matrix->item[i].type = T_REAL;
        matrix->item[i].u.real = identity[i];
    }
    push_refed_array(matrix);
}

void f_translate (void)
{
    array_t *matrix;
    double x, y, z;
    Matrix current_matrix;
    Matrix trans_matrix;
    Matrix final_matrix;
    int i;

    if ((sp - 1)->type != T_REAL) {
	bad_arg(3, F_TRANSLATE);
    }
    if (sp->type != T_REAL) {
	bad_arg(4, F_TRANSLATE);
    }
    /*
     * get arguments from stack.
     */
    matrix = (sp - 3)->u.arr;
    x = (sp - 2)->u.real;
    y = (sp - 1)->u.real;
    z = sp->u.real;
    sp -= 3;

    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create translation matrix.
     */
    translate_matrix(x, y, z, trans_matrix);
    /*
     * compute transformed matrix.
     */
    mult_matrix(current_matrix, trans_matrix, final_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = final_matrix[i];
    }
}

void f_scale (void)
{
    array_t *matrix;
    double x, y, z;
    Matrix current_matrix;
    Matrix scaling_matrix;
    Matrix final_matrix;
    int i;

    if ((sp - 1)->type != T_REAL) {
	bad_arg(3, F_SCALE);
    }
    if (sp->type != T_REAL) {
	bad_arg(4, F_SCALE);
    }
    /*
     * get arguments from stack.
     */
    matrix = (sp - 3)->u.arr;
    x = (sp - 2)->u.real;
    y = (sp - 1)->u.real;
    z = sp->u.real;
    sp -= 3;
    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create scaling matrix.
     */
    scale_matrix(x, y, z, scaling_matrix);
    /*
     * compute transformed matrix.
     */
    mult_matrix(current_matrix, scaling_matrix, final_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = final_matrix[i];
    }
}

void f_rotate_x (void)
{
    array_t *matrix;
    double angle;
    Matrix current_matrix;
    Matrix rot_matrix;
    Matrix final_matrix;
    int i;

    /*
     * get arguments from stack.
     */
    matrix = (sp - 1)->u.arr;
    angle = (sp--)->u.real;
    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create x rotation matrix.
     */
    rotate_x_matrix(angle, rot_matrix);
    /*
     * compute transformed matrix.
     */
    mult_matrix(current_matrix, rot_matrix, final_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = final_matrix[i];
    }
}

void f_rotate_y (void)
{
    array_t *matrix;
    double angle;
    Matrix current_matrix;
    Matrix rot_matrix;
    Matrix final_matrix;
    int i;

    /*
     * get arguments from stack.
     */
    matrix = (sp - 1)->u.arr;
    angle = (sp--)->u.real;
    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create y rotation matrix.
     */
    rotate_y_matrix(angle, rot_matrix);
    /*
     * compute transformed matrix.
     */
    mult_matrix(current_matrix, rot_matrix, final_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = final_matrix[i];
    }
}

void f_rotate_z (void)
{
    array_t *matrix;
    double angle;
    Matrix current_matrix;
    Matrix rot_matrix;
    Matrix final_matrix;
    int i;

    /*
     * get arguments from stack.
     */
    matrix = (sp - 1)->u.arr;
    angle = (sp--)->u.real;
    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create z rotation matrix.
     */
    rotate_z_matrix(angle, rot_matrix);
    /*
     * compute transformed matrix.
     */
    mult_matrix(current_matrix, rot_matrix, final_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = final_matrix[i];
    }
}

void f_lookat_rotate (void)
{
    array_t *matrix;
    double x, y, z;
    Matrix current_matrix;
    Matrix lookat_matrix;
    int i;

    if ((sp - 1)->type != T_REAL) {
	bad_arg(3, F_LOOKAT_ROTATE);
    }
    if (sp->type != T_REAL) {
	bad_arg(4, F_LOOKAT_ROTATE);
    }
    /*
     * get arguments from stack.
     */
    matrix = (sp - 3)->u.arr;
    x = (sp - 2)->u.real;
    y = (sp - 1)->u.real;
    z = sp->u.real;
    sp -= 3;
    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create new viewing transformation matrix.
     */
    lookat_rotate(current_matrix, x, y, z, lookat_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = lookat_matrix[i];
    }
}

#ifdef F_LOOKAT_ROTATE2
void f_lookat_rotate2 (void)
{
    array_t *matrix;
    double ex, ey, ez, lx, ly, lz;
    Matrix current_matrix;
    Matrix lookat_matrix;
    int i, j;

    for (j = 4; j >= 0; j--) {
	if ((sp - j)->type != T_REAL) {
	    bad_arg(7 - j, F_LOOKAT_ROTATE2);
	}
    }
    /*
     * get arguments from stack.
     */
    matrix = (sp - 6)->u.arr;
    ex = (sp - 5)->u.real;
    ey = (sp - 4)->u.real;
    ez = (sp - 3)->u.real;
    lx = (sp - 2)->u.real;
    ly = (sp - 1)->u.real;
    lz = sp->u.real;
    sp -= 5;
    free_array((sp--)->u.arr);

    /*
     * convert vec matrix to float matrix.
     */
    for (i = 0; i < 16; i++) {
	current_matrix[i] = matrix->item[i].u.real;
    }
    /*
     * create new viewing transformation matrix.
     */
    lookat_rotate2(ex, ey, ez, lx, ly, lz, lookat_matrix);
    /*
     * convert float matrix to vec matrix.
     */
    for (i = 0; i < 16; i++) {
	matrix->item[i].u.real = lookat_matrix[i];
    }
}
#endif

#ifdef DEBUG
static void print_matrix (Matrix m, char * label)
{
    int i;
    int j;

    fprintf(stderr, "%s:\n", label);
    for (i = 0; i < 4; i++) {
	for (j = 0; j < 4; j++) {
	    fprintf(stderr, "%f\t", m[i * 4 + j]);
	}
	fprintf(stderr, "\n");
    }
}

static void print_array (Vector * v, char * label)
{
    fprintf(stderr, "%s:\t%f\t%f\t%f\n", label, v->x, v->y, v->z);
}
#endif

static Vector *normalize_array (Vector * v)
{
    double xx, yy, zz, mm, m;

    xx = v->x * v->x;
    yy = v->y * v->y;
    zz = v->z * v->z;
    mm = xx + yy + zz;
    m = sqrt(mm);
    if (m) {
	v->x /= m;
	v->y /= m;
	v->z /= m;
    }
    return (v);
}

static Vector *cross_product (Vector * v, Vector * va, Vector * vb)
{
    v->x = (va->y * vb->z) - (va->z * vb->y);
    v->y = (va->z * vb->x) - (va->x * vb->z);
    v->z = (va->x * vb->y) - (va->y * vb->x);
    return (v);
}

static Vector *points_to_array (Vector * v, Vector * pa, Vector * pb)
{
    v->x = pa->x - pb->x;
    v->y = pa->y - pb->y;
    v->z = pa->z - pb->z;
    return (v);
}

void lookat_rotate (Matrix T, double x, double y, double z, Matrix M)
{
    static Vector N, V, U;
    static Vector ep, lp;

    lp.x = x;
    lp.y = y;
    lp.z = z;
    ep.x = T[12];
    ep.y = T[13];
    ep.z = T[14];
    points_to_array(&N, &lp, &ep);
    normalize_array(&N);

    U.x = T[0];
    U.y = T[4];
    U.z = T[8];
    cross_product(&V, &N, &U);
    normalize_array(&V);

    cross_product(&U, &V, &N);
    normalize_array(&U);

    M[0] = U.x;
    M[1] = V.x;
    M[2] = N.x;
    M[3] = 0.;
    M[4] = U.y;
    M[5] = V.y;
    M[6] = N.y;
    M[7] = 0.;
    M[8] = U.z;
    M[9] = V.z;
    M[10] = N.z;
    M[11] = 0.;
#if 0
    M[12] = ep.x;
    M[13] = ep.y;
    M[14] = ep.z;
    M[15] = 1.;
#endif
#if 0
    M[12] = -U.x * ep.x - U.y * ep.y - U.z * ep.z;
    M[13] = -V.x * ep.x - V.y * ep.y - V.z * ep.z;
    M[14] = -N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
    M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
    M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
    M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
    M[15] = 1.;

#ifdef DEBUG
    print_array(&lp, "look point");
    print_array(&ep, "eye point");
    print_array(&N, "normal array");
    print_array(&V, "V = N x U");
    print_array(&U, "U = V x N");
    print_matrix(M, "final matrix");
#endif				/* DEBUG */
}

void lookat_rotate2 (double ex, double ey, double ez, double lx, double ly, double lz, Matrix M)
{
    static Vector N, V, U;
    static Vector ep, lp;

    ep.x = ex;
    ep.y = ey;
    ep.z = ez;
    lp.x = lx;
    lp.y = ly;
    lp.z = lz;
    points_to_array(&N, &lp, &ep);
    normalize_array(&N);

    U.x = 0.;
    U.y = 1.;
    U.z = 0.;
    cross_product(&V, &N, &U);
    normalize_array(&V);

    cross_product(&U, &V, &N);
    normalize_array(&U);

    M[0] = U.x;
    M[1] = V.x;
    M[2] = N.x;
    M[3] = 0.;
    M[4] = U.y;
    M[5] = V.y;
    M[6] = N.y;
    M[7] = 0.;
    M[8] = U.z;
    M[9] = V.z;
    M[10] = N.z;
    M[11] = 0.;
#if 0
    M[12] = ep.x;
    M[13] = ep.y;
    M[14] = ep.z;
    M[15] = 1.;
#endif
#if 0
    M[12] = -U.x * ep.x - U.y * ep.y - U.z * ep.z;
    M[13] = -V.x * ep.x - V.y * ep.y - V.z * ep.z;
    M[14] = -N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
    M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
    M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
    M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
    M[15] = 1.;

#ifdef DEBUG
    print_array(&lp, "look point");
    print_array(&ep, "eye point");
    print_array(&N, "normal array");
    print_array(&V, "V = N x U");
    print_array(&U, "U = V x N");
    print_matrix(M, "final matrix");
#endif				/* DEBUG */
}

void translate_matrix (double x, double y, double z, Matrix m)
{
    m[0] = 1.;
    m[1] = 0.;
    m[2] = 0.;
    m[3] = 0.;
    m[4] = 0.;
    m[5] = 1.;
    m[6] = 0.;
    m[7] = 0.;
    m[8] = 0.;
    m[9] = 0.;
    m[10] = 1.;
    m[11] = 0.;
    m[12] = x;
    m[13] = y;
    m[14] = z;
    m[15] = 1.;
}

void scale_matrix (double x, double y, double z, Matrix m)
{
    m[0] = x;
    m[1] = 0.;
    m[2] = 0.;
    m[3] = 0.;
    m[4] = 0.;
    m[5] = y;
    m[6] = 0.;
    m[7] = 0.;
    m[8] = 0.;
    m[9] = 0.;
    m[10] = z;
    m[11] = 0.;
    m[12] = 0.;
    m[13] = 0.;
    m[14] = 0.;
    m[15] = 1.;
}

void rotate_x_matrix (double a, Matrix m)
{
    double a_rad;
    double c, s;

    a_rad = (double) (a * RADIANS_PER_DEGREE);
    c = cos(a_rad);
    s = sin(a_rad);
    m[0] = 1.;
    m[1] = 0.;
    m[2] = 0.;
    m[3] = 0.;
    m[4] = 0.;
    m[5] = c;
    m[6] = s;
    m[7] = 0.;
    m[8] = 0.;
    m[9] = -s;
    m[10] = c;
    m[11] = 0.;
    m[12] = 0.;
    m[13] = 0.;
    m[14] = 0.;
    m[15] = 1.;
}

void rotate_y_matrix (double a, Matrix m)
{
    double a_rad;
    double c, s;

    a_rad = (double) (a * RADIANS_PER_DEGREE);
    c = cos(a_rad);
    s = sin(a_rad);
    m[0] = c;
    m[1] = 0.;
    m[2] = -s;
    m[3] = 0.;
    m[4] = 0.;
    m[5] = 1.;
    m[6] = 0.;
    m[7] = 0.;
    m[8] = s;
    m[9] = 0.;
    m[10] = c;
    m[11] = 0.;
    m[12] = 0.;
    m[13] = 0.;
    m[14] = 0.;
    m[15] = 1.;
}

void rotate_z_matrix (double a, Matrix m)
{
    double a_rad;
    double c, s;

    a_rad = (double) (a * RADIANS_PER_DEGREE);
    c = cos(a_rad);
    s = sin(a_rad);
    m[0] = c;
    m[1] = s;
    m[2] = 0.;
    m[3] = 0.;
    m[4] = -s;
    m[5] = c;
    m[6] = 0.;
    m[7] = 0.;
    m[8] = 0.;
    m[9] = 0.;
    m[10] = 1.;
    m[11] = 0.;
    m[12] = 0.;
    m[13] = 0.;
    m[14] = 0.;
    m[15] = 1.;
}

void mult_matrix (Matrix ma, Matrix mb, Matrix m)
{
    m[0] = ma[0] * mb[0] + ma[1] * mb[4] + ma[2] * mb[8] + ma[3] * mb[12];

    m[1] = ma[0] * mb[1] + ma[1] * mb[5] + ma[2] * mb[9] + ma[3] * mb[13];

    m[2] = ma[0] * mb[2] + ma[1] * mb[6] + ma[2] * mb[10] + ma[3] * mb[14];

    m[3] = ma[0] * mb[3] + ma[1] * mb[7] + ma[2] * mb[11] + ma[3] * mb[15];

    m[4] = ma[4] * mb[0] + ma[5] * mb[4] + ma[6] * mb[8] + ma[7] * mb[12];

    m[5] = ma[4] * mb[1] + ma[5] * mb[5] + ma[6] * mb[9] + ma[7] * mb[13];

    m[6] = ma[4] * mb[2] + ma[5] * mb[6] + ma[6] * mb[10] + ma[7] * mb[14];

    m[7] = ma[4] * mb[3] + ma[5] * mb[7] + ma[6] * mb[11] + ma[7] * mb[15];

    m[8] = ma[8] * mb[0] + ma[9] * mb[4] + ma[10] * mb[8] + ma[11] * mb[12];

    m[9] = ma[8] * mb[1] + ma[9] * mb[5] + ma[10] * mb[9] + ma[11] * mb[13];

    m[10] = ma[8] * mb[2] + ma[9] * mb[6] + ma[10] * mb[10] + ma[11] * mb[14];

    m[11] = ma[8] * mb[3] + ma[9] * mb[7] + ma[10] * mb[11] + ma[11] * mb[15];

    m[12] = ma[12] * mb[0] + ma[13] * mb[4] + ma[14] * mb[8] + ma[15] * mb[12];

    m[13] = ma[12] * mb[1] + ma[13] * mb[5] + ma[14] * mb[9] + ma[15] * mb[13];

    m[14] = ma[12] * mb[2] + ma[13] * mb[6] + ma[14] * mb[10] + ma[15] * mb[14];

    m[15] = ma[12] * mb[3] + ma[13] * mb[7] + ma[14] * mb[11] + ma[15] * mb[15];
}