/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	actions
 *
 * DESCRIPTION:	Functions to implement various semantic actions associated
 *		with the defining grammer.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <math.h>
#include <ctype.h>
#include "cdefs.h"
#include "types.h"
#include "symbols.h"
#include "error.h"

extern matrix fullrange;
#define FULLRANGE (&fullrange)

extern GTYPE valueStack[];

double minimumValue();
double maximumValue();

GTYPE *binop (gr, name, g1, g2)
GTYPE *gr;
char *name;
GTYPE *g1, *g2;
{
	int vi;

	vi = valueStackIndex();
	push_value_stack (*g1);
	push_value_stack (*g2);
	gr->val.i = 1;
	gr->name  = name;
 	if (function (gr, 2, &valueStack[vi]) == -1)
	 { return (NULL);
	 }
 	if (pop_value_stack (2) == -1)
	 { return (NULL);
	 }

	gr->name =  NULL;
	gr->row_i = NULL;
	gr->col_i = NULL;

 	return (gr);
}

GTYPE *monop (gr, name, g1)
GTYPE *gr;
char *name;
GTYPE *g1;
{
	int vi;

	vi = valueStackIndex();
	push_value_stack (*g1);
	gr->val.i = 1;
	gr->name  = name;
 	if (function (gr, 1, &valueStack[vi]) == -1)
 	 { return (NULL);
	 }
 	if (pop_value_stack (1) == -1)
	 { return (NULL);
	 }

	gr->name =  NULL;
	gr->row_i = NULL;
	gr->col_i = NULL;

 	return (gr);
}

GTYPE *buildRangeMatrix (gr, g1, g2, g3)
GTYPE *gr, *g1, *g2, *g3;
{
	double d, d1, d2, dinc;
	matrix *mr;
	int j, col;

	if ((getScalarValue (&d1, g1) == -1) ||
	    (getScalarValue (&d2, g2) == -1))
	 { error (0, "non-scalar in range specification\n");
	   return (NULL);
	 }
 	if (g3 == NULL)
	 { if (d2 > d1)
	    { dinc = 1.0;
	    }
	   else
	    { dinc = -1.0;
	    }
	 }
 	else
 	 { if (getScalarValue (&dinc, g3) == -1)
	    { error (0, "non-scalar in range specification\n");
	      return (NULL);
	    }
	 }

	col = fabs((d2-d1)/dinc) + 1;
	if ((mr = talloc_matrix (1, col)) == NULL)
	 { return (NULL);
	 }

	if ((d1 <= d2 && dinc > 0.0) ||
	    (d2  > d1 && dinc < 0.0))
	 { d = d1;
	   for (j=0; j<col; j++)
	    { mr->buf[j] = d;
	      d += dinc;
	    }
	 }
	gr->val.m = mr;
	gr->type = MATRIX;

	gr->name =  NULL;
	gr->row_i = NULL;
	gr->col_i = NULL;

	return (gr);	
}

