/* swap two nodes' file line, their related nodes(father and children nodes) will also be updated. */
void VSTree::swapNodeFileLine(VNode* _p_node_a, VNode* _p_node_b)
{
    int oldNodeAFileLine = _p_node_a->getFileLine();
    int oldNodeBFileLine = _p_node_b->getFileLine();
    int newNodeAFileLine = oldNodeBFileLine;
    int newNodeBFileLine = oldNodeAFileLine;

    // at first, we should get their fathers' and children's pointer.
    VNode* nodeAFatherPtr = _p_node_a->getFather(*(this->node_buffer));
    int nodeARank = _p_node_a->getIndexInFatherNode(*(this->node_buffer));
    VNode* nodeBFatherPtr = _p_node_b->getFather(*(this->node_buffer));
    int nodeBRank = _p_node_b->getIndexInFatherNode(*(this->node_buffer));
    VNode* nodeAChildPtr[VNode::MAX_CHILD_NUM];
    VNode* nodeBChildPtr[VNode::MAX_CHILD_NUM];

    int nodeAChildNum = _p_node_a->getChildNum();
    int nodeBChildNum = _p_node_b->getChildNum();
    for (int i=0;i<nodeAChildNum;i++)
    {
        nodeAChildPtr[i] = _p_node_a->getChild(i, *(this->node_buffer));
    }
    for (int i=0;i<nodeBChildNum;i++)
    {
        nodeBChildPtr[i] = _p_node_b->getChild(i, *(this->node_buffer));
    }

    // update nodes self file line.
    _p_node_a->setFileLine(newNodeAFileLine);
    _p_node_b->setFileLine(newNodeBFileLine);

    // update nodes' fathers' child file line.
    if (!_p_node_a->isRoot())
    {
        nodeAFatherPtr->setChildFileLine(nodeARank, newNodeAFileLine);
    }
    if (!_p_node_b->isRoot())
    {
        nodeBFatherPtr->setChildFileLine(nodeBRank, newNodeBFileLine);
    }

    // update nodes' children's father file line.
    if (!_p_node_a->isLeaf())
    {
        for (int i=0;i<nodeAChildNum;i++)
        {
            nodeAChildPtr[i]->setFatherFileLine(newNodeAFileLine);
        }
    }
    if (!_p_node_b->isLeaf())
    {
        for (int i=0;i<nodeBChildNum;i++)
        {
            nodeBChildPtr[i]->setFatherFileLine(newNodeBFileLine);
        }
    }

    // update the node_buffer.
    this->node_buffer->update(newNodeAFileLine, _p_node_a);
    this->node_buffer->update(newNodeBFileLine, _p_node_b);
}

// Can pass root as an argument, but will not affect performance much
double DESPOT::WEU(VNode* vnode, double xi) {
	VNode* root = vnode;
	while (root->parent() != NULL) {
		root = root->parent()->parent();
	}
	return Gap(vnode) - xi * vnode->Weight() * Gap(root);
}

//NOTICE:this can only be done by one thread
//write out all the elements to hard disk. 
bool LRUCache::flush()
{
#ifdef DEBUG_VSTREE
	cout<<"to flush in LRUCache"<<endl;
#endif
	FILE* filePtr = fopen(this->dataFilePath.c_str(), "r+b");

	if (filePtr == NULL)
	{
		cerr << "error, can't open file. @LRUCache::flush" << endl;
		return false;
	}

	int startIndex = LRUCache::DEFAULT_NUM;
	int endIndex = startIndex + this->size;
	size_t vNodeSize = VNode::VNODE_SIZE;
	//size_t vNodeSize = sizeof(VNode);

	//NOTICE:values are continuous
	for (int i = startIndex; i < endIndex; ++i)
	{
		VNode* nodePtr = this->values[i];
		int line = this->keys[i];
		//cout<<"file line to write "<<line<<endl;

#ifdef DEBUG
		if (nodePtr->getFileLine() != line)
		{
			cout << "line error at !!!" << line << " " << nodePtr->getFileLine() << endl;
		}
#endif

		if (nodePtr == NULL)
		{
			cerr << "error, VNode do not exist. @LRUCache::flush" << endl;
			return false;
		}

		if(!nodePtr->isDirty())
		{
			continue;
		}

		int flag = 0;
		long long seekPos = (long long)line * vNodeSize;
		flag = fseek(filePtr, seekPos, SEEK_SET);

		if (flag != 0)
		{
			cerr << "error, can't seek to the fileLine. @LRUCache::flush" << endl;
			return false;
		}

		//fwrite((char *)nodePtr, vNodeSize, 1, filePtr);
		nodePtr->writeNode(filePtr);
	}
	fclose(filePtr);

	return true;
}

