//balanced binary tree queue implemented by Michael Hines

TQItem::TQItem() {
	left_ = nil;
	right_ = nil;
	parent_ = nil;
}

TQItem::~TQItem() {
	if (left_) {
		delete left_;
	}
	if (right_) {
		delete right_;
	}
}

static void deleteitem(TQItem* i) {
	if (i->left_) {
		deleteitem(i->left_);
	}
	if (i->right_) {
		deleteitem(i->right_);
	}
	i->left_ = nil;
	i->right_ = nil;
	tpool_->free(i);
}

boolean TQItem::check() {
#if DOCHECK
	if (left_ && left_->t_ > t_) {
		printf("left %g not <= %g\n", left_->t_, t_);
		return false;
	}
	if (right_ && right_->t_ < t_) {
		printf("right %g not >= %g\n", right_->t_, t_);
		return false;
	}
	if (parent_) {
		if (parent_->left_ == this) {
			if (t_ > parent_->t_) {
				printf("%g not <= parent %g\n",
				  t_, parent_->t_);
				return false;
			}
		}else if (parent_->right_ == this) {
			if (t_ < parent_->t_) {
				printf("%g not >= parent %g\n",
				  t_, parent_->t_);
				return false;
			}
		}else{
			printf("this %g is not a child of its parent %g\n",
			   t_, parent_->t_);
			return false;
		}
	}
	if (w_ != wleft() + wright() + 1) {
		printf("%g: weight %d inconsistent with left=%d and right=%d\n",
			t_, w_, wleft(), wright());
		return false;
	}
#endif
	return true;
}

static void prnt(const TQItem* b, int level) {
	int i;
	for (i=0; i < level; ++i) {
		printf("    ");
	}
	printf("%g %c %d\n", b->t_, b->data_?'x':'o', b->w_);
}

static void chk(TQItem* b, int level) {
	if (!b->check()) {
		hoc_execerror("chk failed", errmess_);
	}
}

void TQItem::t_iterate(void (*f)(const TQItem*, int), int level) {
	if (left_) {
		left_->t_iterate(f, level+1);
	}
	f(this, level);
	if (right_) {
		right_->t_iterate(f, level+1);
	}
}
		
#if BBTQ == 0 // balanced binary tree implemented by Michael Hines

TQueue::TQueue() {
	if (!tpool_) {
		tpool_ = new TQItemPool(1000);
	}
	root_ = nil;
	least_ = nil;

#if COLLECT_TQueue_STATISTICS
	nmove = ninsert = nrem = nleast = nbal = ncmplxrem = 0;
	nfastmove = ncompare = nleastsrch = nfind = nfindsrch = 0;
#endif
}

TQueue::~TQueue() {
	if (root_) {
		deleteitem(root_);
		
	}
}
	
void TQueue::print() {
	if (root_) {
		root_->t_iterate(prnt, 0);
	}
}

void TQueue::forall_callback(void(*f)(const TQItem*, int)) {
	if (root_) {
		root_->t_iterate(f, 0);
	}
}

void TQueue::check(const char* mes) {
#if DOCHECK
	errmess_ = mes;
	if (root_) {
		root_->t_iterate(chk, 0);
	}
	errmess_ = nil;
#endif
}

double TQueue::least_t() {
	TQItem* b = least();
	if (b) {
		return b->t_;
	}else{
		return 1e15;
	}
}

TQItem* TQueue::least() {
	STAT(nleast)
#if !FAST_LEAST || DOCHECK
	TQItem* b;
	b = root_;
	if (b) for (; b->left_; b = b->left_) {
		STAT(nleastsrch)
	}
#if DOCHECK
	assert(least_ == b);
#else
	least_ = b;
#endif
#endif
	return least_;

}

void TQueue::new_least() {
	assert(least_);
	assert(!least_->left_);
	TQItem* b = least_->right_;
	if (b) {
		for (;b->left_; b = b->left_) {;}
		least_ = b;
	}else{
		b = least_->parent_;
		if (b) {
			assert(b->left_ == least_);
			least_ = b;
		}else{
			least_ = nil;
		}
	}
}