GTYPE *buildMatrixRow (gr, items)
GTYPE *gr;
int items;
{
	matrix *mr;
	int vi, i, j, k, col, row;

	/* --- Sanity check --- */

	if (items <= 0)
  	 { error (0, "internal error, build matrix -- no. of items = %d\n",
		  items);
	   return (NULL);
	 }

	vi = valueStackIndex() - items;
	col = 0; row = 0;
	for (k=0; k<items; k++)
	 { switch (valueStack[vi+k].type)
	    { case DOUBLE:
	      case INTEGER:
	         { if (row && (row != 1))
		    { error (0, "build matrix -- row/column mismatch\n");
		      return (NULL);
		    }
		   else
		    { row = 1;
		    }
		   col += 1;
		   break;
		 }
	      case MATRIX:
		 { matrix *m;

		   m = valueStack[vi+k].val.m;
		   if (m == FULLRANGE)
		    { error (0, "build matrix -- illegal use of ':'\n");
		      return (NULL);
		    }
		   if (row && (row != m->row))
		    { error (0, "build matrix -- row/column mismatch\n");
		      return (NULL);
		    }
		   else
		    { row = m->row;
		    }
		   col += m->col;
		   break;
		 }
	    }
	 }
	if ((mr = talloc_matrix (row, col)) == NULL)
 	 { return (NULL);
	 }
	col = 0;
	for (k=0; k<items; k++)
	 { switch (valueStack[vi+k].type)
	    { case DOUBLE:
		 { mr->buf[col] = valueStack[vi+k].val.d;
		   col++;
		   break;
		 }
	      case INTEGER:
		 { mr->buf[col] = valueStack[vi+k].val.i;
		   col++;
		   break;
		 }
 	      case MATRIX:
		 { matrix *m = valueStack[vi+k].val.m;
		   
		   for (i=0; i<m->row; i++)
		    { for (j=0; j<m->col; j++)
		       { ELEMENT(mr,i,j+col) = ELEMENT(m,i,j);
		       }
		    }
		   col += m->col;
		   break;
		 }
	    }
	 }
	pop_value_stack(items);
	gr->val.m = mr;
	gr->type = MATRIX;

	gr->name =  NULL;
	gr->row_i = NULL;
	gr->col_i = NULL;

	return (gr);	
}

GTYPE *buildMatrix (gr, rows)
GTYPE *gr;
int rows;
{
	matrix *mr;
	int vi, i, j, k, row, col;

	if (rows <= 0)
	 { error (0, "internal error, build matrix -- no. of rows = %d\n",
		  rows);
	   return (NULL);
	 }
	vi = valueStackIndex() - rows;
	row = 0; col = valueStack[vi].val.m->col;
	for (k=0; k<rows; k++)
	 { matrix *m = valueStack[vi+k].val.m;
	   if (m->col != col)
	    { error (0, "build matrix -- row/column mismatch\n");
	      return (NULL);
	    }
	   row += m->row;
	 }
 	if ((mr = talloc_matrix (row, col)) == NULL)
	 { return (NULL);
	 }
	row = 0;
	for (k=0; k<rows; k++)
	 { matrix *m = valueStack[vi+k].val.m;
	   for (i=0; i<m->row; i++)
	    { for (j=0; j<m->col; j++)
	       { ELEMENT (mr,row+i,j) = ELEMENT (m,i,j);
	       }
	    }
	   row += m->row;
	 }
	pop_value_stack (rows);	   

	if (mr->row == 1 && mr->col == 1)
	 { gr->val.d = mr->buf[0];
	   gr->type = DOUBLE;
	 }
 	else
	 { gr->val.m = mr;
	   gr->type = MATRIX;
	 }

	gr->name =  NULL;
	gr->row_i = NULL;
	gr->col_i = NULL;

	return (gr);	
}

matrix *convertToMatrix (g)
GTYPE *g;
{
	matrix *mr;

	switch (g->type)
 	 { case INTEGER:
	      { if ((mr = talloc_matrix (1,1)) == NULL)
		 { return (NULL);
		 }
		mr->buf[0] = g->val.i;
		break;
	      }
	   case DOUBLE:
	      { if ((mr = talloc_matrix (1,1)) == NULL)
		 { return (NULL);
		 }
		mr->buf[0] = g->val.d;
		break;
	      }
	   case MATRIX:
	      { mr = g->val.m;
		break;
	      }
	   case STRING:
	      { error (0, "matrices do not accept string types\n");
	        return (NULL);
		break;
	      }
	 }
	return (mr);
}