//create a new node when one node need splitting. 
VNode* 
VSTree::createNode()
{
    VNode* newNodePtr = new VNode();
	int key = -1;
	if(this->free_nid_list.empty())
	{
		key = this->max_nid_alloc++;
		//cout<<"get key by adding "<<key<<endl;
	}
	else
	{
		key = *(this->free_nid_list.begin());
		this->free_nid_list.pop_front();

		//cout<<"createNode() - get key "<<key<<endl;
		//int nkey = *(this->free_nid_list.begin());
		//cout<<"createNode() - next key "<<nkey<<endl;
	}
	//key = this->node_num;
    newNodePtr->setFileLine(key);
    this->node_buffer->set(key, newNodePtr);
    this->node_num++;

    return newNodePtr;
}

std::string VSTree::to_str()
{
    //debug
    {
        stringstream _ss;
        _ss << "after build tree, root is:" << endl;
        _ss << this->getRoot()->to_str() << endl;
        Util::logging(_ss.str());
    }
	std::stringstream _ss;

	 std::queue<int> nodeFileLineQueue;
	 nodeFileLineQueue.push(this->getRoot()->getFileLine());
	 while(! nodeFileLineQueue.empty())
	 {
	        int currentNodeFileLine = nodeFileLineQueue.front();
	        nodeFileLineQueue.pop();
	        VNode* currentNodePtr = this->getNode(currentNodeFileLine);


	        _ss << currentNodePtr->to_str();

	        int childNum = currentNodePtr->getChildNum();
	        for(int i = 0; i < childNum; i ++)
	        {
	        	if(! currentNodePtr->isLeaf())
	        	{
                	int childNodeFileLine = currentNodePtr->getChildFileLine(i);
                	nodeFileLineQueue.push(childNodeFileLine);
	        	}
	       }
	 }

	return _ss.str();
}

//Replace the Entry(_enitty_id)'s EntityBitSet with _bitset
//Entry of _entity_id must exists    
bool 
VSTree::replaceEntry(int _entity_id, const EntityBitSet& _bitset)
{
    VNode* leafNodePtr = this->getLeafNodeByEntityID(_entity_id);

    if (leafNodePtr == NULL)
    {
        cerr << "error, can not find the mapping leaf node. @VSTree::replaceEntry" << endl;
        return false;
    }

    // find the mapping child entry, update it and refresh signature.
    int childNum = leafNodePtr->getChildNum();
    bool findFlag = false;
    for (int i = 0; i < childNum; i++)
    {
        const SigEntry& entry = leafNodePtr->getChildEntry(i);
        if (entry.getEntityId() == _entity_id)
        {
            SigEntry newEntry(EntitySig(_bitset), _entity_id);
            leafNodePtr->setChildEntry(i, newEntry);
            leafNodePtr->refreshAncestorSignature(*(this->node_buffer));
            findFlag = true;
            break;
        }
    }

    if (!findFlag)
    {
        cerr << "error, can not find the mapping child entry in the leaf node. @VSTree::replaceEntry" << endl;
        return false;
    }

    return true;
}