void TQueue::move_least(double tnew) {
	TQItem* b = least();
	if (b) {
		move(b, tnew);
	}
}

void TQueue::move(TQItem* i, double tnew) {
PSTART(1)
	STAT(nmove)
#if 0
	// this is a bug
	// check if it really needs moving
	STAT(ncompare)
	double tmin = -1e9, tmax = 1e9;
	if (i->left_) {
		tmin = i->left_->t_;
	}else if (i->parent_ && i->parent_->right_ == i) {
		tmin = i->parent_->t_;
	}
	if (i->right_) {
		tmax = i->right_->t_;
	}else if (i->parent_ && i->parent_->left_ == i) {
		tmax = i->parent_->t_;
	}
	if ( tmin <= tnew && tnew < tmax) { // confusing to check equality
		STAT(nfastmove)
		i->t_ = tnew;
i->check();
PSTOP(1)
		return;
	}
#endif
	// the long way
	remove1(i);
	insert1(tnew, i);
PSTOP(1)
}

TQItem* TQueue::find(double t) {
	TQItem* b;
	STAT(nfind)
	for (b = root_;b;) {
		STAT(nfindsrch)
		STAT(ncompare)
		if (t == b->t_) {
//printf("found %g\n", t);
			return b;
		}else if (t < b->t_) {
			b = b->left_;
		}else{
			b = b->right_;
		}
	}
printf("couldn't find %g\n", t);
	return b;
}

void TQueue::remove(TQItem* i) {
PSTART(1)
	if (i) {
		remove1(i);
		deleteitem(i);
	}
PSTOP(1)
}

void TQueue::remove1(TQItem* i) {
	STAT(nrem)
	// merely remove if leaf
	// if no left, right becomes left of parent
	// replace i with rightmost on left (say x)
	  // we could just replace i.data with x.data
	  // and remove x. But then, data holding pointer to
	  // x would be invalid. So take the trouble to reset
	  // pointer belonging to x
check("begin remove1");
#if FAST_LEAST
	if (least_ && least_ == i) {
		new_least();
	}
#endif
	TQItem* p = i->parent_;
	TQItem** child;
	boolean doweight = true;
	if (p) {
//printf("removing with a parent %g\n", i->t_);
		if (p->left_ == i) {
			child = &p->left_;
		}else{
			child = &p->right_;
		}
	}

	if (i->left_) {
		STAT(ncmplxrem);
//printf("removing with a left %g\n", i->t_);
		// replace i with rightmost on left
		TQItem* x;
		for (x = i->left_; x->right_; x = x->right_) {
			;
		}
		if (x == i->left_) {
//printf("x == i->left\n");
			if (p) {
				*child = x;
			}else{
				root_ = x;
			}
			x->parent_ = i->parent_;
			x->right_ = i->right_;
			if (x->right_) {
				x->right_->parent_ = x;
//				x->w_ += x->right_->w_;
				x->w_ = i->w_ - 1;
			}
		}else{	
			remove1(x);
			doweight = false;
			if (p) {
				*child = x;
			}else{
				root_ = x;
			}
			x->parent_ = i->parent_;
			x->right_ = i->right_;
			x->left_ = i->left_;
			x->left_->parent_ = x;
			if (x->right_) {
				x->right_->parent_ = x;
			}
			x->w_ = i->w_;
		}
	}else if (i->right_) {
//printf("removing with a right but no left %g\n", i->t_);
//check(); printf("checked\n");
		// no left
		if (p) {
			*child = i->right_;
			(*child)->parent_ = p;
		}else{
			root_ = i->right_;
			root_->parent_ = nil;
		}
	}else{
		// a leaf
//printf("removing leaf %g\n", i->t_);
		if (p) {
			*child = nil;
		}else{
			root_ = nil;
		}
	}
	if (doweight) {
		while (p) {
			--p->w_;
			p = p->parent_;
		}
	}	
	i->right_ = nil;
	i->left_ = nil;
	i->parent_ = nil;
check("end remove1");
}

