/** Maintain red-black tree balance after inserting node x
 *
 */
static void insert_fixup(rbtree_t *tree, rbnode_t *x)
{
	/* check RED-BLACK properties */
	while ((x != tree->root) && (x->parent->colour == RED)) {
		/* we have a violation */
		if (x->parent == x->parent->parent->left) {
			rbnode_t *y = x->parent->parent->right;
			if (y->colour == RED) {

				/* uncle is RED */
				x->parent->colour = BLACK;
				y->colour = BLACK;
				x->parent->parent->colour = RED;
				x = x->parent->parent;
			} else {

				/* uncle is BLACK */
				if (x == x->parent->right) {
					/* make x a left child */
					x = x->parent;
					rotate_left(tree, x);
				}

				/* recolour and rotate */
				x->parent->colour = BLACK;
				x->parent->parent->colour = RED;
				rotate_right(tree, x->parent->parent);
			}
		} else {

			/* mirror image of above code */
			rbnode_t *y = x->parent->parent->left;
			if (y->colour == RED) {

				/* uncle is RED */
				x->parent->colour = BLACK;
				y->colour = BLACK;
				x->parent->parent->colour = RED;
				x = x->parent->parent;
			} else {

				/* uncle is BLACK */
				if (x == x->parent->left) {
					x = x->parent;
					rotate_right(tree, x);
				}
				x->parent->colour = BLACK;
				x->parent->parent->colour = RED;
				rotate_left(tree, x->parent->parent);
			}
		}
	}

	tree->root->colour = BLACK;
}

 void balanced_pst::erase_fixup(node* x) {
   while (x != root && x->color == BLACK) {
     if (x == x->p->left) {
       node* w = x->p->right;
       if (w->color == RED) {
         w->color = BLACK;
         x->p->color = RED;
         rotate_left(x->p);
         w = x->p->right;
       }
       if (w->left->color == BLACK && w->right->color == BLACK) {
         w->color = RED;
         x = x->p;
       } else {
         if (w->right->color == BLACK) {
           w->left->color = BLACK;
           w->color = RED;
           rotate_right(w);
           w = x->p->right;
         }
         w->color = x->p->color;
         x->p->color = BLACK;
         w->right->color = BLACK;
         rotate_left(x->p);
         x = root;
       }
     } else {
       node* w = x->p->left;
       if (w->color == RED) {
         w->color = BLACK;
         x->p->color = RED;
         rotate_right(x->p);
         w = x->p->left;
       }
       if (w->right->color == BLACK && w->left->color == BLACK) {
         w->color = RED;
         x = x->p;
       } else {
         if (w->left->color == BLACK) {
           w->right->color = BLACK;
           w->color = RED;
           rotate_left(w);
           w = x->p->left;
         }
         w->color = x->p->color;
         x->p->color = BLACK;
         w->left->color = BLACK;
         rotate_right(x->p);
         x = root;
       }
     }
   }
   x->color = BLACK;
 }

void make_balance_after_insert(ptr_rbnode node) {
    //case 1 : root node
    if (node->parent == NULL) {
        node->color = BLACK;
        return;
    }
    //case 2 : parent is black
    if (node->parent->color == BLACK) {
        return; //nothing to do
    }
    //now it is guaranteed that node has grandparent
    //case 3 : p and u = red, g = black
    ptr_rbnode g = get_grandparent(node), u = get_uncle(node), p = node->parent;
    assert(g);
    if (p->color == RED && u->color == RED && g->color == BLACK) {
        p->color = BLACK; u->color = BLACK; g->color = RED;
        make_balance_after_insert(g);
        /* this recursive operation will take O(log N) in worst case and O(1) in average case
        because the time complexity distribution would have a form of geometric distribution. */
        return;
    }

    //case 4,5 : p = red, u = black, g = black; -> guaranteed
    //if node, p and g is not on a line, rotate
    //case 4 would be changed into case 5
    assert(p->color == RED && u->color == BLACK && g->color == BLACK);
    if (g->left == p && p->right == node) {
        rotate_left(p);
        node = node->left;
        //for case 5
        g = get_grandparent(node), u = get_uncle(node), p = node->parent;
    }

    else if (g->right == p && p->left == node) {
        rotate_right(p);
        // for case 5
        g = get_grandparent(node), u = get_uncle(node), p = node->parent;
        node = node->right;
    }
    g = get_grandparent(node), u = get_uncle(node), p = node->parent;
    //case 5
    assert((p->left == node && g->left == p)
           || (p->right == node && g->right == p));
    p->color = BLACK;
    g->color = RED;

    if (p->left == node) {
        rotate_right(g);
    }
    else {
        rotate_left(g);
    }
}

