/*median.c - incremental median computation in a moving window.
  Copyright (c) 1995 Matthew Belmonte

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  If you find this program useful, please send mail to Matthew Belmonte.
  <mkb4@Cornell.edu>.  If you base a publication on data processed by this
  program, please notify Matthew Belmonte and include the following citation
  in your publication:

	Matthew Belmonte, `A Software System for Analysis of
	Steady-State Evoked Potentials', Association for Computing
	Machinery SIGBIO Newsletter 17:1:9-14 (April 1997).
*/

/*Frequently in signal processing applications it's necessary to median-filter
  a long epoch, that is, to slide a window across a time series and to compute
  the median of the values in the window at each step.  Thus each new median
  computation will involve all but one of the points in the previous median
  computation, and only one new point.  So if these computations are implemented
  independently of each other, much useful data will be discarded and
  recomputed.  Suppose there are M points in the window and a total of N points
  in the time series.  Implementing the N computations independently using an
  optimal-time, O(M) algorithm such as a clipped Quicksort gives a total running
  time of O(N*M).  (An unclipped but optimal sort will give O(N*M*log(M)), and a
  really ditzy idea such as a Bubblesort will give O(N*M*M).)  But implementing
  them using these binary-tree algorithms gives O(N*log(M)), since insertion of
  a new point into the window, deletion of an old point from the window, and
  location of the median all have expected time O(log(M)).  A pathological data
  set such as an increasing or decreasing series will produce worst-case
  linear performance and thus reduce these algorithms to the O(N*M) performance
  of independent computations.  However, real signals, especially the kinds of
  signals that one wants to median-filter, usually don't do this.  If you're
  concerned about such difficulties you may want to alter this code so that it
  implements AVL trees.
*/

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <malloc.h>
#include "median.h"

/*Use overloaded relational operators to define the standard linear ordering
  on QVALs, whatever a QVAL happens to be.*/
static int less_than(a, b)
QVAL *a, *b;
  {
  return((*a<*b)?-1:(*a==*b)?0:1);
  }

/*Allocate a node whose fields contain the given values.*/
static node *create_node(v, rc, src, p, l, r)
QVAL v;
int rc, src;
node *p, *l, *r;
  {
  register node *n;
  n = (node *)malloc(sizeof(node));
  n->value = v;
  n->ref_count = rc;
  n->subtree_ref_count = src;
  n->parent = p;
  n->left_child = l;
  n->right_child = r;
  return(n);
  }

/*Deallocate a node.*/
static void destroy_node(n)
node *n;
  {
  free(n);
  }

/*Deallocate all the nodes of the tree rooted at the given node 'n'.*/
static void destroy_tree(n)
node *n;
  {
  register node *temp;
  while(n != NULLTREE)
    {
    destroy_tree(n->left_child);
    temp = n;
    n = n->right_child;
    destroy_node(temp);
    }
  }

/*Allocate a btree whose queue is large enough to hald 'size' items.*/
btree *create_btree(size)
int size;
  {
  register btree *t;
  t = (btree *)malloc(sizeof(btree));
  t->tree = NULLTREE;
  t->compare = less_than;
  t->head = 0;
  t->tail = -1;
  t->qsize = size;
  t->queue = (node **)calloc(1+size, sizeof(node*));
  return(t);
  }

/*Deallocate the given btree (and the tree structure to which it points).*/
void destroy_btree(t)
btree *t;
  {
  destroy_tree(t->tree);
  free(t->queue);
  free(t);
  }

/*Prune node 'n' from the tree rooted at 't->tree', rearranging the tree as
  necessary in order to preserve the descendants of 'n'.*/