void DESPOT::Update(QNode* qnode) {
	double lower = qnode->step_reward;
	double upper = qnode->step_reward;
	double utility_upper = qnode->step_reward
		+ Globals::config.pruning_constant;

	map<OBS_TYPE, VNode*>& children = qnode->children();
	for (map<OBS_TYPE, VNode*>::iterator it = children.begin();
		it != children.end(); it++) {
		VNode* vnode = it->second;

		lower += vnode->lower_bound();
		upper += vnode->upper_bound();
		utility_upper += vnode->utility_upper_bound;
	}

	if (lower > qnode->lower_bound()) {
		qnode->lower_bound(lower);
	}
	if (upper < qnode->upper_bound()) {
		qnode->upper_bound(upper);
	}
	if (utility_upper < qnode->utility_upper_bound) {
		qnode->utility_upper_bound = utility_upper;
	}
}

LocalVar* find_local_var(const char* name, bool& in_closure) {
   VNode* vnode = getVStack();
   ClosureParseEnvironment* cenv = thread_get_closure_parse_env();
   in_closure = false;

   if (vnode && !vnode->lvar)
      vnode = vnode->nextSearch();

   //printd(5, "find_local_var('%s' %p) vnode: %p\n", name, name, vnode);

   while (vnode) {
      assert(vnode->lvar);
      if (cenv && !in_closure && cenv->getHighWaterMark() == vnode)
	 in_closure = true;

      //printd(5, "find_local_var('%s' %p) v: '%s' %p in_closure: %d match: %d\n", name, name, vnode->getName(), vnode->getName(), in_closure, !strcmp(vnode->getName(), name));

      if (!strcmp(vnode->getName(), name)) {
         //printd(5, "find_local_var() %s in_closure: %d\n", name, in_closure);
         if (in_closure)
	    cenv->add(vnode->lvar);
	 vnode->setRef();
	 return vnode->lvar;
      }
      vnode = vnode->nextSearch();
   }

   //printd(5, "find_local_var('%s' %p) returning 0 NOT FOUND\n", name, name);
   return 0;
}

ValuedAction DESPOT::Evaluate(VNode* root, vector<State*>& particles,
	RandomStreams& streams, POMCPPrior* prior, const DSPOMDP* model) {
	double value = 0;

	for (int i = 0; i < particles.size(); i++) {
		particles[i]->scenario_id = i;
	}

	for (int i = 0; i < particles.size(); i++) {
		State* particle = particles[i];
		VNode* cur = root;
		State* copy = model->Copy(particle);
		double discount = 1.0;
		double val = 0;
		int steps = 0;

		while (!streams.Exhausted()) {
			int action =
				(cur != NULL) ?
					OptimalAction(cur).action : prior->GetAction(*copy);

			assert(action != -1);

			double reward;
			OBS_TYPE obs;
			bool terminal = model->Step(*copy, streams.Entry(copy->scenario_id),
				action, reward, obs);

			val += discount * reward;
			discount *= Discount();

			if (!terminal) {
				prior->Add(action, obs);
				streams.Advance();
				steps++;

				if (cur != NULL && !cur->IsLeaf()) {
					QNode* qnode = cur->Child(action);
					map<OBS_TYPE, VNode*>& vnodes = qnode->children();
					cur = vnodes.find(obs) != vnodes.end() ? vnodes[obs] : NULL;
				}
			} else {
				break;
			}
		}

		for (int i = 0; i < steps; i++) {
			streams.Back();
			prior->PopLast();
		}

		model->Free(copy);

		value += val;
	}

	return ValuedAction(OptimalAction(root).action, value / particles.size());
}

/* create a new node when one node need splitting. */
VNode* VSTree::createNode()
{
    VNode* newNodePtr = new VNode();
    newNodePtr->setFileLine(this->node_num);
    this->node_buffer->set(this->node_num, newNodePtr);
    this->node_num ++;

    return newNodePtr;
}