void delete_one_child(ptr_rbnode node) {
    ptr_rbnode s = get_sibiling(node), p = node->parent;
    if (p) {
        if (s->color == RED) {
            p->color = RED; s->color = BLACK;

            if (p->left == node) {
                rotate_left(p);
            }
            else {
                rotate_right(p);
            }
        }

        s = get_sibiling(node), p = node->parent;
        if (s->color == BLACK && s->left->color == BLACK && s->right->color == BLACK) {
            if (p->color == BLACK) {
                s->color = RED;
                delete_one_child(p);
            }
            else {
                s->color = RED;
                p->color = BLACK;
            }
        }
        else {
            if (node->parent->left == node && s->right->color == RED) {
                s->color = p->color; p->color = BLACK; s->right->color = BLACK;
                rotate_left(p);
            }
            else if (node->parent->left == node && s->left->color == RED) {
                s->left->color = p->color; p->color = BLACK;
                rotate_right(s);
                rotate_left(s->parent->parent);
            }
            else if (node->parent->right == node && s->left->color == RED) {
                s->color = p->color; p->color = BLACK; s->left->color = BLACK;
                rotate_right(p);
            }
            else if (node->parent->right == node && s->right->color == RED) {
                s->right->color = p->color; p->color = BLACK;
                rotate_left(s);
                rotate_right(s->parent->parent);
            }
            else
                assert(0 && "balance error");
        }

    }
}

static void insert_fix_up(rbtree_t rb,struct rbnode *n)
{
	while(n->parent->color == RED)
	{
		struct rbnode *parent = n->parent;
		struct rbnode *grand_parent = parent->parent;
		if(parent == grand_parent->left)
		{
			struct rbnode *ancle = grand_parent->right;
			if(ancle->color == RED)
			{
				color_flip(grand_parent);
				n = grand_parent;
			}
			else
			{
				if(n == parent->right)
				{
					n = parent;
					rotate_left(rb,n);
				}

				n->parent->color = BLACK;
				n->parent->parent->color = RED;
				rotate_right(rb,n->parent->parent);
			}
		}
		else
		{
			struct rbnode *ancle = grand_parent->left;
			if(ancle->color == RED)
			{
				color_flip(grand_parent);
				n = grand_parent;
			}
			else
			{
				if(n == parent->left)
				{
					n = parent;
					rotate_right(rb,n);
				}
				n->parent->color = BLACK;
				n->parent->parent->color = RED;
				rotate_left(rb,n->parent->parent);
			}
		}
	}
	rb->root->color = BLACK;
}

void redblacktree::red_black_check(rbtnode* act){
  //funkcja sprawdzajaca wlasciwosci czerwono-czarne
  //uwaga hardcore
  while( (act != this->getroot()) && (act->getfather()->getcolor() == 'r') ){
    if(act->getfather() == act->getfather()->getfather()->getleft()){
      rbtnode* uncle = act->getfather()->getfather()->getright();

      if(uncle->getcolor() == 'r'){ // przypadek pierwszy : czerwony wujek
        act->getfather()->setcolor('b');
        uncle->setcolor('b');
        act->getfather()->getfather()->setcolor('r');
        act = act->getfather()->getfather();
        continue;
      }

      if(act == act->getfather()->getright()){ //wujek czarny z prawej, dodany jest prawym synem
        act = act->getfather();
        rotate_left(act);
      }

      act->getfather()->setcolor('b'); //wujek czarny, dodany jest lewym synem
      act->getfather()->getfather()->setcolor('r');
      rotate_right(act->getfather()->getfather());
      break;
    }
        /* Lustrzane przypadki */
    else{
      rbtnode* uncle = act->getfather()->getfather()->getleft();

      if(uncle->getcolor() == 'r'){ //czerwony wujek, symetrycznie
        act->getfather()->setcolor('b');
        uncle->setcolor('b');
        act->getfather()->getfather()->setcolor('r');
        act = act->getfather()->getfather();
        continue;
      }

      if(act == act->getfather()->getleft()){//czarny wujek z lewej, dodany lewym synem
        act = act->getfather();
        rotate_right(act);
      }

      act->getfather()->setcolor('b'); // czarny wujek, dodany prawyym synem
      act->getfather()->getfather()->setcolor('r');
      rotate_left(act->getfather()->getfather());
      break;
    }
  }
  this->getroot()->setcolor('b');
}

