/*
** xform.c
*/

#include <stdio.h>
#include <math.h>
#include "peek.h"

/*
** vect_matx()
** left multiplies given row vector vect1 with given matrix matx, puts 
** result in other given row vector vect2.  Does the divide by the last row
** vector element in vect2 to do normalization
*/
int vect_matx(float *vect1, Matx *matx, float *vect2, int size){
  int i, j;

  for (i=0; i<=size; i++) {
    vect2[i] = 0;
    for (j=0; j<=size; j++) {
      vect2[i] += (j < size ? (vect1[j]) : 1)*(matx->m[j][i]);
    }
  }
  if (!vect2[size]) {
    sprintf(err, "vect_matx: coord[space_d] == 0: can't normalize coords\n");
    return(-1);
  }
  for (i=0; i<=size; i++)
    vect2[i] = (i < size ? vect2[i]/vect2[size] : 0);
  return(0);
}

/*
** update_coords()
** given a piece, applies the xform matrix to 
** coord_hold[] arrays to get new coord[] arrays
*/
int update_coords(Piece *piece) {
  int n, i, j, p = 1;
  Part *vert;
  char err2[ERRSTRLEN];
  
  if (!piece) {
    sprintf(err, "apply: got null object\n");
    return(-1);
  }
  if (piece->offset) {
    /* offset info is in fact useful, run through only verts */
    for (n=0; n<=piece->offset[1]-1; n++) {
      vert = piece->catalog[n];
      if (vect_matx(vert->coord_hold, piece->xform, 
		     vert->coord, piece->space_d)) {
	sprintf(err2, "update_coords: on vert/part %d\n", vert->ID);
	strcat(err, err2); return(-1);
      }
    }
  }
  else {
    /* no offset info, run through all pieces looking for verts */
    for (n=0; n<=piece->ID_count - 1; n++) {
      if (0 == piece->catalog[n]->d) {
	vert = piece->catalog[n];
	if (vect_matx(vert->coord_hold, piece->xform, 
		       vert->coord, piece->space_d)) {
	  sprintf(err2, "update_coords: on vert/part %d\n", vert->ID);
	  strcat(err, err2); return(-1);
	}
      }
    }
  }
  return(0);
}

/*
** xform()
** multiplies the old xform matrix with the given matrix to obtain new
** xform matrix, then calls update_coords().
*/
int xform(Piece *piece, Matx *matx2) {
  float 
    temp[MAXSPACED][MAXSPACED];
  int i, j, k, p = 1, size;
  char err2[ERRSTRLEN]; 

  if (!piece) {
    sprintf(err, "xform: got null object\n");
    return(-1);
  }
  /* else */
  do {
    size = piece->space_d;
    /* multiply rotations */
    for (i=0; i<=size-1; i++) {
      for (j=0; j<=size-1; j++) {
	temp[i][j] = 0; 
	for (k=0; k<=size-1; k++) {
	  temp[i][j] += (piece->xform->m[i][k])*(matx2->m[k][j]);
	}
      }
    }
    /* add translations */
    for (i=0; i<=size-1; i++) {
      temp[size][i] = piece->xform->m[size][i] + matx2->m[size][i];
    }

    for (i=0; i<=size; i++) {
      for (j=0; j<=size-1; j++) {
	piece->xform->m[i][j] = temp[i][j];
      }
    }
    if (update_coords(piece)) {
      sprintf(err2, "xform: working on piece %d\n", p);
      strcat(err, err2); return(-1);
    }
    ++p;
  } while (piece->next);
  return(0);
}


