#include <stdio.h>
#include "vogle.h"

static	Mstack	*msfree = (Mstack *)NULL;
/******************************************************************************/
/*
 * copyvector
 *
 * Copy the 4 vector b to a.
 *
 */
void copyvector(register Vector a, register Vector b){
	a[0] = b[0];
	a[1] = b[1];
	a[2] = b[2];
	a[3] = b[3];
}
/******************************************************************************/
/*
 * copymatrix
 *
 * Copy the  4 x 4 matrix b to the 4 x 4 matrix a
 *
 */
void copymatrix(register Matrix a, register Matrix b){
	register int	i;
	register float	*pa, *pb;

	pa = (float *)a;
	pb = (float *)b;
	for(i = 0; i < 16; i++)
		*(pa++) = *(pb++);
}
/******************************************************************************/
/*
 * copytranspose
 *
 *	copy the transpose of the 4 x 4 matrix b to the 4 x 4 matrix a.
 */
void copytranspose(register Matrix a, register Matrix b){
	register int	i, j;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			a[i][j] = b[j][i];
}
/******************************************************************************/
/*
 * Retrieve the top matrix on the stack and place it in m
 */
void getmatrix(Matrix m){
	copymatrix(m, vdevice.transmat->m);
}
/******************************************************************************/
/*
 * pushmatrix
 *
 * Push the top matrix of the stack down, placing a copy
 * of it on the top of the stack.
 *
 */
void pushmatrix(void){
	Mstack	*tmpmat;
	Token	*p;

	if (vdevice.inobject) {
		p = newtokens(1);

		p->i = OBJ_PUSHMATRIX;

		return;
	}

	if (msfree != (Mstack *)NULL) {
		tmpmat = vdevice.transmat;
		vdevice.transmat = msfree;
		msfree = msfree->back;
		vdevice.transmat->back = tmpmat;
		copymatrix(vdevice.transmat->m, tmpmat->m);
	} else {
		tmpmat = (Mstack *)vallocate(sizeof(Mstack),"from pushmatrix");
		tmpmat->back = vdevice.transmat;
		copymatrix(tmpmat->m, vdevice.transmat->m);
		vdevice.transmat = tmpmat;
	}
}
/******************************************************************************/
/*
 * popmatrix
 *
 * Pop the top matrix from the stack.
 *
 */
void popmatrix(void){
	Token	*p;
	Mstack	*oldtop;

	if (vdevice.inobject) {
		p = newtokens(1);

		p->i = OBJ_POPMATRIX;

		return;
	}

	if (vdevice.transmat->back == (Mstack *)NULL)
		verror("popmatrix: matrix stack empty");
	else {
		oldtop = vdevice.transmat;
		vdevice.transmat = vdevice.transmat->back;
		oldtop->back = msfree;
		msfree = oldtop;
	}
	/* do we need to be freeing anything? */
	/*
		vfree(xxx,"from popmatrix");
	*/

	/* may have changed mapping from world to device coords */
	vdevice.cpVvalid = 0;	
}
/******************************************************************************/
void loadmatrix(Matrix mat){
   register int i;
   register float *cm, *mp;
   Token    *p;

   if (!vdevice.initialised)
      verror("loadmatrix: vogle not initialised");

   if (vdevice.inobject) {
      p = newtokens(17);

      p[0].i = OBJ_LOADMATRIX;
      cm = (float *)mat;
      for (i = 0; i < 16; i++)
         (++p)->f = *cm++;

      return;
   }

   cm = (float *)vdevice.transmat->m;
   mp = (float *)mat;
   for (i = 0; i < 16; i++){

      *cm++ = *mp++; 
    }

   /* may have changed mapping from world to device coords */
   vdevice.cpVvalid = 0;
}
/******************************************************************************/
/*
 * mult4x4
 *
 *	multiply 4 x 4 matrices b and c assigning them into a. Readers are
 * reminded that speed can be important here.
 *
 */