//choose the best leaf node to insert the _entry, return the choosed leaf node's pointer.  Recursion!
VNode* 
VSTree::chooseNode(VNode* _p_node, const SigEntry& _entry)
{
    if(_p_node->isLeaf())
    {
        return _p_node;
    }
    else
    {
		int minDis = Signature::ENTITY_SIG_LENGTH + 1;
        //int maxDis = Signature::ENTITY_SIG_LENGTH + 1;
        int candidateIndex[VNode::MAX_CHILD_NUM];
        int candidateNum = 0;
        int childNum = _p_node->getChildNum();
        for(int i = 0; i < childNum; i++)
        {
            int curDis = _p_node->getChildEntry(i).xEpsilen(_entry);
            if(minDis >= curDis)
            {
                if(minDis > curDis)
                {
                    minDis = curDis;
                    candidateNum = 0;
                }
                candidateIndex[candidateNum++] = i;
            }
        }

		//NOTICE: the basic idea is to place similar signatures together?(the smaller num?)
		//BETTER: recursion is too costly , and the performance maybe not so good

		minDis = Signature::ENTITY_SIG_LENGTH + 1;
        //maxDis = Signature::ENTITY_SIG_LENGTH + 1;
        VNode* ret = NULL;
        for(int i = 0; i < candidateNum; i++)
        {
        	int child_i = candidateIndex[i];
        	VNode* p_child = _p_node->getChild(child_i, *(this->node_buffer));
			//Recursion 
            VNode *candidateLeafPtr = this->chooseNode(p_child, _entry);
            int curDis = candidateLeafPtr->getEntry().xEpsilen(_entry);

            if(curDis == 0)
            {
                return candidateLeafPtr;
            }

            if(minDis > curDis)
            {
                minDis = curDis;
                ret = candidateLeafPtr;
            }
        }

        return ret;
    }
}

// reiserfs_close_dir
static status_t
reiserfs_close_dir(fs_volume *fs, fs_vnode *_node, void *cookie)
{
	TOUCH(fs); TOUCH(cookie);
//	FUNCTION_START();
	VNode *node = (VNode*)_node->private_node;
FUNCTION(("node: (%Ld: %lu, %lu)\n", node->GetID(), node->GetDirID(),
		  node->GetObjectID()));
	TOUCH(node);
	return B_OK;
}

double AEMS::AEMS2Likelihood(QNode* qnode) {
	VNode* vnode = qnode->parent();
	QNode* qstar = NULL;
	for (int action = 0; action < vnode->children().size(); action++) {
		QNode* child = vnode->Child(action);

		if (qstar == NULL || child->upper_bound() > qstar->upper_bound())
			qstar = child;
	}

	return qstar == qnode;
}

//read the value from hard disk, and put it to the values[_pos].
//before use it, you must make sure that the _pos element in cache is free(unoccupied).
bool LRUCache::readIn(int _pos, int _fileLine)
{
#ifdef DEBUG_LRUCACHE
	//cout<<"pos: "<<_pos<<" "<<"fileline: "<<_fileLine<<endl;
#endif
	VNode* nodePtr = new VNode(true);
	//VNode* nodePtr = NULL;
	FILE* filePtr = fopen(this->dataFilePath.c_str(), "rb");

	//if (nodePtr == NULL)
	//{
		//cerr << "error, can not new a VNode. @LRUCache::readIn" << endl;
		//return false;
	//}

	if (filePtr == NULL)
	{
		cerr << "error, can't open " <<
			"[" << this->dataFilePath << "]" <<
			". @LRUCache::readIn" << endl;
		return false;
	}

	int line = _fileLine;
	size_t vNodeSize = VNode::VNODE_SIZE;
	//size_t vNodeSize = sizeof(VNode);
	int flag = 0;
	long long seekPos = (long long)line * vNodeSize;

	flag = fseek(filePtr, seekPos, SEEK_SET);

	if (flag != 0)
	{
		cerr << "error,can't seek to the fileLine. @LRUCache::readIn" << endl;
		return false;
	}

	//bool is_node_read = (fread((char *)nodePtr, vNodeSize, 1, filePtr) == 1);
	//fread((char *)nodePtr, vNodeSize, 1, filePtr);
	nodePtr->readNode(filePtr);
	fclose(filePtr);

	if (nodePtr == NULL || nodePtr->getFileLine() != _fileLine)
	{
		cout<<"node file line: "<<nodePtr->getFileLine()<<endl;
		cerr << "error,node fileLine error. @LRUCache::readIn" << endl;
	}

	this->setElem(_pos, _fileLine, nodePtr);

	return true;
}

