/*
   File:        vctr.c
   Author:      Andrew W. Moore, Dan Pelleg
   Description: "virtual centers" ADT for write-once dyv's
   $Id: vctr.c,v 1.15 2001/05/24 14:01:14 dpelleg Exp $

   Copyright 1999, Carnegie Mellon University
*/

#include "vctr.h"
#include "debug.h"
#include "simple_kdtree.h"

static const char *const CVSID = "$Id: vctr.c,v 1.15 2001/05/24 14:01:14 dpelleg Exp $";

#define NOCHILD (-1)
#define DEAD (-2)
#define NOPARENT (-3)
#define NOMAP (-4)
#define ZOMBIE (-5)

/* if has_dead_children, kills them and frees resources */
static void kill_zombie_children(vctr *obj, int parent);

/* add to vctr, with given parent (physical index) */
static int add_to_vctr_wparent(vctr *obj, const dyv *ctr, int pparent);

static dyv *vctr_delta;			/* used for temp. calculations */

vctr *mk_empty_vctr(int ndims)
{
  int i;
  vctr *ret = AM_MALLOC(vctr);
  ret->pctr = mk_empty_dyv_array();
  for(i=0; i<VCTR_NCHILDREN; i++)
	ret->child[i] = mk_ivec(0);
  ret->parent = mk_ivec(0);
  ret->pidx = mk_ivec(0);
  ret->freelist = mk_ivec(0);
  ret->nfree = 0;
  ret->nzombies = 0;
  ret->ndims = ndims;
  return ret;
}

vctr *mk_copy_vctr(const vctr *other)
{
  int i;
  vctr *ret = AM_MALLOC(vctr);
  ret->pctr = mk_copy_dyv_array(other->pctr);
  for(i=0; i<VCTR_NCHILDREN; i++)
	ret->child[i] = mk_copy_ivec(other->child[i]);
  ret->parent = mk_copy_ivec(other->parent);
  ret->pidx = mk_copy_ivec(other->pidx);
  ret->freelist = mk_copy_ivec(other->freelist);
  ret->nfree = other->nfree;
  ret->nzombies = other->nzombies;
  ret->ndims = other->ndims;
  return ret;
}

int add_to_vctr(vctr *obj, const dyv *ctr)
{
  return add_to_vctr_wparent(obj, ctr, NOPARENT);
}

static int add_to_vctr_wparent(vctr *obj, const dyv *ctr, int pparent)
{
  int i;
  int newborn;  /* physical index of new ctr */
  int new_id = ivec_size(obj->pidx);	/* virtual index of new ctr */
  if(obj->nfree > 0) {					/* just grab it from free list */
	newborn = ivec_ref(obj->freelist, --(obj->nfree));
	dyv_array_set(obj->pctr, newborn, ctr);
	for(i=0; i<VCTR_NCHILDREN; i++)
	  ivec_set(obj->child[i], newborn, NOCHILD);
	ivec_set(obj->parent, newborn, pparent);
  } else {						/* create a new entry everywhere */
	newborn = ivec_size(obj->child[0]);
	add_to_dyv_array(obj->pctr, ctr);
	for(i=0; i<VCTR_NCHILDREN; i++)
	  add_to_ivec(obj->child[i], NOCHILD);
	add_to_ivec(obj->parent, pparent);
  }
  add_to_ivec(obj->pidx, newborn);
  DBG_PRINTF(D_VCTR, ("Added %d(%d) to vctr\n", new_id, newborn));
  return new_id;
}

void remove_from_vctr(vctr *obj, int idx, bool kill_children)
{
  const int pidx = vctr_pidx(obj, idx);
#ifndef AMFAST
  if(pidx == NOMAP)
	my_error("Center is already dead");
#endif /* ! AMFAST */

  if(kill_children) {
	/* kill zombie children and reclaim the resources */
	kill_zombie_children(obj, idx);
  }

  /* insert index in free list */
  if(obj->nfree == ivec_size(obj->freelist))/* have to increase freelist */
	add_to_ivec(obj->freelist, pidx);
  else
	ivec_set(obj->freelist, obj->nfree, pidx);
  obj->nfree++;
  
  /* mark this ctr as dead */
  ivec_set(obj->child[0], pidx, DEAD);
  ivec_set(obj->pidx, idx, NOMAP);

  DBG_PRINTF(D_VCTR, ("Removed %d(%d) from vctr\n", idx, pidx));
}