void mult4x4(register Matrix a, register Matrix b, register Matrix c){
a[0][0]= b[0][0] *c[0][0] +b[0][1] *c[1][0] +b[0][2] *c[2][0] +b[0][3] *c[3][0];
a[0][1]= b[0][0] *c[0][1] +b[0][1] *c[1][1] +b[0][2] *c[2][1] +b[0][3] *c[3][1];
a[0][2]= b[0][0] *c[0][2] +b[0][1] *c[1][2] +b[0][2] *c[2][2] +b[0][3] *c[3][2];
a[0][3]= b[0][0] *c[0][3] +b[0][1] *c[1][3] +b[0][2] *c[2][3] +b[0][3] *c[3][3];

a[1][0]= b[1][0] *c[0][0] +b[1][1] *c[1][0] +b[1][2] *c[2][0] +b[1][3] *c[3][0];
a[1][1]= b[1][0] *c[0][1] +b[1][1] *c[1][1] +b[1][2] *c[2][1] +b[1][3] *c[3][1];
a[1][2]= b[1][0] *c[0][2] +b[1][1] *c[1][2] +b[1][2] *c[2][2] +b[1][3] *c[3][2];
a[1][3]= b[1][0] *c[0][3] +b[1][1] *c[1][3] +b[1][2] *c[2][3] +b[1][3] *c[3][3];

a[2][0]= b[2][0] *c[0][0] +b[2][1] *c[1][0] +b[2][2] *c[2][0] +b[2][3] *c[3][0];
a[2][1]= b[2][0] *c[0][1] +b[2][1] *c[1][1] +b[2][2] *c[2][1] +b[2][3] *c[3][1];
a[2][2]= b[2][0] *c[0][2] +b[2][1] *c[1][2] +b[2][2] *c[2][2] +b[2][3] *c[3][2];
a[2][3]= b[2][0] *c[0][3] +b[2][1] *c[1][3] +b[2][2] *c[2][3] +b[2][3] *c[3][3];

a[3][0]= b[3][0] *c[0][0] +b[3][1] *c[1][0] +b[3][2] *c[2][0] +b[3][3] *c[3][0];
a[3][1]= b[3][0] *c[0][1] +b[3][1] *c[1][1] +b[3][2] *c[2][1] +b[3][3] *c[3][1];
a[3][2]= b[3][0] *c[0][2] +b[3][1] *c[1][2] +b[3][2] *c[2][2] +b[3][3] *c[3][2];
a[3][3]= b[3][0] *c[0][3] +b[3][1] *c[1][3] +b[3][2] *c[2][3] +b[3][3] *c[3][3];
}

/******************************************************************************/
/*
 * multmatrix
 *
 * Premultipy the top matrix on the stack by "mat"
 *
 */
void multmatrix(Matrix mat){
	Matrix	prod;
	float	*m;
	Token	*p;
	int	i;

	if (vdevice.inobject) {
		p = newtokens(17);

		p[0].i = OBJ_MULTMATRIX;
		m = (float *)mat;
		for (i = 0; i < 16; i++)
			(++p)->f = *m++;

		return;
	}

	mult4x4(prod, mat, vdevice.transmat->m);
	loadmatrix(prod);
}

/******************************************************************************/
/*
 * identmatrix
 *
 * Return a 4 x 4 identity matrix
 *
 */
void identmatrix(Matrix a){
	register float	*p;

	for (p = (float *)a; p != (float *)a + 16; p++)
		*p = 0;

	a[0][0] = a[1][1] = a[2][2] = a[3][3] = 1;
}

/******************************************************************************/
/*
 * multvector
 *
 * Multiply the vector a and the matrix b to form v. Need it to be snappy again.
 * 
 */
void multvector(register Vector v, register Vector a, register Matrix b){
   v[0] = a[0] * b[0][0] + a[1] * b[1][0] + a[2] * b[2][0] + a[3] * b[3][0];
   v[1] = a[0] * b[0][1] + a[1] * b[1][1] + a[2] * b[2][1] + a[3] * b[3][1];
   v[2] = a[0] * b[0][2] + a[1] * b[1][2] + a[2] * b[2][2] + a[3] * b[3][2];
   v[3] = a[0] * b[0][3] + a[1] * b[1][3] + a[2] * b[2][3] + a[3] * b[3][3];
}

/******************************************************************************/
/*
 * premultvector
 *
 * PreMultiply the vector a and the matrix b to form v. 
 * Need it to be snappy again.
 * 
 */
void premultvector(Vector v, Vector a, Matrix b){
   v[0] = a[0] * b[0][0] + a[1] * b[0][1] + a[2] * b[0][2] + a[3] * b[0][3];
   v[1] = a[0] * b[1][0] + a[1] * b[1][1] + a[2] * b[1][2] + a[3] * b[1][3];
   v[2] = a[0] * b[2][0] + a[1] * b[2][1] + a[2] * b[2][2] + a[3] * b[2][3];
   v[3] = a[0] * b[3][0] + a[1] * b[3][1] + a[2] * b[3][2] + a[3] * b[3][3];
}

/******************************************************************************/
#ifdef DEBUG 

/*
 * printmat
 *
 *	print s and then dump matrix m. Useful for debugging, you get
 * sick of typing in the print loop otherwise.
 */
void printmat(char *s, Matrix m){
	int	i, j;

	printf("%s\n", s);
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++)
			printf("%f ",m[i][j]);
		printf("\n");
	}
}
/******************************************************************************/
void printvect(char *s, Vector v){
	printf("%s %f %f %f %f\n", s, v[0], v[1], v[2], v[3]);
}
/******************************************************************************/
#endif
/******************************************************************************/