//traverse the tree_node_file_path file, load the mapping from entity id to file line. 
bool 
VSTree::loadEntityID2FileLineMap()
{
    FILE* filePtr = fopen(VSTree::tree_node_file_path.c_str(), "rb");

    if (filePtr == NULL)
    {
        cerr << "error, can not open tree node file. @VSTree::loadEntityID2FileLineMap"  << endl;
        return false;
    }

    size_t vNodeSize = sizeof(VNode);
    int flag = 0;

    flag = fseek(filePtr, 0, SEEK_SET);


    if (flag != 0)
    {
        cerr << "error,can't seek to the fileLine. @VSTree::loadEntityID2FileLineMap" << endl;
        return false;
    }

    this->entityID2FileLineMap.clear();

    VNode* nodePtr = new VNode();
    int cycle_count = 0;
    while (!feof(filePtr))
    {
        bool is_node_read = (fread((char *)nodePtr,vNodeSize,1,filePtr) == 1);
		//NOTICE:not consider invalid node
        if (is_node_read && nodePtr->getFileLine() >= 0)
        {
            this->updateEntityID2FileLineMap(nodePtr);
            //debug
            //{
                //stringstream _ss;
                //if (cycle_count != nodePtr->getFileLine())
                //{
                    //_ss << "line=" << cycle_count << " nodeLine=" << nodePtr->getFileLine() << endl;
                    //Util::logging(_ss.str());
                //}
            //}
            cycle_count ++;
        }
    }
    delete nodePtr;

    fclose(filePtr);

    return true;
}

// reiserfs_free_cookie
static status_t
reiserfs_free_cookie(fs_volume *fs, fs_vnode *_node, void *cookie)
{
	TOUCH(fs);
//	FUNCTION_START();
	VNode *node = (VNode*)_node->private_node;
FUNCTION(("node: (%Ld: %lu, %lu)\n", node->GetID(), node->GetDirID(),
		  node->GetObjectID()));
	TOUCH(node);
	StreamReader *reader = (StreamReader*)cookie;
	delete reader;
	return B_OK;
}

// reiserfs_free_dir_cookie
static status_t
reiserfs_free_dir_cookie(fs_volume *fs, fs_vnode *_node, void *cookie)
{
	TOUCH(fs);
//	FUNCTION_START();
	VNode *node = (VNode*)_node->private_node;
FUNCTION(("node: (%Ld: %lu, %lu)\n", node->GetID(), node->GetDirID(),
		  node->GetObjectID()));
	TOUCH(node);
	DirectoryCookie *iterator = (DirectoryCookie*)cookie;
	delete iterator;
	return B_OK;
}

/* choose the best leaf node to insert the _entry,
 * return the choosed leaf node's pointer.
 * Recursion function! */
VNode* VSTree::chooseNode(VNode* _p_node, const SigEntry& _entry)
{
    if (_p_node->isLeaf())
    {
        return _p_node;
    }
    else
    {
        int minDis = Signature::ENTITY_SIG_LENGTH + 1;
        int candidateIndex[VNode::MAX_CHILD_NUM];
        int candidateNum = 0;
        int childNum = _p_node->getChildNum();
        for (int i=0;i<childNum;i++)
        {
            int curDis = _p_node->getChildEntry(i).xEpsilen(_entry);
            if (minDis >= curDis)
            {
                if (minDis > curDis)
                {
                    minDis = curDis;
                    candidateNum = 0;
                }
                candidateIndex[candidateNum ++] = i;
            }
        }

        minDis = Signature::ENTITY_SIG_LENGTH + 1;
        VNode* ret = NULL;
        for (int i=0;i<candidateNum;i++)
        {
        	int child_i = candidateIndex[i];
        	VNode* p_child = _p_node->getChild(child_i, *(this->node_buffer));
        	/* Recursion */
            VNode *candidateLeafPtr = this->chooseNode(p_child, _entry);
            int curDis = candidateLeafPtr->getEntry().xEpsilen(_entry);

            if (curDis == 0)
            {
                return candidateLeafPtr;
            }

            if (minDis > curDis)
            {
                minDis = curDis;
                ret = candidateLeafPtr;
            }
        }

        return ret;
    }
}