void remove_children_from_vctr(vctr *obj, int parent)
{
  const int pparent = vctr_pidx(obj, parent);
  int i;
#ifndef AMFAST
  if(pparent == NOMAP || ivec_ref(obj->child[0], pparent) == NOCHILD)
	my_error("Center is dead or has no children");
  if(is_zombie(obj, parent))
	my_error("Center is zombie");
  if(ivec_ref(obj->child[0], pparent) == DEAD)
	my_error("Center is dead");
#endif /* ! AMFAST */
  DBG_PRINTF(D_VCTR, ("zombifying children of %d(%d) from vctr\n", parent, pparent));
  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int child_pidx = vctr_pidx(obj, ivec_ref(obj->child[i], pparent));
#ifndef AMFAST
	if(ivec_ref(obj->child[0], child_pidx) != NOCHILD)
	  my_error("Child has children?");
#endif /* ! AMFAST */
	ivec_set(obj->child[0], child_pidx, ZOMBIE);
  }
  obj->nzombies += VCTR_NCHILDREN;
}

static void kill_zombie_children(vctr *obj, int parent)
{
  int i;
  const int pidx = vctr_pidx(obj, parent);

  /* do nothing if no children */
  if(ivec_ref(obj->child[0], pidx) == NOCHILD)
	return;

#ifndef AMFAST
  if(ivec_ref(obj->child[0], pidx) == DEAD)
	my_error("Center is dead");
  if(pidx == NOMAP)
	my_error("Center is weird");
#endif /* ! AMFAST */

  
  DBG_PRINTF(D_VCTR, ("killing zombie children of %d(%d) from vctr\n", parent, pidx));

  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int child_idx = ivec_ref(obj->child[i], pidx);
	if(child_idx >= 0) {
	  remove_from_vctr(obj, child_idx, TRUE);
	  obj->nzombies--;
	}
	ivec_set(obj->child[i], pidx, NOCHILD);
  }
}

double move_vctr(vctr *obj, int idx, dyv *new_loc, double new_logprob, double new_sigsqd)
{
  const int pidx = vctr_pidx(obj, idx);
  double ret;
  int pparent = ivec_ref(obj->parent, pidx);
  int newidx, newpidx;

#ifndef AMFAST
  if(ivec_ref(obj->child[0], pidx) == DEAD)
	my_error("Center is dead");
  if(is_zombie(obj, idx))
	my_error("Center is zombie");
#endif /* ! AMFAST */
  if(!vctr_delta)
	vctr_delta = mk_dyv(dyv_size(new_loc));
  dyv_subtract(dyv_array_ref(obj->pctr, pidx), new_loc, vctr_delta);
  DBG_PRINTF(D_VCTR, ("Moving ctr %d(%d)\n", idx, pidx));

  /* by removing before inserting we may keep the same pidx */
  remove_from_vctr(obj, idx, TRUE);
  newidx = add_to_vctr_wparent(obj, new_loc, pparent);
  newpidx = vctr_pidx(obj, newidx);
  ret = dyv_magnitude(vctr_delta);
  if(pparent != NOPARENT) {		/* find the reference to this child and change it */
	int i;
	for(i=0; i<VCTR_NCHILDREN; i++) {
	  if(ivec_ref(obj->child[i], pparent) == idx) {
		ivec_set(obj->child[i], pparent, newidx);
		break;
	  }
	}
	if(!(i<VCTR_NCHILDREN))
	  my_error("Could not find the parent of the node");
  }

  return ret;
}

void vctr_give_birth(vctr *obj, int parent, const dyv_array *children)
{
  const int pparent = vctr_pidx(obj, parent);
  int i;
  DBG_PRINTF(D_VCTR, ("ctr %d(%d) giving birth...\n", parent, pparent));
#ifndef AMFAST
  if(pparent == NOMAP || ivec_ref(obj->child[0], pparent) != NOCHILD)
	my_error("Center already has children or is dead");
  if(dyv_array_size(children) != VCTR_NCHILDREN)
	my_error("Can't have a number of children different than VCTR_NCHILDREN");
#endif /* ! AMFAST */
  /* mark children in children array */
  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int child = add_to_vctr_wparent(obj, dyv_array_ref(children, i), pparent);
	ivec_set(obj->child[i], pparent, child);
	DBG_PRINTF(D_VCTR, ("child #%d is %d(%d)\n", i, child, vctr_pidx(obj, child)));
  }
}