static void prune(t, n)
btree *t;
node *n;
  {
  register node **prune_site, *largest;
  register int ref_count_of_largest;
  prune_site = (n->parent==NULLTREE? &(t->tree): n==n->parent->left_child? &(n->parent->left_child): &(n->parent->right_child));
  if(n->left_child == NULLTREE)
    {
    *prune_site = n->right_child;
    if(*prune_site != NULLTREE)
      (*prune_site)->parent = n->parent;
    destroy_node(n);
    }
  else if(n->right_child == NULLTREE)
    {
    *prune_site = n->left_child;
    if(*prune_site != NULLTREE)
      (*prune_site)->parent = n->parent;
    destroy_node(n);
    }
  else
    {
  /*find the largest value in the left subtree of 'n'*/
    for(largest = n->left_child; largest->right_child != NULLTREE; largest = largest->right_child)
      ;
  /*adjust reference counts to reflect the pruning of this largest value*/
    ref_count_of_largest = largest->ref_count;
    for(largest = n->left_child; largest->right_child != NULLTREE; largest = largest->right_child)
      largest->subtree_ref_count -= ref_count_of_largest;
  /*prune the largest value by replacing it with its left subtree*/
    if(largest==largest->parent->left_child)
      {
      largest->parent->left_child = largest->left_child;
      if(largest->parent->left_child != NULLTREE)
	largest->parent->left_child->parent = largest->parent;
      }
    else
      {
      largest->parent->right_child = largest->left_child;
      if(largest->parent->right_child != NULLTREE)
	largest->parent->right_child->parent = largest->parent;
      }
  /*substitute this largest-valued node for node 'n'*/
    if(n->parent == NULLTREE)
      t->tree = largest;
    else if(n == n->parent->left_child)
      n->parent->left_child = largest;
    else
      n->parent->right_child = largest;
    largest->parent = n->parent;
    largest->left_child = n->left_child;
    largest->right_child = n->right_child;
    if(largest->left_child != NULLTREE)
      largest->left_child->parent = largest;
    if(largest->right_child != NULLTREE)
      largest->right_child->parent = largest;
    largest->subtree_ref_count = largest->ref_count + (largest->left_child==NULLTREE? 0: largest->left_child->subtree_ref_count) + (largest->right_child==NULLTREE? 0: largest->right_child->subtree_ref_count);
    destroy_node(n);
    }
  }

/*Delete from the given btree the node at the head of the associated queue.*/
void delete_oldest(t)
btree *t;
  {
  register node *n;
  if((t->tail+1)%(t->qsize+1) == t->head)
    {
    fprintf(stderr, "delete_oldest: queue of size %d is empty!\n", t->qsize);
    return;
    }
  for(n = t->queue[t->head]->parent; n != NULLTREE; n = n->parent)
    n->subtree_ref_count--;
  n = t->queue[t->head];
  t->head = (t->head+1)%(t->qsize+1);
  if(n->ref_count == 1)
    prune(t, n);
  else
    {
    n->ref_count--;
    n->subtree_ref_count--;
    }
  }

/*Return the median of all the values in the given btree.  Do not alter the
  btree.*/
QVAL extract_median(t)
btree *t;
  {
  register node *n;
  register int left_count, right_count, left_size, right_size, middle_position;
  if((t->tail+1)%(t->qsize+1) == t->head)
    {
    fprintf(stderr, "extract_median: queue of size %d is empty!\n", t->qsize);
    return((QVAL)0);
    }
  n = t->tree;
  middle_position = n->subtree_ref_count/2+1;
  left_count = right_count = 0;
  left_size = (n->left_child==NULLTREE? 0: n->left_child->subtree_ref_count);
  right_size = (n->right_child==NULLTREE? 0: n->right_child->subtree_ref_count);
  while(abs(left_count+left_size - (right_count+right_size)) > n->ref_count)
  /*inv: 'left_count' is the number of values less than the median that have
    been excluded during traversal of the path from 't->tree' to 'n', and
    'left_size' is the size of the left subtree of the current node 'n'.
    'right_count' and 'right_size' are similar.*/
    {
    if(left_count+left_size+n->ref_count >= middle_position)
      {
      right_count += n->ref_count+right_size;
      n = n->left_child;
      }
    else
      {
      left_count += n->ref_count+left_size;
      n = n->right_child;
      }
    left_size = (n->left_child==NULLTREE? 0: n->left_child->subtree_ref_count);
    right_size = (n->right_child==NULLTREE? 0: n->right_child->subtree_ref_count);
    }
  return(n->value);
  }

/*Return the minimum of all the values in the given btree.  Do not alter the
  btree.*/