void TQueue::reverse(TQItem* b) { // switch item and parent
	TQItem* p = b->parent_;
	if (p) {
		STAT(nbal)
if(p->parent_) {
	if (!p->parent_->check()) printf("p->parent failed start\n");
}
if (!p->check()) printf("p failed at start\n");
if (!b->check()) printf("b failed at start\n");
		if (p->parent_) {
			b->parent_ = p->parent_;
			if (p->parent_->left_ == p) {
				p->parent_->left_ = b;
			}else{
				p->parent_->right_ = b;
			}
		}else{
			assert(root_ == p);
			b->parent_ = nil;
			root_ = b;
		}
		b->w_ = p->w_;
		p->parent_ = b;
		if (p->left_ == b) {
			p->left_ = b->right_;
			b->right_ = p;
			if (p->left_) {
				p->left_->parent_ = p;
			}
		}else{
			p->right_ = b->left_;
			b->left_ = p;
			if (p->right_) {
				p->right_->parent_ = p;
			}
		}
		p->w_ = p->wleft() + p->wright() + 1;
if(b->parent_) {
	if (!b->parent_->check()) printf("b->parent failed end\n");
}
if (!p->check()) printf("p failed at end\n");
if (!b->check()) printf("b failed at end\n");
	}
}	

TQItem* TQueue::insert(double t, void* data) {
PSTART(1)
//	TQItem* i = new TQItem();
	TQItem* i = tpool_->alloc();
	i->data_ = data;
	insert1(t, i);
PSTOP(1)
	return i;
}

void TQueue::insert1(double t, TQItem* i) {
check("begin insert1");
#if FAST_LEAST
	STAT(ncompare)
	if (!least_ || t < least_->t_) {
		least_ = i;
	}
#endif
	TQItem* p = root_;
	STAT(ninsert)
	if (p) {
		for (;;) {
			STAT(ncompare)
			if (t < p->t_) {
				if (p->left_) {
					p = p->left_;
//					if (p->unbalanced()) {
//						reverse(p);
//					}
				}else{
					p->left_ = i;
					break;
				}
			}else{
				if (p->right_) {
					p = p->right_;
//					if (p->unbalanced()) {
//						reverse(p);
//					}
				}else{
					p->right_ = i;
					break;
				}
			}
		}
	}else{
		root_ = i;
	}
	i->parent_ = p;
	i->t_ = t;
	i->w_ = 1;
	for (p = i->parent_; p; p = p->parent_) {
		++p->w_;
	}
	for (p = i->parent_; p; ) {
		if (p->unbalanced()) {
			reverse(p);
		}else{
			p = p->parent_;
		}
	}
check("end insert1");
}

boolean TQItem::unbalanced() {
	int balance, dbalance;
	if (parent_) {
		balance = parent_->wright() - parent_->wleft();
		if (parent_->left_ == (TQItem*)this) {
			if (balance < -(wright() + 1)) {
//printf("balance %d to %d\n", balance, balance + 2*(wright()+1));
				return true;
			}
		}else{
			if (balance > wleft() + 1) {
//printf("balance %d to %d\n", balance, balance - 2*(wleft()+1));
				return true;
			}
		}
	}
	return false;
}

void TQueue::statistics() {
#if COLLECT_TQueue_STATISTICS
	printf("insertions=%lu  moves=%lu fastmoves=%lu removals=%lu calls to least=%lu\n", ninsert, nmove, nfastmove, nrem, nleast);
	printf("calls to find=%lu balances=%lu complex removals=%lu\n", nfind, nbal, ncmplxrem);
	printf("comparisons=%lu leastsearch=%lu findsearch=%lu\n", ncompare, nleastsrch, nfindsrch);
#else
	printf("Turn on COLLECT_TQueue_STATISTICS_ in tqueue.h\n");
#endif
}

#endif