dyv *safe_vctr_ref(const vctr *obj, int index)
{
  const int pindex = vctr_pidx(obj, index);
#ifndef AMFAST
  if(pindex == NOMAP)
	my_error("Center is dead");
  if(ivec_ref(obj->child[0], pindex) == ZOMBIE)
	my_error("Center is zombie");
#endif /* ! AMFAST */
  return dyv_array_ref(obj->pctr, pindex);
}

bool has_dead_children(const vctr *obj, int parent)
{
  const int pparent = vctr_pidx(obj, parent);
  const int child0 = ivec_ref(obj->child[0], pparent);
#ifndef AMFAST
  if(pparent == NOMAP)
	my_error("Center is dead");
  if(is_zombie(obj, parent))
	my_error("Center is zombie");
#endif /* ! AMFAST */
  return child0 != NOCHILD && is_zombie(obj, child0);
}

void resurrect_children(vctr *obj, int parent)
{
  const int pparent = vctr_pidx(obj, parent);
  int i;
#ifndef AMFAST
  if(pparent == NOMAP)
	my_error("Center is dead");
  if(is_zombie(obj, parent))
	my_error("Center is zombie");
  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int pchild = vctr_pidx(obj, ivec_ref(obj->child[i], pparent));
	if(ivec_ref(obj->child[0], pchild) != ZOMBIE)
	  my_error("Child isn't zombie!");
  }
  DBG_PRINTF(D_VCTR, ("Resurrecting %d(%d)'s children\n", parent, pparent));
#endif /* ! AMFAST */
  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int pchild = vctr_pidx(obj, ivec_ref(obj->child[i], pparent));
	ivec_set(obj->child[0], pchild, NOCHILD);
  }

  obj->nzombies -= VCTR_NCHILDREN;
}

bool safe_is_zombie(const vctr *obj, int idx)
{
  const int pindex = vctr_pidx(obj, idx);
  return ivec_ref(obj->child[0], pindex) == ZOMBIE;
}

ivec *mk_vctr_children(const vctr *obj, int parent)
{
  const int pparent = vctr_pidx(obj, parent);
  ivec *ret = mk_ivec(VCTR_NCHILDREN);
  int i;
#ifndef AMFAST
  if(pparent == NOMAP)
	my_error("Center is dead");
  if(is_zombie(obj, parent))
	my_error("Center is zombie");
  if(ivec_ref(obj->child[0], pparent) == NOCHILD)
	my_error("Center has no children");
  for(i=0; i<VCTR_NCHILDREN; i++) {
	const int child_pidx = vctr_pidx(obj, ivec_ref(obj->child[i], pparent));
	if(ivec_ref(obj->child[0], child_pidx) == ZOMBIE)
	  my_error("child is zombie");
  }
#endif /* ! AMFAST */
  for(i=0; i<VCTR_NCHILDREN; i++) {
	ivec_set(ret, i, ivec_ref(obj->child[i], pparent));
  }
  ivec_sort(ret, ret);
  return ret;
}

int safe_vctr_pidx(const vctr *obj, int idx)
{
  const int pidx = ivec_ref(obj->pidx, idx);
#ifndef AMFAST
  if(pidx == NOMAP)
	my_error("Center is alredy dead");
#endif /* ! AMFAST */
  return pidx;
}

int get_first_live_vctr(const vctr *obj)
{
  /* can't use vctr_pidx due to error check there */
  const int pidx0 = ivec_ref(obj->pidx, 0);
  if(pidx0 != NOMAP && !is_zombie(obj, 0)) {
	return 0;
  }
  return get_next_live_vctr(obj, 0);
}

int get_next_live_vctr(const vctr *obj, int idx)
{
  const int sz = ivec_size(obj->pidx);
  /* can't use vctr_pidx due to error check there */
  do
	idx++;
  while(idx < sz && (ivec_ref(obj->pidx, idx) == NOMAP || is_zombie(obj, idx)));
  return idx < sz ? idx : -1;
}

bool is_valid_vctr(const vctr *obj, int idx)
{
  return idx >= 0 && idx < ivec_size(obj->pidx)
	&& vctr_pidx(obj, idx) != NOMAP
	&& !is_zombie(obj, idx);
}

int vctr_max_pidx(const vctr *obj)
{
  return ivec_size(obj->child[0])-1;
}

