#include "matrix.h"
#include <stdlib.h>
#include "mex.h"

/*
 * Versione 1.1 5/3/2004
 *
 *   - Corretto un piccolo baco che si verificava quando qualche simbolo
 *     aveva probabilita` nulla.
 *
 *   - Aggiustato un piccolo problema di portabilita`
 *
 *
 * Versione 1.0 4/3/2004
 *
 * Autore: Riccardo Bernardini, bernardini@uniud.it
 */

/* --- Funzione di debug --- */
#if 0
#  define dbg mexPrintf
#else
#  define dbg ignora
void ignora() {}; 
#endif


#define assert(x) mx_myAssert(x, #x)

void mx_myAssert(int expr, char *assertion)
{
  if (! expr)
    {
      mexErrMsgTxt(assertion);
    }

  return;
}

/* --- Albero per tabella di Huffman --- */

typedef struct tree_s {
  struct tree_s *figli[2];
  int val;
} tree; 


/* --- Code di interi --- */
typedef struct {
  int *buf;
  int len;
  int nitems;
} queue;

queue *new_queue()
{
  queue *result=(queue*) mxCalloc(1, sizeof(queue));
  result->buf = (int*) mxMalloc(sizeof(int));
  result->len = 1;
  result->nitems = 0;

  return result;
}


#if 0
void destroy_queue(queue *q)
{
  mxFree(q->buf);
  mxFree(q);
}
#endif

void append(queue *q, int val)
{
  dbg("Appendo %d. nitem=%d, len=%d\n", val, q->nitems, q->len);
  if (q->nitems == q->len)
    {
      int *tmp;
      int new_len = q->len+30;

      tmp = mxRealloc(q->buf, new_len*sizeof(int));
      if (tmp == NULL)
	mexErrMsgTxt("Out of Memory");

      q->buf = tmp;
      q->len = new_len;
    }

  q->buf[q->nitems]=val;
  q->nitems++;

  dbg("Ho appena appeso %d. Ora nitem=%d, len=%d\n", val, q->nitems, q->len);
}

/* --- Creazione dell'albero --- */
tree *new_node()
{
  tree *node = (tree*) mxCalloc(1, sizeof(tree));
  assert(node != NULL);
  dbg("new_node in\n");

  node->figli[0] = node->figli[1] = NULL;
  node->val = -1;
  dbg("new_node out\n");
  return node;
}

#if 0
void destroy_tree(tree *t)
{
  if (t->figli[0] != NULL)
    destroy_tree(t->figli[0]);

  if (t->figli[1] != NULL)
    destroy_tree(t->figli[1]);

  mxFree(t);
}
#endif

/*
 * Crea un albero di Huffman a partire dalla tabella table di nsymb righe 
 * e nbit colonne.  La riga n-sima di table contiene la parola di 
 * codice corrispondente a n.
 */
tree *make_tree(char *table, int nbit, int nsymb)
{
  tree *root, *nodo, *new_entry;
  int simbolo, bit;
  char c;

  dbg("make_tree, in\n");
  dbg("nsymb=%d\n", nsymb);

  root = new_node();

  for (simbolo=0; simbolo < nsymb; simbolo++)
    {
      dbg("simbolo=%d\n\n", simbolo);

      nodo=root;
      for(bit=0; bit<nbit; bit++)
	{
	  c=table[bit*nsymb+simbolo];
	  dbg("c='%c'\n\n", c);

	  if (c==' ')
	    { break; }
	  else 
	    {
	      c=c-'0';
	      if (nodo->figli[c]==NULL)
		{
		  /*
		   * Qui il nodo corrente non ha un figlio associato 
		   * al ramo relativo al bit 0.  Crea un nuovo nodo 
		   * e fanne il figlio del nodo corrente.
		   */

		  nodo->figli[c] = new_node();
		}

	      /*
	       * Qui il nodo corrente ha un figlio associato al bit c.
	       * Promuovi il figlio a nodo corrente
	       */

	      dbg("val=%d, root=%d\n\n", nodo->val, nodo==root);
	      assert(nodo->val == -1);
	      nodo = nodo->figli[c];
	    }
	}

      /*
       * Verifica che il nodo finale non abbia figli.  L'unico caso che puo`
       * capitare e` quando la stringa bits contiene solo spazi 
       * (questo capita in corrispondenza dei simboli con probabilita` 
       * nulla).  In questo caso non mi sposto dalla radice ed il nodo finale 
       * e` root
       */

      if (nodo != root)
	{
	  assert((nodo->figli[0]==NULL && nodo->figli[1]==NULL));
	  
	  nodo->val = simbolo;
	}
    }

  dbg("make_tree, out\n");
  return root;
}


queue *do_decode(char *bitstream, int len, char *table, int nbit, int nsymb)
{
  tree *root, *nodo;
  queue *buf = new_queue();
  int bit;

  dbg("do_decode, in\n");

  root = make_tree(table, nbit, nsymb);

  nodo=root;
  for(bit=0; bit<len; bit++)
    {
      dbg("do_decode bit=%d,[%c](%d)\n", bit, bitstream[bit], (int)(bitstream[bit]-'0'));

      nodo=nodo->figli[bitstream[bit]-'0'];
      if (nodo->val >= 0)
	{
	  dbg("Trovato simbolo %d\n", nodo->val);
	  append(buf, nodo->val);
	  nodo = root;
	}
    }

  dbg("do_decode, out\n");
  return buf;
}

char *get_string(const mxArray *p, int *nrow, int *ncol)
{
  int buflen;
  int status;
  char * buf;

  dbg("get_string in\n");

  *nrow = mxGetM(p);
  *ncol = mxGetN(p);

  /* Get the length of the input string. */
  buflen = (*nrow)*(*ncol)+1;

  /* Allocate memory for input and output strings. */
  buf = mxCalloc(buflen, sizeof(char));

  /* 
   * Copy the string data from prhs[0] into a C string 
   * input_buf. If the string array contains several rows, 
   * they are copied, one column at a time, into one long 
   * string array. 
   */
  status = mxGetString(p, buf, buflen);
  if (status != 0) 
    mexWarnMsgTxt("Not enough space. String is truncated.");

  dbg("get_string out\n");
  return buf;
}

void mexFunction(int nlhs, mxArray *plhs[], 
		 int nrhs, const mxArray *prhs[])
{

  char *bitstream, *table;
  queue *decod;
  int  nr, nc;
  int nbit, nsymb;
  int stream_length;

  mxClassID tipo;
  int n;
  double *data;

  /* Check for proper number of arguments. */
  if (nrhs != 2) 
    mexErrMsgTxt("Two inputs required.");
  
  if (nlhs > 1) 
    mexErrMsgTxt("Too many output arguments.");

  /* Input must be a string. */
  if (mxIsChar(prhs[0]) != 1 || mxIsChar(prhs[1]) != 1)
    mexErrMsgTxt("Input must be a string.");

  /*
   * Estrai il bitstream dal primo parametro
   */
  bitstream = get_string(prhs[0], &nr, &nc);
  stream_length = nr*nc;

  /*
   * Estrai la tabella dal secondo parametro
   */
  table = get_string(prhs[1], &nsymb, &nbit);

  /*
   * Decodifica
   */ 
  decod = do_decode(bitstream, stream_length, table, nbit, nsymb);


  /*
   * Copia il risultato nella variabile di uscita
   */

  plhs[0] = mxCreateDoubleMatrix(1, decod->nitems, mxREAL);

  data = mxGetPr(plhs[0]);
  
  for (n=0; n<decod->nitems; n++)
    data[n]=decod->buf[n];
}