/*
*	Balance the tree. Start balancing from the given node.
*/
void AVL_tree::balance(AVL_tree::Node *node) {
	Node *tmp = NULL;
	int diff = 0, diff2 = 0;;

	while (node != NULL) {
		diff = balance_index(node);

		if (diff == 2) { 
			tmp = node->left_child;

			diff2 = balance_index(tmp);

			// Double rotation situation.
			if (diff2 == -1) {
				rotate_left(tmp->right_child);
			}

			rotate_right(node->left_child);
			// node->parent is a new root of the subtree.
			// Update heights to the root.
			update_heights(node->parent);

			break;
		}
		else if (diff == -2) {
			tmp = node->right_child;

			diff2 = balance_index(tmp);

			// Double rotation situation.
			if (diff2 == 1) {
				rotate_right(tmp->left_child);
			}

			rotate_left(node->right_child);
			// node->parent is a new root of the subtree.
			// Update heights to the root.
			update_heights(node->parent);

			break;
		}
		
		// If subtree is balanced, just update node's height.
		// Check parent subtree.
		node->height = 1 + std::max(height(node->left_child),
								 height(node->right_child));
		node = node->parent;
	}
}

node* insert(node *root, int data){  
  if(root && root->data == data)
    return root;
  if(root == NULL){
    root = (node *)malloc(sizeof(node ));
    root->data = data;
    root->h = 1;
    root->left = NULL;
    root->right = NULL;
    return root;
  }
  printf("data: %d ", root->data);
  if(root->data < data){
    root->right = insert(root->right, data);
  }
  else{
    root->left = insert(root->left, data);
  }
  // update the height
  root->h = max(height(root->left), height(root->right)) + 1;
  
  //check if the root node is unbalanced
  // there can be four cases
  int bf = balance_factor(root);
  printf("bf is: %d\n", bf);
  if(bf == 2){
    if(balance_factor(root->left) == -1){
      //left-right case
      printf("left-right case..\n");
      root->left = rotate_left(root->left);
    }
    // left-left case
    printf("left-left case...\n");
    return rotate_right(root);
  }
  else if(bf == -2){
    if(balance_factor(root->right) == 1){
      //the right-left case
      printf("right-left case...\n");
      root->right = rotate_right(root->right);
    }
    // right-right case
    printf("right-right case...\n");
    return rotate_left(root);
  }
  // compiler would not tell you anything if you forget 
  // to write this return statement
  return root;
}

    void restore_properties(Node *node)
    {
        if (parent(node) == nullptr)        // Cenário A: node é a raiz
            node->color = Node::BLACK;
        else if (parent(node)->color == Node::BLACK)    // Cenário B
            return;
        else if (uncle(node) and uncle(node)->color == Node::RED)
        {
            // Cenário C: pai e tio vermelhos
            parent(node)->color = Node::BLACK;
            uncle(node)->color = Node::BLACK;
            grandparent(node)->color = Node::RED;

            // Como o pai é vermelho, o avô não é nulo
            restore_properties(grandparent(node));
        } else
        {
            // Cenário D: pai vermelho, tio preto
            auto C = node;
            auto P = parent(node);
            auto G = grandparent(node);

            if (C == P->right and P == G->left)
            {
                rotate_left(G, P, C);
                P = C;
            } else if (node == P->left and P == G->right)
            {
                rotate_right(G, P, C);
                P = C;
            }

            C = P;
            P = G;
            G = parent(P);

            if (C == P->left)
                rotate_right(G, P, C);
            else
                rotate_left(G, P, C);

            // Corner case: após a rotação C é a nova raiz
            if (G == nullptr)
                root = C;

            C->color = Node::BLACK;
            P->color = Node::RED;
        }
    }

int main(){
    int i1 = rotate_right(0x12345678, 4);
    printf("%x\n", i1);

    // NOTE speical case
    i1 = rotate_right(0x12345678, 20);
    printf("%x\n", i1);

    i1 = rotate_right(0x12345678, 32);
    printf("%x\n", i1);

    i1 = rotate_right(0x12345678, 0);
    printf("%x\n", i1);
    return 0;
}