int vctr_live_size(const vctr *obj)
{
  return ivec_size(obj->child[0]) - obj->nfree - obj->nzombies;
}

ivec *mk_live_ctrs(const vctr *obj)
{
  ivec *ret = mk_ivec(vctr_live_size(obj));
  int i, j;

  for(j=0, i=get_first_live_vctr(obj); is_valid_vctr(obj, i); j++, i=get_next_live_vctr(obj, i))
	ivec_set(ret, j, i);

  ivec_sort(ret, ret);
  return ret;
}

void fprintf_vctr(FILE *s, char *m1, vctr *obj, char *m2)
{
  if(vctr_live_size(obj) == 0)
    fprintf(s, "%s = <vctr with zero entries>%s", m1, m2);
  else {
	int i;
	const int sz = ivec_size(obj->pidx);
	for(i=0; i<sz; i++) {
	  /* can't use vctr_pidx because of death check there */
	  const int pi = ivec_ref(obj->pidx, i);
	  char tmp[256];
	  if(pi != NOMAP && !is_zombie(obj, i)) {
		sprintf(tmp, "%s%s[%d]", m1, "->pctr", i);
		fprintf_dyv(s, tmp, vctr_ref(obj, i), m2);
	  }
	}
  }
}

void fprintf_vctr_full(FILE *s, char *m1, vctr *obj, char *m2)
{
  if(vctr_live_size(obj) == 0)
    fprintf(s, "%s = <vctr with zero entries>%s", m1, m2);
  else {
	char tmp[256];
	int i;
	sprintf(tmp, "%s%s", m1, "->pctr");
	fprintf_dyv_array(s, tmp, obj->pctr, m2);
	for(i=0; i<VCTR_NCHILDREN; i++) {
	  sprintf(tmp, "%s%s[%d]", m1, "->children", i);
	  fprintf_ivec(s, tmp, obj->child[i], m2);
	}
	sprintf(tmp, "%s%s", m1, "->freelist");
	fprintf_ivec(s, tmp, obj->freelist, m2);
	fprintf(s, "npidx: %d nfree: %d nzombies: %d %s", ivec_size(obj->pidx), obj->nfree, 
			obj->nzombies, m2);
	sprintf(tmp, "%s%s", m1, "->pidx");
	fprintf_ivec(s, tmp, obj->pidx, m2);
	fprintf(s, "ndims=%d%s", obj->ndims, m2);
  }
}

void free_vctr(vctr *obj)
{
  int i;
  free_dyv_array(obj->pctr); obj->pctr = 0;
  for(i=0; i<VCTR_NCHILDREN; i++) {
	free_ivec(obj->child[i]);
	obj->child[i] = 0;
  }
  free_ivec(obj->parent); obj->parent = 0;
  free_ivec(obj->pidx); obj->pidx = 0;
  free_ivec(obj->freelist); obj->freelist = 0;
  if(vctr_delta) {
	free_dyv(vctr_delta);
	vctr_delta = 0;
  }
  AM_FREE(obj, vctr);
}

vctr *mk_vctr_from_filename(const char *filenam)
{
  char *filename = (char *) filenam; /* BUGBUG: un-const cast */
  dym *ctr_dym = mk_dym_from_filename_simple(filename);
  vctr *ret = mk_empty_vctr(dym_cols(ctr_dym));
  const int num_ctrs = dym_rows(ctr_dym);
  int i;
  for(i=0; i<num_ctrs; i++) {
	dyv *row = mk_dyv_from_dym_row(ctr_dym, i);
	add_to_vctr(ret, row);
	free_dyv(row);
  }
  free_dym(ctr_dym);
  return ret;
}

void save_vctr_to_filename(const vctr *obj, const char *filenam)
{
  char *filename = (char *) filenam; /* BUGBUG: un-const cast */
  int cols = dyv_size(vctr_ref(obj, get_first_live_vctr(obj)));
  dym *ctr_dym = mk_dym(vctr_live_size(obj), cols);
  int i,j;

  /* store all current centers in a dym... */
  for(j=get_first_live_vctr(obj), i=0; is_valid_vctr(obj, j); j=get_next_live_vctr(obj, j), i++) {
	copy_dyv_to_dym_row(vctr_ref(obj, j), ctr_dym, i);
  }
  
  /* ... save the dym */
  save_dym_to_filename(filename, ctr_dym);
  free_dym(ctr_dym);
}