VNode* DESPOT::ConstructTree(vector<State*>& particles, RandomStreams& streams,
	ScenarioLowerBound* lower_bound, ScenarioUpperBound* upper_bound,
	const DSPOMDP* model, History& history, double timeout,
	SearchStatistics* statistics) {
	if (statistics != NULL) {
		statistics->num_particles_before_search = model->NumActiveParticles();
	}

	for (int i = 0; i < particles.size(); i++) {
		particles[i]->scenario_id = i;
	}

	VNode* root = new VNode(particles);

	logd
		<< "[DESPOT::ConstructTree] START - Initializing lower and upper bounds at the root node.";
	InitBounds(root, lower_bound, upper_bound, streams, history);
	logd
		<< "[DESPOT::ConstructTree] END - Initializing lower and upper bounds at the root node.";

	if (statistics != NULL) {
		statistics->initial_lb = root->lower_bound();
		statistics->initial_ub = root->upper_bound();
	}

	double used_time = 0;
	int num_trials = 0;
	do {
		double start = clock();
		VNode* cur = Trial(root, streams, lower_bound, upper_bound, model, history, statistics);
		used_time += double(clock() - start) / CLOCKS_PER_SEC;

		start = clock();
		Backup(cur);
		if (statistics != NULL) {
			statistics->time_backup += double(clock() - start) / CLOCKS_PER_SEC;
		}
		used_time += double(clock() - start) / CLOCKS_PER_SEC;

		num_trials++;
	} while (used_time * (num_trials + 1.0) / num_trials < timeout
		&& (root->upper_bound() - root->lower_bound()) > 1e-6);

	if (statistics != NULL) {
		statistics->num_particles_after_search = model->NumActiveParticles();
		statistics->num_policy_nodes = root->PolicyTreeSize();
		statistics->num_tree_nodes = root->Size();
		statistics->final_lb = root->lower_bound();
		statistics->final_ub = root->upper_bound();
		statistics->time_search = used_time;
		statistics->num_trials = num_trials;
	}

	return root;
}

//Incrementally update bitset of _entity_id
//conduct OR operation on Entry(_entity_id)'s EntityBitSet with _bitset
//Entry of _entity_id must exists    
bool VSTree::updateEntry(int _entity_id, const EntityBitSet& _bitset)
{
    VNode* leafNodePtr = this->getLeafNodeByEntityID(_entity_id);

    if (leafNodePtr == NULL)
    {
        cerr << "error, can not find the mapping leaf node. @VSTree::updateEntry" << endl;
        return false;
    }

    // find the mapping child entry, update it and refresh signature.
    int childNum = leafNodePtr->getChildNum();
    bool findFlag = false;
    for (int i = 0; i < childNum; i++)
    {
        const SigEntry& entry = leafNodePtr->getChildEntry(i);

        if (entry.getEntityId() == _entity_id)
        {
            SigEntry newEntry = entry;
            newEntry |= SigEntry(EntitySig(_bitset), _entity_id);

            //debug
//            {
//                if (_entity_id == 10)
//                {
//                    stringstream _ss;
//                    _ss << "lead node line: " << leafNodePtr->getFileLine() << endl;
//                    _ss << "old entry:\n " << Signature::BitSet2str(entry.getEntitySig().entityBitSet) << endl;
//                    _ss << "new entry:\n " << Signature::BitSet2str(newEntry.getEntitySig().entityBitSet) << endl;
//                    Util::logging(_ss.str());
//                }
//            }

            leafNodePtr->setChildEntry(i, newEntry);
            leafNodePtr->refreshAncestorSignature(*(this->node_buffer));
            findFlag = true;

            break;
        }
    }

    if (!findFlag)
    {
        cerr<< "error, can not find the mapping child entry in the leaf node. @VSTree::updateEntry" << endl;
        return false;
    }

    return true;
}