rbtree_node _rbtree_insert(rbtree_node node,int key,int val){
	if(node == NULL){
		return new_node(key,val,RED,NULL,NULL);
	}
	if(key < node->key)
		node->left = _rbtree_insert(node->left,key,val);
	else if(key > node->key){
		node->right = _rbtree_insert(node->right,key,val);
	}else node->val = val;

	/*
	 * 1. 如果插入到右边,只需要变色.
	 * 2. 如果插入到左结点的左边,右旋,变成情况1.
	 * 3. 如果插入到左结点的右边,左旋,变成情况2.
	 * 根据递归的顺序,可以把这些操作统一,自底向上返回.
	 */

	/* 情况1:强制左倾 */
	if( is_red(node->right) && !is_red(node->left) ){
		node = rotate_left(node);
	}
	/* 情况2:调整平衡 */	
	if( is_red(node->left) && is_red(node->left->left) ){
		node = rotate_right(node);
	}
	/* 情况3:分解4-node */
	if( is_red(node->left) && is_red(node->right) ){
		flip_colors(node);
	}
	return node;
}

/* 删除任意值辅助函数 */
rbtree_node  _rbtree_delete(rbtree_node h,int key){
	if(key < h->key){
		if( !is_red(h->left) && !is_red(h->left->left))
			h = move_red_left(h);
		h->left = _rbtree_delete(h->left,key);
	}else{
		if( is_red(h->left) )
			h = rotate_right(h);
		if( key == h->key && h->right == NULL){
			free(h);
			return NULL;
		}
		if( !is_red(h->right) && !is_red(h->right->left) )
			h = move_red_right(h);
		if( key == h->key ){
			//TODO:获得最小值
			rbtree_node x = _rbtree_min(h->right);
			h->key = x->key;
			h->val = x->val;
			h->right = _rbtree_delete_min(h->right);
		}else{
			h->right = _rbtree_delete(h->right,key);
		}
	}
}

void set_URFtoDLF(CubieCube *cubie, short idx) {
	int x, j, k;
	char corner6[] = { URF, UFL, ULB, UBR, DFR, DLF };
	char other_corner[] = { DBL, DRB };
	int b = idx % 720; // Permutation
	int a = idx / 720; // Combination

	for (j = 0; j < 8; j++) {
		cubie->cp[j] = DRB; // Use DRB to invalidate all corners
	}
	for (j = 1, k; j < 6; j++) { // generate permutation from index b
		k = b % (j + 1);
		b /= j + 1;
		while (k-- > 0) {
			rotate_right(corner6, 0, j);
		}
	}
	// generate combination and set corners
	for (x = 5, j = DRB; j >= 0; j--) {
		if (a - Cnk[j][x + 1] >= 0) {
			cubie->cp[j] = corner6[x];
			a -= Cnk[j][x-- + 1];
		}
	}
	for (x = 0, j = URF; j <= DRB; j++) {
		if (cubie->cp[j] == DRB) {
			cubie->cp[j] = other_corner[x++];
		}
	}
}

void set_URtoDF(CubieCube *cubie, int idx) {
	int x, e, j, k;
	char edge6[] = { UR, UF, UL, UB, DR, DF };
	char other_edge[] = { DL, DB, FR, FL, BL, BR };
	int b = idx % 720; // Permutation
	int a = idx / 720; // Combination

	for (e = 0; e < 12; e++) {
		cubie->ep[e] = BR;// Use BR to invalidate all edges
	}
	for (j = 1, k; j < 6; j++) { // generate permutation from index b
		k = b % (j + 1);
		b /= j + 1;
		while (k-- > 0) {
			rotate_right(edge6, 0, j);
		}
	}
	// generate combination and set edges
	for (x = 5, j = BR; j >= 0; j--) {
		if (a - Cnk[j][x + 1] >= 0) {
			cubie->ep[j] = edge6[x];
			a -= Cnk[j][x-- + 1];
		}
	}
	// set the remaining edges DL..BR
	for (x = 0, j = UR; j <= BR; j++) {
		if (cubie->ep[j] == BR) {
			cubie->ep[j] = other_edge[x++];
		}
	}
}