/*
** WtoV_sysI()
**
** NOTE : Assumes that the object has been unedged and oriented
**
** World to View coordinate transform, system I (pg. 59 Watt).
** given position in spherical coords (pos), and viewing distance
** (dis), converts coordinates in coord array to coordinates
** in coord_view.  
**
** --> For the time being, the transform is done on vertices only.
** --> For the time being, the transform doesn't care if the catalog
**     for any of the pieces are sorted or not. It may or may be 
**     worth the overhead to sort the catalog by this point.
**
** (pos[0] = mu; pos[1] = theta; pos[2] = phi)
**
*/
WtoS_sysI(Piece *object, Enviro *env) {
  int i;
  Part *part;
  float *v, *w, *s, *norm, *vert_pos, dot, *pos;
 
  if (!object) {
    sprintf(err, "WtoS_sysI: got null object\n");
    return(-1);
  }

  pos = env->eye;
  /* go through and transform all the vertices */
  for (i=0; i<=(object->ID_count - 1); i++) {
    if (0 == (object->catalog)[i]->d) {
      part = object->catalog[i];
      w = part->coord;     /* abbreviations to make the following shorter */
      v = part->coord_view;
      s = part->coord_screen;
      v[0] = -w[0]*sin(pos[1]) 
	     +w[1]*cos(pos[1]);
      v[1] = -w[0]*cos(pos[1])*cos(pos[2]) 
	     -w[1]*sin(pos[1])*cos(pos[2]) 
	     +w[2]*sin(pos[2]);
      v[2] = -w[0]*cos(pos[1])*sin(pos[2]) 
	     -w[1]*sin(pos[1])*sin(pos[2]) 
	     -w[2]*cos(pos[2]) 
	     +pos[0];
      s[0] = v[0]*(env->distance)/v[2];
      s[1] = v[1]*(env->distance)/v[2];
#ifdef DEBUG
      printf("WtoV_sysI: for vert %d: \n", part->ID);
      printf("world: %g  %g  %g\n", w[0], w[1], w[2]);
      printf("view: %g  %g  %g\n", v[0], v[1], v[2]);
      printf("screen: %g  %g\n", s[0], s[1]);
#endif
    }
  }
  /* go through and find all faces, setting spare to 1 if the face will
     be seen from pos, otherwise setting it to 0 */
  if (object->thought->sense->d < THREE) {
    /* if dimension of surface is less than three then whole surface
       will be visible */
    object->thought->sense->spare = 1;
#ifdef DEBUG
    printf("WtoS_sysI: surface dimension = %d, setting spare to 1\n",
	   object->thought->sense->d);
#endif
  }
  else {
#ifdef DEBUG
    printf("WtoS_sisI: surface dimension = %d\n",
	   object->thought->sense->d);
#endif
    for (i=0; i<=(object->ID_count - 1); i++) {
      if (2 == (object->catalog)[i]->d) {
	norm = (object->catalog)[i]->norm;
	vert_pos = (object->catalog)[i]->thought->sense->coord;
	/* dot = face's norm dotted with translated eye position */
	dot = norm[0]*(pos[0]*sin(pos[2])*cos(pos[1])-vert_pos[0]) 
     	  + norm[1]*(pos[0]*sin(pos[2])*sin(pos[1])-vert_pos[1])
	    + norm[2]*(pos[0]*cos(pos[2])-vert_pos[2]);
#ifdef DEBUG
	printf("WtoS_sysI: on face %d: \n", (object->catalog)[i]->ID);
	printf("norm: %g %g %g\n", norm[0], norm[1], norm[2]);
	printf("pos: %g  %g  %g\nvert_pos: %g  %g  %g\n",
	       pos[0]*sin(pos[2])*cos(pos[1]),
	       pos[0]*sin(pos[2])*sin(pos[1]),
	       pos[0]*cos(pos[2]), 
	       vert_pos[0], vert_pos[1], vert_pos[2]);
	printf("pos - vert_pos: %g %g %g\n", 
	       pos[0]*sin(pos[2])*cos(pos[1])-vert_pos[0], 
	       pos[0]*sin(pos[2])*sin(pos[1])-vert_pos[1], 
	       pos[0]*cos(pos[2])-vert_pos[2]);
	printf("dot: %g\n", dot);
#endif
	(object->catalog)[i]->spare = (dot > 0 ? 1 : 0);
      }
    }
  }
  return(0);
}