QVAL extract_minimum(t)
btree *t;
  {
  register node *n;
  if((t->tail+1)%(t->qsize+1) == t->head)
    {
    fprintf(stderr, "extract_minimum: queue of size %d is empty!\n", t->qsize);
    return((QVAL)0);
    }
  for(n = t->tree; n->left_child != NULLTREE; n = n->left_child)
    ;
  return(n->value);
  }

/*Return the maximum of all the values in the given btree.  Do not alter the
  btree.*/
QVAL extract_maximum(t)
btree *t;
  {
  register node *n;
  if((t->tail+1)%(t->qsize+1) == t->head)
    {
    fprintf(stderr, "extract_maximum: queue of size %d is empty!\n", t->qsize);
    return((QVAL)0);
    }
  for(n = t->tree; n->right_child != NULLTREE; n = n->right_child)
    ;
  return(n->value);
  }

/*Insert the given value into the given btree and place it at the tail of the
  associated queue.*/
void insert_newest(v, t)
QVAL v;
btree *t;
  {
  register node *n, *p;
  if((t->tail+2)%(t->qsize+1) == t->head)
    {
    fprintf(stderr, "insert_newest: queue of size %d is full; deleting oldest to make room\n", t->qsize);
    delete_oldest(t);
    }
  t->tail = (t->tail+1)%(t->qsize+1);
  p = NULLTREE;
  n = t->tree;
  while((n != NULLTREE) && (n->value != v))
  /*inv: 'p' is the parent of 'n'.  All 'subtree_ref_count' fields on the path
    from 't->tree' to 'p' have been incremented.  The proper location for the
    new value 'v' lies somewhere in the subtree rooted at 'n'.*/
    {
    n->subtree_ref_count++;
    p = n;
#ifdef MSDOS
    /*QuickC chokes on the function dereference*/
    n = (v<n->value? n->left_child: n->right_child);
#else
    n = (((*(t->compare))(&v, &(n->value)) < 0)? n->left_child: n->right_child);
#endif
    }
  if(n == NULLTREE)
    {
    register node **graft_site;
    graft_site = (p==NULLTREE? &(t->tree):
#ifdef MSDOS
      /*QuickC chokes on the function dereference*/
      v<p->value? &(p->left_child): &(p->right_child)
#else
      ((*(t->compare))(&v, &(p->value)) < 0)? &(p->left_child): &(p->right_child)
#endif
      );
    *graft_site = create_node(v, 1, 1, p, NULLTREE, NULLTREE);
    t->queue[t->tail] = *graft_site;
    }
  else
    {
    n->ref_count++;
    n->subtree_ref_count++;
    t->queue[t->tail] = n;
    }
  }

/*Search tree 't' for value 'v'.*/
int search_btree(v, t)
QVAL v;
btree *t;
  {
  register node *n;
  n = t->tree;
  while((n != NULLTREE) && (n->value != v))
  /*inv: If the value 'v' is in the tree, then it is within the subtree rooted
    at 'n'.*/
    {
#ifdef MSDOS
    /*QuickC chokes on the function dereference*/
    n = (v<n->value? n->left_child: n->right_child);
#else
    n = (((*(t->compare))(&v, &(n->value)) < 0)? n->left_child: n->right_child);
#endif
    }
  return(n != NULLTREE);
  }

/*Store an inorder traversal of subtree 'n' in array segment 'b'.  Return the
  length of the traversal.*/
static int traverse_help(n, b)
node *n;
QVAL *b;
  {
  register int ref, left_subtree_size, traversal_size;
  traversal_size = 0;
  while(n != NULLTREE)
    {
    left_subtree_size = traverse_help(n->left_child, b);
    b += left_subtree_size;
    for(ref = 0; ref != n->ref_count; ref++)
      *(b++) = n->value;
    traversal_size += left_subtree_size+n->ref_count;
    n = n->right_child;
    }
  return(traversal_size);
  }

/*Store an inorder traversal of tree 't' in array 'b'.  Return the length of
  the traversal.*/
int traverse_btree(t, b)
btree *t;
QVAL *b;
  {
  return(traverse_help(t->tree, b));
  }