void delete_case5(rb_node* n, rb_tree* tree) {
	rb_node* s = sibling(n);
	
	/* this if statement is trivial, 
	due to Case 2 (even though Case two changed the sibling to a sibling's child, 
	the sibling's child can't be red, since no red parent can have a red child). */
	/* the following statements just force the red to be on the left of the left of the parent, 
	or right of the right, so case six will rotate correctly. */
	if	(s->color == BLACK) { 
		if ((n == n->parent->left) &&
		(s->right->color == BLACK) &&
		(s->left->color == RED)) { /* this last test is trivial too due to cases 2-4. */
			s->color = RED;
			s->left->color = BLACK;
			rotate_right(s, tree);
		} else if ((n == n->parent->right) &&
		(s->left->color == BLACK) &&
		(s->right->color == RED)) {/* this last test is trivial too due to cases 2-4. */
			s->color = RED;
			s->right->color = BLACK;
			rotate_left(s, tree);
		}
	}
	delete_case6(n, tree);
}

void pulse_fine_rotate_right(){
    rotate_right(9);
    current_pulse_delay = 140;
    start_pulse_timer(40);
    pulse_fn = pulse_fine_rotate_right;
    is_pulsing = true;
}

void erase(Treap* &node, int key)
{
	if(node->key > key)
	{
		erase(node->left, key);
		balance(node);
	}
	else if(node->key < key)
	{
		erase(node->right, key);
		balance(node);
	}
	else
	{
		if(node->left == nil && node->right == nil)
		{
			delete node;
			node = nil;
			return;
		}
		if(node->left->pri > node->right->pri)
		{
			rotate_left(node);
			erase(node->right, key);
			balance(node);
		}
		else
		{
			rotate_right(node);
			erase(node->left, key);
			balance(node);
		}
	}
}

void balance_tree(BinaryTree** tree){
  BinaryTree** child;
  if(balance_factor(*tree) == 2){
    child = &(*tree)->left;
    if(balance_factor(*child) == -1){
      rotate_left(child);
    }
    rotate_right(tree);
  }else if(balance_factor(*tree) == -2){
    child = &(*tree)->right;
    if(balance_factor(*child) == 1){
      rotate_right(child);
    }
    rotate_left(tree);
  }
}

static wtreeNode_t* purge_rc(wtreeRoot_t* root, wtreeNode_t* node) {
	if(!root)
		return NULL;
	if(node->right) {
		node->right = purge_rc(root, node->right);
		node = merge_next(root, node);
	}
	if(node->left) {
		node->left = purge_rc(root, node->left);
		node = merge_prev(root, node);
	}
	if (node->size == node->base_size) {
		if(node->left && (node->left->base_size != node->left->size)) {
			node = rotate_right(node);
			node->right = purge_rc(root, node->right);
		} else if(node->right && (node->right->base_size != node->right->size)) {
			node = rotate_left(node);
			node->left = purge_rc(root, node->left);
		} else if(!node->left && !node->right) {
			if(root->adapter->onfree) {
				if(root->adapter->onremoved) root->adapter->onremoved(node, root->ext_ctx, FALSE);
				root->adapter->onfree(node->top - node->base_size, node->base_size,node, root->ext_ctx);
				return NULL;
			}
		}
	}
	return node;
}

/* Returns bnode if it wasn't freed by merge, left sibling otherwise. */
static bnode_t*
rebalance_bnode(piojo_btree_t *tree, size_t pidx, bnode_t *bnode,
                bnode_t *parent)
{
        bnode_t *lsibling=NULL, *rsibling=NULL;
        if (pidx > 0){
                lsibling = parent->children[pidx - 1];
        }
        if (pidx < parent->ecnt){
                rsibling = parent->children[pidx + 1];
        }
        if (lsibling != NULL && lsibling->ecnt >= tree->cmin){
                PIOJO_ASSERT(bnode->ecnt > 0 && bnode->ecnt < tree->cmax - 1);
                PIOJO_ASSERT(lsibling->ecnt > 1);
                rotate_right(pidx - 1, lsibling, bnode, parent, tree);
        }else if (rsibling != NULL && rsibling->ecnt >= tree->cmin){
                PIOJO_ASSERT(bnode->ecnt > 0 && bnode->ecnt < tree->cmax - 1);
                PIOJO_ASSERT(rsibling->ecnt > 1);
                rotate_left(pidx, bnode, rsibling, parent, tree);
        }else if (lsibling != NULL){
                merge_bnodes(tree, pidx - 1, lsibling, bnode, parent);
                return lsibling;
        }else{
                PIOJO_ASSERT(rsibling);
                merge_bnodes(tree, pidx, bnode, rsibling, parent);
        }
        return bnode;
}