buildSubscriptMatrices (m1, m2, subscripts)
matrix **m1, **m2;
int subscripts;
{
	matrix *mi[2];
	int vi, j, k;

	if (subscripts > 2)
	 { error (0, "more than 2 subscripts are not supported\n");
	   return (-1);
	 }
	
	mi[0] = mi[1] = NULL;

	vi = valueStackIndex() - subscripts;
	for (k=0; k<subscripts; k++)
	 { matrix *m;

	   if ((m = convertToMatrix (&valueStack[vi+k])) == NULL)
	    { return (-1);
	    }
	   if (m == FULLRANGE)
	    { mi[k] = m;
	    }
	   else
	    { if ((mi[k] = talloc_matrix(1, m->col)) == NULL)
	       { return (-1);
	       }
	      for (j=0; j<mi[k]->col; j++)
	       { mi[k]->buf[j] = m->buf[j];
	       }
	    }
	 }
 	pop_value_stack (subscripts);

	*m1 = mi[0];
	*m2 = mi[1];
	return (1);
}

GTYPE *subscriptValue (gr, g1, row_i, col_i)
GTYPE *gr, *g1;
matrix *row_i, *col_i;
{
	matrix *mr, *m1;
	int numRows, numCols, i, j, rindex, cindex;

	if (row_i == NULL && col_i == NULL)
	 { *gr = *g1;
	 }
 	else
	 { if ((m1 = convertToMatrix (g1)) == NULL)
	    { return (NULL);
	    }

	   if (col_i == NULL)
	    { if (m1->col != 1 && m1->row != 1)
	       { error (0, "single subscript used on non-vector\n");
		 return (NULL);
	       }
	      if (row_i == FULLRANGE)
	       { mr = m1;
		 gr->val.m = mr;
		 gr->type = MATRIX;
	       }
	      else if (row_i->col == 1)
	       { cindex = row_i->buf[0] - 1;
		 if (cindex < 0 || cindex >= m1->row*m1->col)
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 gr->val.d = m1->buf[ cindex ];
		 gr->type = DOUBLE;
	       }
	      else
	       { if ((int)minimumValue(row_i) < 1 ||
		     (int)maximumValue(row_i) > m1->row*m1->col)
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 if ((mr = talloc_matrix (row_i->col, 1)) == NULL)
		  { return (NULL);
		  }
		 for (j=0; j<mr->col; j++)
		  { cindex = row_i->buf[j]-1;
		    mr->buf[j] = m1->buf[ cindex ];
		  }
		 if (m1->row == 1)		/* fast and loose with indices */
		  { mr->col = mr->row;
		    mr->row = 1;
		  }
		 gr->val.m = mr;
		 gr->type = MATRIX;
	       }
	    }
	   else
	    { if (row_i == FULLRANGE && col_i == FULLRANGE)
	       { mr = m1;
		 gr->val.m = mr;
		 gr->type = MATRIX;
	       }
	      else if (row_i->col == 1 && col_i->col == 1)
	       { rindex = row_i->buf[0] - 1;
		 cindex = col_i->buf[0] - 1;
   
		 if ( rindex < 0 || rindex >= m1->row ||
		      cindex < 0 || cindex >= m1->row )
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 gr->val.d = ELEMENT (m1, rindex, cindex);
		 gr->type = DOUBLE;
	       }
	      else
	       { if (row_i == FULLRANGE)
		  { numRows = m1->row;
		  }
		 else
		  { if ((int)minimumValue(row_i) < 1 ||
			(int)maximumValue(row_i) > m1->row) 
		     { error (0, "subscript out of range\n");
		       return (NULL);
		     }
		    numRows = row_i->col;
		  }
		 if (col_i == FULLRANGE)
		  { numCols = m1->col;
		  }
		 else
		  { if ((int)minimumValue(col_i) < 1 ||
			(int)maximumValue(col_i) > m1->col) 
		     { error (0, "subscript out of range\n");
		       return (NULL);
		     }
		    numCols = col_i->col;
		  }
		 if ((mr = talloc_matrix(numRows, numCols)) == NULL)
		  { return (NULL);
		  }
   
		 for (i=0; i<mr->row; i++)
		  { for (j=0; j<mr->col; j++)
		     { 
		       if (row_i == FULLRANGE)
			{ rindex = i;
			}
		       else
			{ rindex = row_i->buf[i] - 1;
			}
		       if (col_i == FULLRANGE)
			{ cindex = j;
			}
		       else
			{ cindex = col_i->buf[j] - 1;
			}
		       ELEMENT (mr, i, j) = ELEMENT (m1, rindex, cindex);
		     }
		  }
		 gr->val.m = mr;
		 gr->type = MATRIX;
	       }
	    }
	 }

	gr->name =  g1->name;
	gr->row_i = row_i;
	gr->col_i = col_i;

	return (gr);
}

SYMBOL_TABLE_ENTRY *locateSymbol();
SYMBOL_TABLE_ENTRY *insertSymbol();

GTYPE *assignValue (gr, g1, g2)
GTYPE *gr, *g1, *g2;
{
	SYMBOL_TABLE_ENTRY *entry;
	GTYPE *gs, gbuf;
	matrix *mr, *m1, *ms;
	int i, j, maxRow, maxCol, cindex, rindex;

	if ((entry = locateSymbol(g1->name)) == NULL)
 	 { gbuf.type = DOUBLE;
	   gbuf.val.d = 0.0;
	   gs = &gbuf;
	 }
	else
	 { if (entry->protection & WRITE)
	    { tcopy_gtype (gr, &entry->g);

	      gr->name =  g1->name;
	      gr->row_i = NULL;
	      gr->col_i = NULL;

	      return (gr);
	    }
	   else
	    { gs = &entry->g;
	    }
	 }

	if (g1->row_i == NULL && g1->col_i == NULL)
	 { gr->type = g2->type;
	   gr->val  = g2->val;
	 }
	else
 	 { if ((m1 = convertToMatrix (g2)) == NULL)
	    { return (NULL);
	    }
	   if ((ms = convertToMatrix (gs)) == NULL)
	    { return (NULL);
	    }

	   if (g1->col_i == NULL)
	    { int msLength, m1Length;

	      /* Check that (1) the symbol value is a vector and (2) 
		 that the assigned value is a vector of the same type
		 (row/column) */

	      if (ms->row != 1 && ms->col != 1)
	       { error (0, "single subscript used on non-vector\n");
	         return (NULL);
	       }
	      if ((ms->row != 1 && m1->col != 1) ||
		  (ms->col != 1 && m1->row != 1))
	       { error (0, "bad submatrix assignment\n");
		 return (NULL);
	       }

	      msLength = ms->row*ms->col;
	      m1Length = m1->row*m1->col;

	      if (g1->row_i != FULLRANGE)
	       { if (g1->row_i->col != m1Length)
		  { error (0, "assign matrix -- row/column mish-mash\n");
		    return (NULL);
		  }
		 if ((int)(minimumValue(g1->row_i)) < 1)
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 maxRow = maximumValue(g1->row_i);
	       }
	      else
	       { maxRow = m1Length;
	       }

	      maxRow = MAX (maxRow, msLength);

	      if ((mr = talloc_matrix (maxRow, 1)) == NULL)
	       { return (NULL);
	       }
		
	      /* convert result to a row or column vector depending,
		 1st, upon what the target is, or, if the target is
		 1X1, what the assigned quantity is */

	      if (ms->col != 1 || (ms->row == 1 && m1->row == 1))
	       { mr->col = mr->row;
		 mr->row = 1;
	       }

	      for (i=0; i<maxRow; i++)
	       { if (i >= msLength)
		  { mr->buf[i] = 0.0;
		  }
		 else
		  { mr->buf[i] = ms->buf[i];
		  }
	       }

	      for (i=0; i<m1Length; i++)
	       { if (g1->row_i == FULLRANGE)
		  { rindex = i;
		  }
		 else
		  { rindex = g1->row_i->buf[i]-1;
		  }
	         mr->buf[rindex] = m1->buf[i];
	       }
	    }
	   else
	    {
	      if (g1->row_i != FULLRANGE)
	       { if (g1->row_i->col != m1->row)
		  { error (0, "assign matrix -- row/column mish-mash\n");
		    return (NULL);
		  }
		 if ((int)(minimumValue(g1->row_i)) < 1)
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 maxRow = maximumValue(g1->row_i);
	       }
	      else
	       { maxRow = m1->row;
	       }
	      if (g1->col_i != FULLRANGE)
	       { if (g1->col_i->col != m1->col)
		  { error (0, "assign matrix -- row/column mish-mash\n");
		    return (NULL);
		  }
		 if ((int)(minimumValue(g1->col_i)) < 1)
		  { error (0, "subscript out of range\n");
		    return (NULL);
		  }
		 maxCol = maximumValue(g1->col_i);
	       }
	      else
	       { maxCol = m1->col;
	       }

	      maxCol = MAX (maxCol, ms->col);
	      maxRow = MAX (maxRow, ms->row);
	
	      if ((mr = talloc_matrix (maxRow, maxCol)) == NULL)
	       { return (NULL);
	       }
	      for (i=0; i<mr->row; i++)
	       { for (j=0; j<mr->row; j++)
		  { if (i >= ms->row || j >= ms->col)
		     { ELEMENT (mr, i, j) = 0.0;
		     }
		    else
		     { ELEMENT (mr, i, j) = ELEMENT (ms, i, j);
		     }
		  }
	       }
	  
	      for (i=0; i<m1->row; i++)
	       { for (j=0; j<m1->col; j++)
		  { if (g1->row_i == FULLRANGE)
		     { rindex = i;
		     }
		    else
		     { rindex = g1->row_i->buf[i]-1;
		     }
		    if (g1->col_i == FULLRANGE)
		     { cindex = j;
		     }
		    else
		     { cindex = g1->col_i->buf[j]-1;
		     }
		    ELEMENT (mr, rindex, cindex) = ELEMENT (m1, i, j);
		  }
	       }
	    }

	   if (mr->row == 1 && mr->col == 1)
	    { gr->type = DOUBLE;
	      gr->val.d = mr->buf[0];
	    }
	   else
	    { gr->type = MATRIX;
	      gr->val.m = mr;
	    }
	 }

	if (entry == NULL)
  	 { if ((entry = insertSymbol (g1->name)) == NULL)
	    { return (NULL);
	    }
	   entry->protection = 0;
	 }
 	else
	 { free_gtype (&entry->g);
	 }
	if (mcopy_gtype (&entry->g, gr) == NULL)
	 { return (NULL);
	 }

	gr->val   = g2->val;
	gr->type  = g2->type;

	gr->name  = g1->name;
	gr->row_i = g1->row_i;
	gr->col_i = g1->col_i;

	return (gr);
}

getScalarValue (d, g)
double *d;
GTYPE *g;
{
	switch (g->type)
	 { case INTEGER:
	       { *d = g->val.i;
		 return (1);
		 break;
	       }
	   case DOUBLE:
	       { *d = g->val.d;
		 return (1);
		 break;
	       }
	   case MATRIX:
	       { matrix *m = g->val.m;
	       
	         if (m->row == 1 && m->col == 1)
		  { *d = m->buf[0];
		    return (1);
		  }
	       }
	 }
	return (-1);
}

/* returns the maximum value of a matrix */

double maximumValue (m)
matrix *m;
{
	double max = -HUGE;
	int i, j;

	for (i=0; i<m->row; i++)
 	 { for (j=0; j<m->col; j++)
	    { if (ELEMENT (m, i, j) > max)
	       { max = ELEMENT (m, i, j);
	       }
	    }
	 }
 	return (max);
}

/* returns the minimum value of a matrix */

double minimumValue (m)
matrix *m;
{
	double min = HUGE;
	int i, j;

	for (i=0; i<m->row; i++)
 	 { for (j=0; j<m->col; j++)
	    { if (ELEMENT (m, i, j) < min)
	       { min = ELEMENT (m, i, j);
	       }
	    }
	 }
 	return (min);
}
