#include "MarginalTree.h"
#include <stack>
#include <queue>
#include "PhylogenyTreeBasic.h"
#include "UnWeightedGraph.h"
#include "Utils4.h"

////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Define a utility class
// GLobal utility function

//static void OutputQuotedString(ofstream &outFile, const char *buf)
//{
//	outFile << '"';
//	outFile << buf;
//	outFile << '"';
//}

void RemapLeafIntLabelsTaxaMap( MarginalTree &mtree, map<string,string> &mapper )
{
    // map the leaf labels to new integer labels
    for(int i=0; i<mtree.GetNumLeaves(); ++i)
    {
        int lbl = mtree.GetLabel(i);
        char buf[100];
        snprintf(buf, sizeof(buf), "%d",lbl);
        snprintf(buf, sizeof(buf), "%d",lbl);
        string strbuf=buf;
        YW_ASSERT_INFO(mapper.find(strbuf) != mapper.end(), "Fail to find");
        string strLbl = mapper[strbuf];
        int lblNewInt;
        sscanf(strLbl.c_str(), "%d", &lblNewInt);
        mtree.SetLabel(i, lblNewInt);
    }
}

void RemapMargTree( MarginalTree &mtree, TaxaMapper &refTMapper )
{
    //
    // map the leaf labels to new integer labels
//cout << "RemapMargTree: mtree:" << mtree.GetNewick() << endl;
//mtree.Dump();
    for(int i=0; i<mtree.GetNumLeaves(); ++i)
    {
        int lbl = mtree.GetLabel(i);
        string strlbl = refTMapper.GetString( lbl );
        int lblNew = lbl;
        sscanf(strlbl.c_str(),"%d",&lblNew);
        mtree.SetLabel(i, lblNew);
    }
}

static bool ReadinOneMarginalTree( ifstream &inFile, int numNodes, MarginalTree &tree)
{
    // first read in the node ids 
    for( int i=0; i<numNodes; ++i )
    {
        int tmp;
        inFile >> tmp;
        tree.listNodeLabels.push_back( tmp );
    }
    for( int i=0; i<numNodes; ++i )
    {
        int tmp;
        inFile >> tmp;
        tree.listParentNodePos.push_back( tmp );
    }
    for( int i=0; i<numNodes; ++i )
    {
        double tmp;
        inFile >> tmp;
        tree.listEdgeDist.push_back( tmp );
    }

    return true;

}
#if 0
static void ReadNewickLen(const string &strNewick, map< set<int>, double> & mapClusterLen, TaxaMapper *pTMapper )
{
//cout << "ReadNewickLen: strNewick = " << strNewick << endl;
	// the first letter must be (
	//YW_ASSERT_INFO( strNewick.length() > 0 && strNewick[0] == '(', "Bad Newick format" );

	const char *strNwBuf = strNewick.c_str();

	// reverse and find the last ) to get dist
	int posLastState = -1;
	double bLen = 1.0;
	for( int i = (int)strNewick.length()-1; i>=0; --i )
	{
		if(  strNewick[i] == ':'  )
		{
			float fLen = 1.0;
			sscanf(  strNwBuf+i+1, "%f", &fLen );
			bLen = fLen;
			if(strNewick[i] != ')')
			{
				posLastState = i-2;
				break;
			}
		}
		else if( strNewick[i] == ')' )
		{
			// should also stop
			posLastState = i-1;
			break;
		}
	}
	// accumlate all the labels
	PhylogenyTreeBasic phTree;
	phTree.ConsOnNewick(strNewick, -1, false, pTMapper);

	//see if zero is in, if not, must have 1 and decrease by 1
	set<int> lvids;
	phTree.GetLeaveIds(lvids);	
//cout << "ReadNewickLen: lvids = ";
//DumpIntSet( lvids );

	// add a record
	mapClusterLen.insert(  map<set<int>, double> :: value_type( lvids, bLen ) );
//cout << "Subtree len = " << bLen << ", for leaf set = ";
//DumpIntSet( lvids );

	// given newick format, read in the edge length of the clusters
	// we will perform this recursively
	// first find the position where it is the first ,
	int posSplit = -1;
	int netParen = 0;
	for( int i=0; i<(int)strNewick.length(); ++i )
	{
		if( strNewick[i] == '(' )
		{
			netParen ++;
		}
		else if( strNewick[i] == ')' )
		{
			netParen --;
		}
		if( netParen == 1 &&  strNewick[i] == ',' )
		{
			posSplit = i;
			break;
		}
	}
	//YW_ASSERT_INFO( netParen >= 0 && posSplit >= 1, "Bad Newick format" );

	// now recurisvely to two children (if needed)
	if( posSplit >= 0 )
	{
		YW_ASSERT_INFO( posSplit-1 >= 1, "Newick format wrong" );
		string strLeft = strNewick.substr(1, posSplit-1 );
		ReadNewickLen(strLeft, mapClusterLen, pTMapper);
		YW_ASSERT_INFO( posSplit+1 <= posLastState , "Newick format wrong" );
		string strRight = strNewick.substr(posSplit+1, posLastState-posSplit );
		ReadNewickLen(strRight, mapClusterLen, pTMapper);
	}
}
#endif

static int UpdateMTreeWithNWString( MarginalTree &treeToChange, int &leafNext, int &nodeIntNext, string & strNewick, TaxaMapper *pTMapper )
{
//cout << "UpdateMTreeWithNWString: strNewick = " << strNewick  << ", leafNext = " << leafNext << ", nodeIntNext = " << nodeIntNext << endl;
	// a recursive call to change all the nodes from nodeToChnage to the correct length specified
	// by the string strNewick (and all the underlying nodes)
	// return the current node
    
    // conslidate the newick string first
    string strNewickUse = strNewick;
    NewickUtils :: ConsolidateSinglChildChain(strNewickUse);
if( strNewickUse != strNewick)
{
//cout << "**Newick: " << strNewick << ", after consolidate: " << strNewickUse << endl;
}
	// first find current length by finding the rightmost : (outside any ))
	// now find the separator in order to proceed recurisvely
	string strNW1, strNW2;
	bool fNonAtom = NewickUtils :: FindSplitIn(strNewickUse, strNW1, strNW2);
	int nodeCurrent;
	if( fNonAtom == true )
	{
		// now recursive
        if(  nodeIntNext < treeToChange.GetNumLeaves() )
        {
            treeToChange.Dump();
            cout << "nodeIntNext: " << nodeIntNext << ", ";
            cout << "Tree to chagne: " << treeToChange.GetNewick() << endl;
        }
		YW_ASSERT_INFO( nodeIntNext >= treeToChange.GetNumLeaves(), "UpdateBranchLenInfo: internal node out of range"  );
		nodeCurrent = nodeIntNext--;
	}
	else
	{
		YW_ASSERT_INFO( leafNext < treeToChange.GetNumLeaves(), "UpdateBranchLenInfo: Leaf out of range");
		// this node is a leaf
		nodeCurrent = leafNext++;
		
		//int idNew;
		//sscanf(strNewick.c_str(), "%d", &idNew);
		int idNew = TaxaMapper :: GetIdFromStr(strNewickUse, pTMapper);
		treeToChange.SetLabel(nodeCurrent, idNew);
//cout << "leaf id: " << idNew << " for node: " << nodeCurrent << endl;
	}
//cout << "nodeCurrent = " << nodeCurrent << endl;

	// by default, set branch length to be 1.0
	float lenCur = 1.0;
	size_t posSep1 = strNewickUse.rfind(':');
	size_t posSep2 = strNewickUse.rfind(')');
	if( posSep1 != string::npos && (posSep1 > posSep2 || posSep2 == string::npos ) )
	{
		// yes, there is a length specified for this
		sscanf( strNewickUse.c_str()+(int)posSep1+1, "%f", &lenCur  );
	}
	double lenCurUse = lenCur;
	treeToChange.SetBranchLen(nodeCurrent, lenCurUse);
//cout << "Found length: " << lenCurUse << " for node " << nodeCurrent << endl;
	// now recurse
//cout << "In UpdateMTreeWithNWString: \n";
//treeToChange.Dump();
	if( fNonAtom == true )
	{
		// now recursive
		int nodeChild1 = UpdateMTreeWithNWString( treeToChange, leafNext, nodeIntNext, strNW1, pTMapper );
		int nodeChild2 = UpdateMTreeWithNWString( treeToChange, leafNext, nodeIntNext, strNW2, pTMapper );

		// update the par pos
		treeToChange.SetParent(nodeChild1, nodeCurrent, false);
		treeToChange.SetParent(nodeChild2, nodeCurrent, false);
	}

	return nodeCurrent;
}


bool ReadinMarginalTrees( ifstream &inFile, vector<MarginalTree> &treeList)
{
    // first read in the number of chrom
    int numLeaves;
    inFile >> numLeaves;
    int nTreeNodes;
    inFile >> nTreeNodes;
    int nTrees;
    inFile >> nTrees;
    treeList.clear();
    for( int i=0; i<nTrees; ++i )
    {
//cout << "Reading TREE " << i << endl;
        MarginalTree tree;
        ReadinOneMarginalTree(inFile, nTreeNodes, tree);
        tree.numLeaves = numLeaves;
        treeList.push_back( tree );

//tree.Dump();
//YW_ASSERT_INFO(false, "early abort");
    }
    return true;
}

void CollapseEquivTrees(const vector<MarginalTree> &listOrigTrees, vector<MarginalTree> &listUniqTrees, vector<int> &listMultiplicity  )
{
	// collect ordered-leaf tree newick strings
	vector<string> listRepOrderedLeafList;
	for(int tr=0; tr<(int)listOrigTrees.size(); ++tr)
	{
		int numLeaves = listOrigTrees[tr].GetNumLeaves();
// make the tree binary
//listTreesMT[tr].Binarize();
//cout << "Processing gene tree: ";
//listTreesMT[tr].Dump();
		PhylogenyTreeBasic *pphtree = new PhylogenyTreeBasic;
		pphtree->ConsOnParPosList( listOrigTrees[tr].listParentNodePos, numLeaves, true );
		pphtree->UpdateIntLabel(listOrigTrees[tr].listNodeLabels);
		pphtree->Order();
		string strNewick1;
		pphtree->ConsNewick(strNewick1);
		delete pphtree;
//		cout << "Constructed one gene Tree = " << strNewick1 << endl;
		listRepOrderedLeafList.push_back(strNewick1);
//		const int ROOT_LABLE = 7;
//TestReroot(pphtree, ROOT_LABLE);
	}


	listUniqTrees.clear();
	listMultiplicity.clear();
	vector<string> listStoredLeafList;

	// check each tree in orig list
	for(int tr=0; tr<(int)listRepOrderedLeafList.size(); ++tr)
	{
		bool fFound = false;
		for(int trNew=0; trNew <(int)listStoredLeafList.size(); ++trNew)
		{
			if( listStoredLeafList[trNew] == listRepOrderedLeafList[tr] )
			{
				// add it
				listMultiplicity[trNew]++;
				fFound = true;
				break;
			}
		}
		if( fFound == false )
		{
			listUniqTrees.push_back( listOrigTrees[tr] );
			listMultiplicity.push_back(1);
			listStoredLeafList.push_back(listRepOrderedLeafList[tr]);
		}
	}
}



bool ReadinMarginalTreesNewick( ifstream &inFile, int numLeaves, vector<MarginalTree> &treeList, TaxaMapper *pTMapper, bool fDup)
{
	// NOTE: RETURN TRUE IF NO LABEL ADJUSTMENT IS DONE
	// RETURN FALSE IF WE SWITCHED LABEL BY DECREASING BY ONE
	// figure out leave num
	bool fNoChange = true;
	int nLvs = numLeaves;
	

	// read marginal trees in newick format
	// here there is no preamble, one line per tree
	while( inFile.eof() == false )
	{
		string treeNewick;
		inFile >> treeNewick;
		if( treeNewick.size() == 0 )
		{
			break;
		}
//cout << "newick tree = " << treeNewick << endl;

//#if 0
		// update numleaves
		multiset<string> setLabels;
		NewickUtils :: RetrieveLabelSet( treeNewick, setLabels);
		nLvs = setLabels.size();
//#endif
		//
		PhylogenyTreeBasic phTree;
		//if( fDup == false )
		//{
		phTree.ConsOnNewick(treeNewick, -1, false, pTMapper);
		//}
		//else
		//{
		//	phTree.ConsOnNewickDupLabels(treeNewick, pTMapper);
		//}

		if( pTMapper != NULL)
		{
			pTMapper->SetInitialized(true);
		}
//string strTr;
//phTree.ConsNewick(strTr);
//cout << "After reconstruction: strTr = " << strTr << endl;
		//see if zero is in, if not, must have 1 and decrease by 1
		set<int> lvids;
		phTree.GetLeaveIds(lvids);
		if( lvids.find(0) == lvids.end() )
		{
			YW_ASSERT_INFO( lvids.find(1) != lvids.end(), "Wrong" );

			// decrease by one
			phTree.InitPostorderWalk();
			while(true)
			{
				TreeNode *pn = phTree.NextPostorderWalk( ) ;
				if( pn == NULL)
				{
					break;      // done with all nodes
				}
				if( pn->IsLeaf() == true )
				{
//cout << "Found leaf id: " << pn->GetID() << endl;
					pn->SetID( pn->GetID()-1 );
					// YW: 8/18/11, changed. NEED VERIFICATION
					//char buf[1000];
					//sprintf(buf, "%d", pn->GetID() );
					//string lblNew = buf;
					//pn->SetLabel( lblNew );
				}
			}

			// mark the change
			fNoChange = false;
		}

		vector<int> nidsList, nparsList;
		phTree.GetNodeParInfo(nidsList, nparsList);
//cout << "nidsList: ";
//DumpIntVec( nidsList );
//cout << "nparsList" << ": ";
//DumpIntVec(nparsList);
		//phTree.GetNodeParInfoNew(nidsList, nparsList);
		//phTree.GetNodeParInfo(nidsList, nparsList);
		//if( nLvs <= 0 )
		//{
		// YW: 09072010, ASSUME the tree is binary tree
		nLvs = (phTree.GetNumVertices() + 1)/2;
//cout << "nlvs = " << nLvs << endl;
		//}
        MarginalTree tree;
		InitMarginalTree(tree, nLvs, nidsList, nparsList);
//cout << "After init, mtree = ";
//tree.Dump();
		// YW: 01/30/12, sort the leaf first
		tree.SortByLeafId();
//cout << "After sorting, tree = ";
//tree.Dump();

//cout << "Initialize a tree: ";
//tree.Dump();
        treeList.push_back( tree );
//cout << "Newick format of this marginal tree: ";
//cout << tree.GetNewick() << endl;
	}
	return fNoChange;
}

bool ReadinMarginalTreesNewickWLenString( const string &strNewick, int numLeaves, MarginalTree &treeOut, bool fStartFromZero, TaxaMapper *pTMapper )
{
//YW_ASSERT_INFO(pTMapper != NULL, "Stop here2");
	// mark the change
	bool fNoChange = true;
	// NOTE: RETURN TRUE IF NO LABEL ADJUSTMENT IS DONE
	// RETURN FALSE IF WE SWITCHED LABEL BY DECREASING BY ONE
	// figure out leave num

	if( strNewick.size() == 0 )
	{
		return fNoChange;
	}
//cout << "newick tree = " << strNewick << endl;

	// make sure leave num is correct
	if( numLeaves < 0 )
	{
		//
		multiset<string> setLabels;
		NewickUtils :: RetrieveLabelSet( strNewick, setLabels);
		numLeaves = setLabels.size();
//cout << "Set number of leaves of marginal tree to: " << numLeaves << endl;
	}

	int nLvs = numLeaves;

	// assume binary tree for now
	int numTotNodes = 2*nLvs - 1;
	// init Marginal tree for now
	vector<int> trLbls, trPos;
	vector<double> trDist;
	for(int i=0; i<numTotNodes; ++i)
	{
		trLbls.push_back(i);
		trPos.push_back(-1);
		trDist.push_back(0.0);
	}
	treeOut.SetNumLeaves(nLvs);
	treeOut.SetLabelList( trLbls );
	treeOut.SetParList( trPos );
	treeOut.SetBranchLenList( trDist );
	//InitMarginalTree(treeOut, nLvs, trLbls, trPos);
	
	// now update tree
	int leafNext = 0;
	int nodeIntNext = numTotNodes-1;
	string strNewickUse = strNewick;
	UpdateMTreeWithNWString( treeOut, leafNext, nodeIntNext, strNewickUse, pTMapper );
//cout << "Immediate after UpdateMTreeWithNWString: treeOut: \n";
//treeOut.Dump();

	// finally prepare marginal tree for query
	treeOut.BuildDescendantInfo();
//cout << "ReadinMarginalTreesNewickWLenString: newick string = \n" << treeOut.GetNewick() << endl;

	if( pTMapper != NULL)
	{
		pTMapper->SetInitialized(true);
	}

	return fNoChange;
}

void ReadinMarginalTreesNewickWLenString2( const string &strNewick, int numLeaves, MarginalTree &treeOut )
{
    // Jan 10, 2023. YW: trying to fix an error in leaf labels
//YW_ASSERT_INFO(pTMapper != NULL, "Stop here2");
    // mark the change
    // NOTE: RETURN TRUE IF NO LABEL ADJUSTMENT IS DONE
    // RETURN FALSE IF WE SWITCHED LABEL BY DECREASING BY ONE
    // figure out leave num

    if( strNewick.size() == 0 )
    {
        return;
    }
//cout << "newick tree = " << strNewick << endl;

    // make sure leave num is correct
    if( numLeaves < 0 )
    {
        //
        multiset<string> setLabels;
        NewickUtils :: RetrieveLabelSet( strNewick, setLabels);
        numLeaves = setLabels.size();
//cout << "Set number of leaves of marginal tree to: " << numLeaves << endl;
    }

    int nLvs = numLeaves;

    // assume binary tree for now
    int numTotNodes = 2*nLvs - 1;
    // init Marginal tree for now
    vector<int> trLbls, trPos;
    vector<double> trDist;
    for(int i=0; i<numTotNodes; ++i)
    {
        trLbls.push_back(i);
        trPos.push_back(-1);
        trDist.push_back(0.0);
    }
    treeOut.SetNumLeaves(nLvs);
    treeOut.SetLabelList( trLbls );
    treeOut.SetParList( trPos );
    treeOut.SetBranchLenList( trDist );
    //InitMarginalTree(treeOut, nLvs, trLbls, trPos);
    
    // now update tree
    int leafNext = 0;
    int nodeIntNext = numTotNodes-1;
    string strNewickUse = strNewick;
    UpdateMTreeWithNWString( treeOut, leafNext, nodeIntNext, strNewickUse, NULL );
//cout << "Immediate after UpdateMTreeWithNWString: treeOut: \n";
//treeOut.Dump();

    // finally prepare marginal tree for query
    treeOut.BuildDescendantInfo();
cout << "ReadinMarginalTreesNewickWLenString2: newick string = \n" << treeOut.GetNewick() << endl;
}

bool ReadinMarginalTreesNewickWLen( ifstream &inFile, int numLeaves, vector<MarginalTree> &treeList, TaxaMapper *pTMapper)
{
//YW_ASSERT_INFO(pTMapper != NULL, "Stop here");
	// NOTE: RETURN TRUE IF NO LABEL ADJUSTMENT IS DONE
	// RETURN FALSE IF WE SWITCHED LABEL BY DECREASING BY ONE
	// figure out leave num
	bool fNoChange = true;
	//int nLvs = numLeaves;

	// read marginal trees in newick format
	// here there is no preamble, one line per tree
	while( inFile.eof() == false )
	{
		string treeNewick;
		inFile >> treeNewick;
		if( treeNewick.size() == 0 )
		{
			break;
		}
		MarginalTree tree;
		bool fres = ReadinMarginalTreesNewickWLenString(treeNewick, numLeaves, tree, true, pTMapper );
		if( fres == false)
		{
			fNoChange = false;
		}
		if( pTMapper != NULL)
		{
			pTMapper->SetInitialized(true);
		}

//cout << "Initialize a tree: ";
//tree.Dump();
        treeList.push_back( tree );
	}
	return fNoChange;
}


void AddRootAsLeafToTree( MarginalTree &tree1, bool fIdNonNeg )
{
//cout << "AddRootAsLeafToTree: tree1 = \n";
//tree1.Dump();
    // we now add the root to the tree as a special leaf
    vector<int> nodesIdNew, nodesParsNew;
    for( int i=0; i<tree1.GetNumLeaves(); ++i )
    {
        nodesIdNew.push_back( tree1.listNodeLabels[ i ]   );
        nodesParsNew.push_back( tree1.GetParent(i) + 1 );
    }
    // add the new special leaf
	int idLeafNew = -2; // -2 is the default unique id for this speical node
	int idNewStart = 3*tree1.GetNumLeaves()+1;
	if( fIdNonNeg == true)
	{
		// use  continuous id
		idLeafNew = tree1.GetNumLeaves();
	}
    nodesIdNew.push_back( idLeafNew ); 
    nodesParsNew.push_back( tree1.GetTotNodesNum() + 1  );
    // add the rest
    for( int i= tree1.GetNumLeaves(); i< tree1.GetTotNodesNum();  ++i )
    {
        nodesIdNew.push_back( tree1.listNodeLabels[ i ]   );
        int oldpar = tree1.GetParent( i );
        if( oldpar < 0 )
        {
            // this is the old root
            nodesParsNew.push_back( tree1.GetTotNodesNum() + 1 );
        }
        else
        {
            nodesParsNew.push_back( oldpar + 1);
        }
    }
    // finally the new root
	int idRootId = -3;  // -3 is the unique id for this speical root
	if( fIdNonNeg == true)
	{
		// use  it
		idRootId = ++idNewStart;
	}    
	nodesIdNew.push_back( idRootId ); 
    nodesParsNew.push_back( -1  );

    // finally increment the number of leaves
    tree1.listNodeLabels = nodesIdNew;
    tree1.listParentNodePos = nodesParsNew;
    tree1.numLeaves++;
//cout << "After adding the root, now tree1 = \n";
//tree1.Dump();
}


void GenRandBinaryTree( int numLeaves, MarginalTree &tree1)
{
    // generate a binary marginal tree with certain number of leaves
    // we do this by random pick two active nodes (a leave without assiging parents)
    tree1.Clear();
    tree1.numLeaves = numLeaves;

    // first add a list of leaves
    set<int> activeNodes;
    for(int i=0; i<numLeaves; ++i)
    {
        tree1.listNodeLabels.push_back( i );
        tree1.listParentNodePos.push_back( -1 );        // for now, set to -1 (un-initialized)
        tree1.listEdgeDist.push_back( 0.0);
        activeNodes.insert(i);
    }

    // now start to setup new internal nodes (and assign parents)
    while(activeNodes.size() >= 2 )
    {
//cout << "activeNodes = ";
//DumpIntSet( activeNodes );

        // uniformly pick two nodes
        int node1 = GetRandItemInSet(activeNodes);
        activeNodes.erase( node1 );
        int node2 = GetRandItemInSet(activeNodes);
        activeNodes.erase( node2 );
//cout << "Select node1 = " << node1 << ", node2 = " << node2 << endl;     
        // now create a new node
        int nodeNew = tree1.listNodeLabels.size();
        tree1.listNodeLabels.push_back( nodeNew );
        tree1.listParentNodePos.push_back( -1 );        // for now, set to -1 (un-initialized)
        tree1.listEdgeDist.push_back( 0.0);
        activeNodes.insert( nodeNew );
//cout << "nodeNew = " << nodeNew << endl;
        // setup parent of two children to it
        tree1.SetParent( node1, nodeNew );
        tree1.SetParent( node2, nodeNew );
    }

}


void GenRandBinaryTreeClock( int numLeaves, double totHt, MarginalTree &tree1)
{
    // generate a binary marginal tree with certain number of leaves and have clock property
    // we do this by random pick two active nodes (a leave without assiging parents)
    map<int, double> mapNodeHeights;
    
    tree1.Clear();
    tree1.numLeaves = numLeaves;
    
    // first add a list of leaves
    set<int> activeNodes;
    for(int i=0; i<numLeaves; ++i)
    {
        tree1.listNodeLabels.push_back( i );
        tree1.listParentNodePos.push_back( -1 );        // for now, set to -1 (un-initialized)
        tree1.listEdgeDist.push_back( 0.0);
        activeNodes.insert(i);
        mapNodeHeights.insert( map<int,double> :: value_type(i, 0.0));
    }
    
    // now start to setup new internal nodes (and assign parents)
    while(activeNodes.size() >= 2 )
    {
        //cout << "activeNodes = ";
        //DumpIntSet( activeNodes );
        
        // uniformly pick two nodes
        int node1 = GetRandItemInSet(activeNodes);
        activeNodes.erase( node1 );
        int node2 = GetRandItemInSet(activeNodes);
        activeNodes.erase( node2 );
        //cout << "Select node1 = " << node1 << ", node2 = " << node2 << endl;
        // now create a new node
        int nodeNew = tree1.listNodeLabels.size();
        tree1.listNodeLabels.push_back( nodeNew );
        tree1.listParentNodePos.push_back( -1 );        // for now, set to -1 (un-initialized)
        tree1.listEdgeDist.push_back( 0.0);
        activeNodes.insert( nodeNew );
        double htNodeCur = totHt*(numLeaves-(double)activeNodes.size())/( numLeaves-1 );
//cout << "Node: " << nodeNew << ", ht = " << htNodeCur << endl;
        // set branches
        mapNodeHeights.insert( map<int,double> :: value_type(nodeNew, htNodeCur));
        YW_ASSERT_INFO( mapNodeHeights.find( node1 ) != mapNodeHeights.end(), "Not found");
        YW_ASSERT_INFO( node1 <(int)tree1.listEdgeDist.size(), "Wrong");
        tree1.listEdgeDist[node1] = htNodeCur - mapNodeHeights[node1];
//cout << "Setting edge " << node1 << " to " <<  tree1.listEdgeDist[node1] << endl;
        YW_ASSERT_INFO( tree1.listEdgeDist[node1] >=0.0, "Negative");
        YW_ASSERT_INFO( mapNodeHeights.find( node2 ) != mapNodeHeights.end(), "Not found");
        YW_ASSERT_INFO( node2 <(int)tree1.listEdgeDist.size(), "Wrong");
        tree1.listEdgeDist[node2] = htNodeCur - mapNodeHeights[node2];
        YW_ASSERT_INFO( tree1.listEdgeDist[node2] >=0.0, "Negative");
//cout << "Setting edge " << node2 << " to " <<  tree1.listEdgeDist[node2] << endl;
        //cout << "nodeNew = " << nodeNew << endl;
        // setup parent of two children to it
        tree1.SetParent( node1, nodeNew, false );
        tree1.SetParent( node2, nodeNew, false );
    }
    
//cout << "Edge dist list: ";
//DumpDoubleVec(tree1.listEdgeDist);
    
}

// find a chain with specified length
static bool FindChainAtNodeInTree( const MarginalTree &tree1, int nodeHead, int lenChain, vector<int> &leaves, 
                                  vector<int> &leaves2 )
{
    leaves.clear();
    leaves2.clear();

    int curn = nodeHead;
    for( int i=0; i<lenChain; ++i )
    {
//cout << "curn = " << curn << endl;
        bool fLeftLeave =  tree1.IsLeaf( tree1.GetLeftDescendant(curn) )  ;
        bool fRightLeave =  tree1.IsLeaf( tree1.GetRightDescendant(curn) )  ;
        if(  (fLeftLeave == true && fRightLeave == true  && i < lenChain-2 )
            || (fLeftLeave == false && fRightLeave == false)  )
        {
//cout << "Fail\n";
            return false;
        }
        // now move down
        if(  fLeftLeave == false  )
        {
            // put right to store
            int child = tree1.GetRightDescendant(curn);
            YW_ASSERT_INFO( tree1.IsLeaf( child ), "Not a leaf" );
            leaves.push_back(child);
            curn = tree1.GetLeftDescendant(curn);
        }
        else if( fRightLeave == false )
        {
            int child = tree1.GetLeftDescendant(curn);
            YW_ASSERT_INFO( tree1.IsLeaf( child ), "Not a leaf" );
            leaves.push_back(child);
            curn = tree1.GetRightDescendant(curn);
        }
        else
        {
            YW_ASSERT_INFO( i >= lenChain-2, "wrong1" );
            //YW_ASSERT_INFO( tree1.IsLeaf( curn ) == false, " a leaf" );
            // in this case, we just save itself
            if( i == lenChain-2 )
            {
                leaves2 = leaves;
                leaves.push_back( tree1.GetLeftDescendant(curn)   );
                leaves.push_back( tree1.GetRightDescendant(curn)   );
                leaves2.push_back( tree1.GetRightDescendant(curn)   );
                leaves2.push_back( tree1.GetLeftDescendant(curn)   );
                break;
            }
            else
            {
                // only save one, try both possibilities
                leaves2 = leaves;
                leaves.push_back( tree1.GetLeftDescendant(curn)   );
                // save leaves2 
                leaves2.push_back( tree1.GetRightDescendant(curn)   );
                break;
            }
        }
    }
    return true;
}

// here, we simply found chain of fixed length (i.e. 4)
void FindChainsInTree( const MarginalTree &tree1,  map< vector<int>, int > &foundChains  )
{
    // 
    foundChains.clear();

    // we simply enumerate all internal node and trace down from it
    for( int nn = tree1.GetNumLeaves(); nn < tree1.GetTotNodesNum(); ++nn  )
    {
        // is this nn a chain-hnead of 4 leaves on one side?
        vector<int> listLeaves, listLeaves2;
        if( FindChainAtNodeInTree(tree1, nn, 4, listLeaves , listLeaves2) == true  )
        {
//cout << "Found one chain at node nn = " << nn << ", with leaves = ";
//DumpIntVec( listLeaves);
            foundChains.insert( map< vector<int>, int > :: value_type( listLeaves, nn )  );

            if(  listLeaves2.size() > 0 )
            {
//cout << "Found one chain at node nn = " << nn << ", with leaves = ";
//DumpIntVec( listLeaves2);
                foundChains.insert( map< vector<int>, int > :: value_type( listLeaves2, nn )  );
            }
        }
    }
}

// construct a marginal tree from nodes and parent info
// NOTE: this function does not take distance. Therefore, we arbitarily assign nodes to their respective 
// heights and thus also assign branch length
// ALSO NOTE: when we assign branch length, the branch length are set uniformly distributed within [0-1].
void InitMarginalTree(MarginalTree &mTree, int numLeaves, const vector<int> &listLabels, const vector<int> &listParentNodePos )
{
//cout << "numLeaves = " << numLeaves << endl;
//cout << "InitMarginalTree: numLeaves = " << numLeaves << endl;
//cout << "listLabels = ";
//DumpIntVec(listLabels);
//cout << "listParentNodePos = ";
//DumpIntVec( listParentNodePos );
	//
	mTree.numLeaves = numLeaves;
    mTree.listNodeLabels = listLabels;
    mTree.listParentNodePos = listParentNodePos;

	// now init edge dist
	mTree.listEdgeDist.clear();
	int numNonLeafNodes = listLabels.size() - numLeaves;
	double unitLen = 1.0/numNonLeafNodes;
	for( int i=0; i<(int) listLabels.size()-1; ++i )
	{
		int parPos = listParentNodePos[i] - numLeaves + 1;
//cout << "par = " << listParentNodePos[i]  << " for node i = " << i << endl;
//cout << "normalized par pos = " << parPos << endl;
		YW_ASSERT_INFO( parPos > 0, "Fatal error in InitMarginalTree" );
		if( i < numLeaves )
		{
			// leaf
			mTree.listEdgeDist.push_back( parPos *unitLen    );
		}
		else
		{
			// need to subtract current pos
			int curpos = i - numLeaves + 1;
//cout << "curpos = " << curpos << endl;
			YW_ASSERT_INFO( curpos < parPos, "Trouble in InitMarginalTree" );
			mTree.listEdgeDist.push_back(  (parPos-curpos) *unitLen    );
		}
	}
	// the root has length-0 by default
	mTree.listEdgeDist.push_back(  0.0    );
	// also build up descendents
	mTree.BuildDescendantInfo();
}


// find the neighborhood of marginal trees within one NNI operation away (incl. the current tree)
void FindOneNNIMTreesFrom(MarginalTree &mTreeSrc, vector<MarginalTree> &listNNITrees, vector< pair<int,int> > *pListPairEdgesSwapped)
{
	// 
	listNNITrees.clear();

	// process each internal node (w/ at least three leaves below) of the mtree, and
	for(int node = mTreeSrc.GetNumLeaves(); node<mTreeSrc.GetTotNodesNum(); ++node)
	{
		// 
		int nodeLeft = mTreeSrc.GetLeftDescendant(node);
		int nodeRight = mTreeSrc.GetRightDescendant(node);
		if( mTreeSrc.IsLeaf(nodeLeft) == true && mTreeSrc.IsLeaf(nodeRight) == true )
		{
			// skip if both children are leaves since in this case swapping has no effect
			continue;
		}
		// now swap its two children's subtree in up to four ways
		int nodesProc1[2], nodesProc2[2];
		nodesProc1[0] = nodeLeft;
		nodesProc1[1] = nodeRight;
		nodesProc2[1] = nodeLeft;
		nodesProc2[0] = nodeRight;
		for(int ii=0; ii<2; ++ii)
		{
			int n1Proc = nodesProc1[ii];
			int n2Proc = nodesProc2[ii];
			if( mTreeSrc.IsLeaf(n1Proc) == false )
			{
				int node1Left = mTreeSrc.GetLeftDescendant(n1Proc);
				int node1Right = mTreeSrc.GetRightDescendant(n1Proc);
				YW_ASSERT_INFO(node1Left >=0 && node1Right >= 0, "Can not miss");

				// two choices to swap: n2Proc with one of the descendents
				int nodesProc1Child[2];
				nodesProc1Child[0] = node1Left;
				nodesProc1Child[1] = node1Right;
				for(int jj=0; jj<2; ++jj)
				{
					MarginalTree mtreeNNI1 = mTreeSrc;
					mtreeNNI1.SwapBranches(nodesProc1Child[jj], n2Proc);
					mtreeNNI1.BuildDescendantInfo();
//cout << "After swap: \n";
//mtreeNNI1.Dump();
					mtreeNNI1.RearrangeParIncOrder();
//cout << "Found a new mtreeNNI1: " << mtreeNNI1.GetNewick() << endl;
//mtreeNNI1.Dump();
					mtreeNNI1.BuildDescendantInfo();
                    // sort by leaf id: YW: Feb 19,2016
                    mtreeNNI1.SortByLeafId();
                    mtreeNNI1.BuildDescendantInfo();
					listNNITrees.push_back(mtreeNNI1);
                    
                    if( pListPairEdgesSwapped != NULL )
                    {
                        pair<int,int> pp( nodesProc1Child[jj], n2Proc );
                        pListPairEdgesSwapped->push_back(pp);
                    }
//cout << "After descendent rebult, " << mtreeNNI1.GetNewick() << endl;
//mtreeNNI1.Dump();
				}
			}
		}
	}
	// finally add self
	listNNITrees.push_back( mTreeSrc );
//exit(1);
}

void CreateSubtreeFromLeaves( MarginalTree &mTreeOrig, const set<int> &setLeafLabels, MarginalTree &mTreeSub, map<int,int> &mapNewNodeToOldNode )
{
//cout << "Original tree: " << mTreeOrig.GetNewick() << ": set of leaves to process: ";
//DumpIntSet( setLeafLabels );
    
    // find a subset of trees with the desired leaves (as matching the given labels)
    // mapNewNodeToOldNode: new node index ==> old node index
    map< pair<int,set<int> >, int > mapShrunkLeavesWithNum;
    
    // get all the clades
    for(int i=0; i<mTreeOrig.GetTotNodesNum(); ++i)
    {
        //
        set<int> setGetDesc;
        mTreeOrig.GetLeavesUnder( i, setGetDesc );
        set<int> setGetDescLbls;
        for(set<int> :: iterator it = setGetDesc.begin(); it != setGetDesc.end(); ++it)
        {
            int lbl = mTreeOrig.GetLabel(*it);
            setGetDescLbls.insert(lbl);
        }
        set<int> sIntsect;
        JoinSets( setGetDescLbls, setLeafLabels, sIntsect );
        
        // ignore empty nodes
        if( sIntsect.size() <= 0 )
        {
            //
            continue;
        }
        
        
        // save it
        pair<int,set<int> > ss( sIntsect.size(), sIntsect );
        if( mapShrunkLeavesWithNum.find(ss) == mapShrunkLeavesWithNum.end() )
        {
            mapShrunkLeavesWithNum.insert(  map< pair<int,set<int> >, int > :: value_type(ss, i) );
        }
        else
        {
            // save the lower (smaller)
            if( mapShrunkLeavesWithNum[ss] > i )
            {
                mapShrunkLeavesWithNum[ss] = i;
            }
        }
    }
#if 0
cout << "mapShrunkLeavesWithNum: ";
for( map< pair<int,set<int> >, int > :: iterator it = mapShrunkLeavesWithNum.begin(); it != mapShrunkLeavesWithNum.end(); ++it )
{
cout << "Size: " << it->first.first << ", orig. node = " << it->second << ", set of leaves: ";
DumpIntSet( it->first.second);
}
#endif
    
    // set up the old and new node position map
    map<int,int> mapNewToOldPos, mapOldToNewPos;
    set<int> setNewParsPosOld;
    int index = 0;
    for(map< pair<int,set<int> >, int > :: iterator it = mapShrunkLeavesWithNum.begin(); it != mapShrunkLeavesWithNum.end(); ++it, ++index)
    {
        //
        mapNewToOldPos.insert(map<int,int> :: value_type(index, it->second));
        mapOldToNewPos.insert(map<int,int> :: value_type(it->second, index));

        setNewParsPosOld.insert( it->second );
    }
    
    
    // now init the tree: note edge labels are ignored!
    mTreeSub.Clear();
    mTreeSub.SetNumLeaves( setLeafLabels.size() );
    vector<int> listLbls;
    PopulateVecBySet( listLbls, setLeafLabels );
    for(int i=(int)setLeafLabels.size(); i<(int)mapShrunkLeavesWithNum.size(); ++i)
    {
        // these are internal nodes
        listLbls.push_back(-1);
    }
    mTreeSub.SetLabelList(listLbls);
    vector<int> listParPos;
    // now set up parent
    for(int i=0; i<(int)listLbls.size(); ++i)
    {
        YW_ASSERT_INFO(mapNewToOldPos.find(i) != mapNewToOldPos.end(), "Fail to find2");
        int posOrig = mapNewToOldPos[i];
        int anc = mTreeOrig.GetFirstNonselfAnces(posOrig, setNewParsPosOld);
        int posNewAnc = -1;
        if( anc >= 0 )
        {
            YW_ASSERT_INFO( mapOldToNewPos.find( anc) != mapOldToNewPos.end(), "Fail to find3");
            posNewAnc = mapOldToNewPos[anc];
        }
        listParPos.push_back(posNewAnc);
    }
    mTreeSub.SetParList( listParPos );
    
    // create nodes mapping
    mapNewNodeToOldNode = mapNewToOldPos;
    
    //
    mTreeSub.BuildDescendantInfo();
    
    // YW: how do we assign branch length
    UpdateBranchLenInSubtree( mTreeOrig, mapNewNodeToOldNode, mTreeSub );
#if 0
cout << "Constructed subtree: " << mTreeSub.GetNewick() << endl;
mTreeSub.Dump();
cout << "mapNewNodeToOldNode: ";
for(map<int,int> :: iterator it=mapNewNodeToOldNode.begin(); it != mapNewNodeToOldNode.end(); ++it)
{
cout << "[" << it->first << "," << it->second << "]  ";
}
cout << endl;
#endif
}

void UpdateBranchLenInSubtree( MarginalTree &mTreeOrig, map<int,int> &mapNewNodeToOldNode, MarginalTree &mTreeSub )
{
    // inverse map
    //map<int,int> mapOldNodeToNewNode;
    //for( map<int,int> :: iterator it = mapNewNodeToOldNode.begin(); it != mapNewNodeToOldNode.end(); ++it  )
    //{
    //    //
    //    YW_ASSERT_INFO( mapOldNodeToNewNode.find(it->second) == mapOldNodeToNewNode.end(), "Wrong" );
    //    mapOldNodeToNewNode.insert( map<int,int> :: value_type(it->second, it->first) );
    //}
    
    //
    vector<double> listBrLens;
    for( map<int,int> :: iterator it = mapNewNodeToOldNode.begin(); it != mapNewNodeToOldNode.end(); ++it )
    {
        double distcur = 0.0;
        //
        int pnew = it->first;
        int pold = it->second;
        int pnewpar = mTreeSub.GetParent(pnew);
        if( pnewpar >= 0 )
        {
            YW_ASSERT_INFO( mapNewNodeToOldNode.find( pnewpar ) != mapNewNodeToOldNode.end(), "Fail to find");
            int poldpar = mapNewNodeToOldNode[pnewpar];
            distcur = mTreeOrig.GetPathLen( pold, poldpar );
        }
        
        listBrLens.push_back(distcur);
    }
    mTreeSub.SetBranchLenList( listBrLens );
}

void FindMatchedSubtrees( MarginalTree &mtreeNew, MarginalTree &mtreeRef, map<int,int> &mapSTNewToRef )
{
    // find the shared subtrees that are in both trees, then create a map: map the subtree index in mtreeNew to mtreeRef
    // find all branches (subtrees below them) that are not in the reference tree
    // setDiffBrs: in this tree but not in reference tree
    // setDiffRefMissed: in reference tree but not in this tree
    vector< set<int> > listSubtreesNew, listSubtreesRef;
    mtreeNew.ConsDecedentLeavesInfoLabels( listSubtreesNew );
    mtreeRef.ConsDecedentLeavesInfoLabels( listSubtreesRef );

    // create fast searching
    map<set<int>, int> mapIndexSTRef;
    for(int i=0; i<(int)listSubtreesRef.size(); ++i)
    {
        mapIndexSTRef.insert( map<set<int>, int> :: value_type( listSubtreesRef[i], i ) );
    }
    
    //
    mapSTNewToRef.clear();
    for(int i=0; i<(int)listSubtreesNew.size(); ++i)
    {
        if( mapIndexSTRef.find( listSubtreesNew[i] ) == mapIndexSTRef.end()  )
        {
            mapSTNewToRef.insert(  map<int,int> :: value_type( i, mapIndexSTRef[ listSubtreesNew[i] ] ) );
        }
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Define a utility class

MarginalTree :: MarginalTree()  : numLeaves( 0 )
{
    // here, we initiailize distance
    // TBD
}

void MarginalTree :: Clear()
{
    numLeaves = 0;
    listNodeLabels.clear();
    listParentNodePos.clear();
    listEdgeDist.clear();
    listLeftDescs.clear();
    listRightDescs.clear();
}


void MarginalTree :: BuildDescendantInfo()
{
    // Note, this only works for binary tree!!!!!
    listLeftDescs.clear();
    listRightDescs.clear();
    int numNodes = GetTotNodesNum();
//cout << "BuildDescendantInfo: numNodes: " << numNodes << endl;
    listLeftDescs.resize( numNodes );
    listRightDescs.resize( numNodes );

    // for leaves, there is no children
    for( int i=0; i<numNodes; ++i )
    {
        listLeftDescs[i] = -1;
        listRightDescs[i] = -1;
    }

    // handle other cases
    for( int i=0; i<numNodes; ++i )
    {
        int p = GetParent(i);
//cout << "paret of " << i << " is " << p << endl;
        if( p < 0 )
        {
            continue;
        }
        // setup p's child to i
        if( listLeftDescs[p] < 0 )
        {
            listLeftDescs[p] = i;
//cout << "Set left descendent of " << p << " to be " << i << endl;
        }
        else
        {
if( listRightDescs[p] >= 0)
{
cout << "Something wrong: the current tree:";
Dump();
}
            
            // make sure this is binary tree
            YW_ASSERT_INFO( listRightDescs[p] < 0, "Not a binary tree2" );
            listRightDescs[p] = i;
//cout << "Set right descendent of " << p << " to be " << i << endl;
        }
    }
}

void MarginalTree :: BuildDescendantInfo2()
{
    // Note, this only works for binary tree!!!!!
    listLeftDescs.clear();
    listRightDescs.clear();
    int numNodes = GetTotNodesNum();
//cout << "BuildDescendantInfo: numNodes: " << numNodes << endl;
    listLeftDescs.resize( numNodes );
    listRightDescs.resize( numNodes );

    // for leaves, there is no children
    for( int i=0; i<numNodes; ++i )
    {
        listLeftDescs[i] = -1;
        listRightDescs[i] = -1;
    }

    // get ordered nodes
    vector<int> listOrderedNodes;
    GetAllNodesBottomUpFrom(GetRoot(), listOrderedNodes);
    
    // handle other cases
    for( int index=0; index<numNodes; ++index )
    {
        int i = listOrderedNodes[index];
        int p = GetParent(i);
//cout << "paret of " << i << " is " << p << endl;
        if( p < 0 )
        {
            continue;
        }
        // setup p's child to i
        if( listLeftDescs[p] < 0 )
        {
            listLeftDescs[p] = i;
//cout << "Set left descendent of " << p << " to be " << i << endl;
        }
        else
        {
if( listRightDescs[p] >= 0)
{
cout << "Something wrong: the current tree:";
Dump();
}
            
            // make sure this is binary tree
            YW_ASSERT_INFO( listRightDescs[p] < 0, "Not a binary tree2" );
            listRightDescs[p] = i;
//cout << "Set right descendent of " << p << " to be " << i << endl;
        }
    }
}

bool MarginalTree :: IsToplogicSame( const MarginalTree &tree ) const
{
    // this function test whether two things are topologically the same
    if( GetTotNodesNum() != tree.GetTotNodesNum()  )
    {
//cout << "Tree node numbers are not equal\n";
        // nodes number are different, then different
        return false;
    }
    // 
    if( GetNumLeaves() != tree.GetNumLeaves()  )
    {
//cout << "Tree leaf numbers are not equal\n";
        return false;
    }
    // make sure node id the same
    //if( listNodeLabels != tree.listNodeLabels )
    //{
//cout << "Tree node ids are not equal\n";
//        return false;
//    }
#if 0
    if( listParentNodePos != tree.listParentNodePos  )
    {
//cout << "Tree node parents are not equal\n";
        return false;
    }
#endif

	// sort the leaves
	//MarginalTree t1 = *this;
	//MarginalTree t2 = tree;
	//t1.SortByLeafId();
	//t2.SortByLeafId();
	vector<set<int> > t1splits, t2splits;
	ConsDecedentLeavesInfo( t1splits );
	tree.ConsDecedentLeavesInfo( t2splits );
	set<set<int> > st1splits, st2splits;
	for(int i=0; i<(int)t1splits.size(); ++i )
	{
		st1splits.insert(t1splits[i]);
	}
	for(int i=0; i<(int)t2splits.size(); ++i )
	{
		st2splits.insert(t2splits[i]);
	}

    if( st1splits != st2splits  )
    {
//cout << "Tree node parents are not equal\n";
//cout << "** tree 1: \n";
//for(int i=0; i<(int)t1splits.size(); ++i )
//{
//DumpIntSet(t1splits[i]);
//}
//cout << "** tree 2: \n";
//for(int i=0; i<(int)t2splits.size(); ++i )
//{
//DumpIntSet(t2splits[i]);
//}
        return false;
    }

    return true;
}

int MarginalTree :: GetLeftDescendant( int node ) const
{
    YW_ASSERT_INFO((int)listLeftDescs.size() == GetTotNodesNum() && 
        (int)listRightDescs.size() == GetTotNodesNum(), "descendant info not set");
    return listLeftDescs[node];
}
int MarginalTree :: GetRightDescendant( int node ) const
{
    YW_ASSERT_INFO((int)listLeftDescs.size() == GetTotNodesNum() && 
        (int)listRightDescs.size() == GetTotNodesNum(), "descendant info not set");
    return listRightDescs[node];
}

void MarginalTree :: SetLeftDescendant(int node, int nodeLeftDesc)
{
    YW_ASSERT_INFO((int)listLeftDescs.size() == GetTotNodesNum() &&
        (int)listRightDescs.size() == GetTotNodesNum(), "descendant info not set");
    listLeftDescs[node] = nodeLeftDesc;
}
void MarginalTree :: SetRightDescendant(int node, int nodeRightDesc)
{
    YW_ASSERT_INFO((int)listLeftDescs.size() == GetTotNodesNum() &&
        (int)listRightDescs.size() == GetTotNodesNum(), "descendant info not set");
    listRightDescs[node] = nodeRightDesc;
}

int MarginalTree :: GetFirstNonselfAnces(int v, const set<int> &setAnces) const
{
    // find the first non-self ancestor from the list; if not found return -1
    int res = -1;
    
    int ncv = v;
    while( ncv >= 0 )
    {
        // get parent
        ncv = GetParent(ncv);
        if( setAnces.find(ncv) != setAnces.end() )
        {
            res = ncv;
            break;
        }
    }
    
    return res;
}

void MarginalTree :: InitDefaultEdgeLen()
{
    listEdgeDist.clear();

    // the default assume the following:
    // (a) all leaves are on the same level
    // (b) the rest of tree nodes are orgnized uniformly in distance
    for( int i = 0; i<GetTotNodesNum()-1; ++i )
    {
        double distRel = GetDefaultEdgeLen( i );
        listEdgeDist.push_back( distRel );
    }
    // the root has no edge here
    listEdgeDist.push_back( 0.0 );
}

void MarginalTree :: InitUnitEdgelen()
{
    //
    listEdgeDist.clear();
    
    // the default assume the following:
    // (a) all leaves are on the same level
    // (b) the rest of tree nodes are orgnized uniformly in distance
    for( int i = 0; i<GetTotNodesNum()-1; ++i )
    {
        listEdgeDist.push_back( 1.0 );
    }
    // the root has no edge here
    listEdgeDist.push_back( 0.0 );
}

double MarginalTree :: GetDefaultEdgeLen( int child  )
{
    int curpos = child;
    int parpos = listParentNodePos[ child ];

    int punorm = CalcNormHeight( parpos);
    int plnorm = CalcNormHeight( curpos);
    int numLeaves = GetNumLeaves();

    if( punorm >= numLeaves  )
    {
        punorm = numLeaves - 1;
    }
    if( plnorm >= numLeaves  )
    {
        plnorm = numLeaves - 1;
    }
    // YW: changed back to old distance, 082306, to see if this matters
    double res = 2.0*(1.0/(numLeaves - punorm) - 1.0/(numLeaves - plnorm+1)  );
//cout << "numLeaves = " << numLeaves << ", punorm = " <<  punorm << ", plnorm = " << plnorm << ", res = " << res << endl;
    // here we assume the distrbution of time is according to exponential distibution of mean 2.0/k(k+1) waiting time
    return res;
}

void MarginalTree :: SetParent( int child, int par, bool fAdjLen )
{
    YW_ASSERT_INFO( child < GetTotNodesNum()  && par < GetTotNodesNum(), "Wrong here" );
    listParentNodePos[child] = par;
    // also setup height
	if( fAdjLen == true)
	{
		listEdgeDist[child] = GetDefaultEdgeLen( child );
	}
}

void MarginalTree :: SwapBranches(int nodeBranch1, int nodeBranch2)
{
//cout << "Swapping nodes: " << nodeBranch1 << ", " << nodeBranch2 << endl;
	// swap two branches ending at the two nodes passed in; here assume the branch length will not change
	// note: may need to reset some other descendents' info after this
	int p1 = GetParent(nodeBranch1);
	int p2 = GetParent(nodeBranch2);
	SetParent(nodeBranch1, p2, false);
	SetParent(nodeBranch2, p1, false);
}

void MarginalTree :: PerformSPR(int nodeSPRSrc, int nodeSPRDest, bool fReOrdering)
{
    // perform tree rearrangement
    // cannot have root be involved in SPR
    // if fReOrdering = true, re-initialze the tree positions of all nodes
    if( nodeSPRSrc == GetRoot() || nodeSPRDest == GetRoot() )
    {
        cout << "WARNING: cannot SPR invoves root\n";
        return;
    }
    
    int nodeSPRSrcSib = GetSibling(nodeSPRSrc);
    int nodeSPRSrcPar = GetParent(nodeSPRSrc);
    int nodeSPRSrcParPar = -1;
    if( nodeSPRSrcPar != GetRoot() )
    {
        nodeSPRSrcParPar = GetParent(nodeSPRSrcPar);
    }
    int nodeSPRDestPar = GetParent(nodeSPRDest);
    if( nodeSPRSrcSib == nodeSPRDest || nodeSPRSrcPar == nodeSPRDest )
    {
        // donot allow SPR src and dest be siblings already
        cout << "WARNING: cannot have two siblings be SPR\n";
        cout << "Current tree: " << GetNewickSorted(false) << endl;
        cout << "nodeSPRSrc:" << nodeSPRSrc << ", nodeSPRDest: " << nodeSPRDest << ", tree:";
        this->Dump();
        
        return;
    }
    SetParent( nodeSPRSrcSib, GetParent(nodeSPRSrcPar), false );
    SetParent( nodeSPRDest, nodeSPRSrcPar, false );
    SetParent( nodeSPRSrcPar, nodeSPRDestPar, false);
    
    if( fReOrdering == false )
    {
        // also need to change descendent info since we are not rebuilding
        // change src'par'par if needed
        if( nodeSPRSrcParPar >= 0 )
        {
            // see left or right to change
            int nppleft = GetLeftDescendant(nodeSPRSrcParPar);
            int nppright = GetRightDescendant(nodeSPRSrcParPar);
            if(nppleft == nodeSPRSrcPar)
            {
                SetLeftDescendant(nodeSPRSrcParPar, nodeSPRSrcSib);
            }
            else
            {
                YW_ASSERT_INFO(nppright == nodeSPRSrcPar, "Must be equal here");
                SetRightDescendant(nodeSPRSrcParPar, nodeSPRSrcSib);
            }
        }
        // change the updated nodesrcpar
        if(GetLeftDescendant(nodeSPRSrcPar) == nodeSPRSrc)
        {
            SetRightDescendant(nodeSPRSrcPar, nodeSPRDest);
        }
        else
        {
            YW_ASSERT_INFO(GetRightDescendant(nodeSPRSrcPar) == nodeSPRSrc, "Must be equal here1");
            SetLeftDescendant(nodeSPRSrcPar, nodeSPRDest);
        }
        // update dest'par
        if( GetLeftDescendant(nodeSPRDestPar) == nodeSPRDest )
        {
            SetLeftDescendant(nodeSPRDestPar, nodeSPRSrcPar);
        }
        else
        {
            YW_ASSERT_INFO( GetRightDescendant(nodeSPRDestPar) == nodeSPRDest, "Must be equal here2");
            SetRightDescendant(nodeSPRDestPar, nodeSPRSrcPar);
        }
        
        return;
    }
    
    // rearrange
    RearrangeParIncOrder();
    
    // re-init descendents
    BuildDescendantInfo();
}

void MarginalTree :: PerformSPRWithDescInfo( int nodeSPRSrc, int nodeSPRDest, const vector<vector<int> > &descNodesInfo  )
{
    // perform tree rearrangement
    // cannot have root be involved in SPR
    // if fReOrdering = true, re-initialze the tree positions of all nodes
    if( nodeSPRSrc == GetRoot() || nodeSPRDest == GetRoot() )
    {
        cout << "WARNING: cannot SPR invoves root\n";
        return;
    }
    
    int nodeSPRSrcSib = GetSiblingFromDescInfo(nodeSPRSrc, descNodesInfo);
    int nodeSPRSrcPar = GetParent(nodeSPRSrc);
    //int nodeSPRSrcParPar = -1;
    //if( nodeSPRSrcPar != GetRoot() )
    //{
    //    nodeSPRSrcParPar = GetParent(nodeSPRSrcPar);
    //}
    int nodeSPRDestPar = GetParent(nodeSPRDest);
    if( nodeSPRSrcSib == nodeSPRDest || nodeSPRSrcPar == nodeSPRDest )
    {
        // donot allow SPR src and dest be siblings already
        cout << "WARNING: cannot have two siblings be SPR2\n";
        return;
    }
    SetParent( nodeSPRSrcSib, GetParent(nodeSPRSrcPar), false );
    SetParent( nodeSPRDest, nodeSPRSrcPar, false );
    SetParent( nodeSPRSrcPar, nodeSPRDestPar, false);
}

void MarginalTree :: RerootSubtree(int nodeST, int nodeSTNewRoot)
{
//cout << "Reroot tree: nodeST:" << nodeST << ", nodeSTNewRoot: " << nodeSTNewRoot << ", tree: " << GetNewickSorted(false) << "     tree details: ";
//Dump();
    // reroot the subtee (rooted at nodeST) at a new root (nodeSTNewroot, a node in the subtee)
    // collect all the nodes above nodeSTNewRoot and their other children (not along the path from nodeST to ndoeSTNewRoot)
    vector<int> listAnces;
    int nodeCurr = nodeSTNewRoot;
    int nodeAnc = GetParent(nodeCurr);
    while( nodeAnc != nodeST )
    {
        YW_ASSERT_INFO( nodeAnc >= 0, "Fail" );
        int ch = GetLeftDescendant(nodeAnc);
        if( ch == nodeCurr )
        {
            ch = GetRightDescendant(nodeAnc);
        }
        listAnces.push_back(nodeAnc);
        
        nodeCurr = nodeAnc;
        nodeAnc = GetParent(nodeCurr);
    }

    if( listAnces.size() == 0 )
    {
        // nothing to do
        return;
    }
//cout << "List Ances: ";
//DumpIntVec(listAnces);
    
    // now reroot from the last
    int nodeNewChild = GetLeftDescendant( nodeST );
    if( nodeNewChild == *listAnces.rbegin()  )
    {
        nodeNewChild = GetRightDescendant(nodeST);
    }
    for(int i=(int)listAnces.size()-1; i>=0; --i)
    {
        int node1 = listAnces[i];
//cout << "Moving node1: " << node1 << ", nodeNewChild: " << nodeNewChild << endl;
        // set the other child to the
        SetParent( nodeNewChild, node1 );
        nodeNewChild = node1;
    }
    SetParent( listAnces[0], nodeST);
    SetParent( nodeSTNewRoot, nodeST);
//cout << "Before cleanup: tree: \n";
//Dump();
    // rearrange
    RearrangeParIncOrder();
    
    // re-init descendents
    BuildDescendantInfo();
    
//cout << "AFTER Reroot tree, tree: " << GetNewickSorted(false) << "     tree details: ";
//Dump();
}

void MarginalTree :: PerformDisjointSPRs(const vector<pair<int,int> > &listSPRs)
{
    if( listSPRs.size() == 0 )
    {
        return;
    }
    // perform multiple disjoint SPRs
    for(unsigned int i=0; i<listSPRs.size(); ++i )
    {
        int nodeSPRSrc = listSPRs[i].first;
        int nodeSPRDest = listSPRs[i].second;
        // perform tree rearrangement
        // cannot have root be involved in SPR
        if( nodeSPRSrc == GetRoot() || nodeSPRDest == GetRoot() )
        {
            cout << "WARNING: cannot SPR invoves root\n";
            continue;
        }
        
        int nodeSPRSrcSib = GetSibling(nodeSPRSrc);
        int nodeSPRSrcPar = GetParent(nodeSPRSrc);
        int nodeSPRDestPar = GetParent(nodeSPRDest);
        if( nodeSPRSrcSib == nodeSPRDest || nodeSPRSrcPar == nodeSPRDest )
        {
            // donot allow SPR src and dest be siblings already
            cout << "WARNING: cannot have two siblings be SPR1\n";
            continue;
        }
        SetParent( nodeSPRSrcSib, GetParent(nodeSPRSrcPar), false );
        SetParent( nodeSPRDest, nodeSPRSrcPar, false );
        SetParent( nodeSPRSrcPar, nodeSPRDestPar, false);
    }
    // rearrange
    RearrangeParIncOrder();
    
    // re-init descendents
    BuildDescendantInfo();
}

int MarginalTree :: CalcNormHeight( int node)
{
    int normHt = node - (GetNumLeaves()-1);
    if( normHt < 0 )
    {
        normHt = 0;
    }
    return normHt;
}

void MarginalTree :: Binarize()
{
    // first initialize distance if not yet
    if(  listEdgeDist.size() == 0 )
    {
        InitDefaultEdgeLen();
    }

    // assume distance has been set properly
    YW_ASSERT_INFO(listEdgeDist.size() > 0, "Tree edge length not set");

    // This function makes this marginal binary
    vector<int> updatedLabels, updatedPars;
    vector<double> updatedDist;

    // find out the current largest label, for the purpose of adding new labels
    int maxLabel = -1;
    for(int i=0; i<(int) listNodeLabels.size(); ++i)
    {
        if( listNodeLabels[i] > maxLabel )
        {
            maxLabel = listNodeLabels[i];
        }
    }
    int labelNextToUse = maxLabel + 1;


    // before doing anything, get the descendent info for each tree node
    vector< vector<int> > listDescendentsVec;
    ConsDecedentInfo(listDescendentsVec);
    //vector< set<int> > listDescendents;
    //for( unsigned int i=0; i<listDescendents.size(); ++i )
    //{
    //    set<int> tmpSet;
    //    PopulateSetByVec( tmpSet,listDescendentsVec[i]  );
    //    listDescendents.push_back(tmpSet);
    //}

    // we need another auxilary data structure to map old position to new position
    // we need this because we are adding some new nodes between two old nodes
    vector<int> mapOldPosToNewPos( GetTotNodesNum() );

    // first copy every thing up to the leaves
    for( int i=0; i<numLeaves; ++i )
    {
        updatedLabels.push_back( listNodeLabels[i] );
        updatedPars.push_back( listParentNodePos[i] );
        updatedDist.push_back( listEdgeDist[i] );

        // leaf is never changed position
        mapOldPosToNewPos[i] = i;
    }
    // now we treat each internal node one by one, and split it when needed
    for( int i=numLeaves; i < GetTotNodesNum(); ++i )
    {
        // the first thing to do is: find children from the constructed portion of tree
        vector<int> &listChildren = listDescendentsVec[i];
//cout << "IN node = " << i << ", children num = " << listChildren.size() << endl;

        // do nothing if there is no mor than 2 children
        // it is possible that an internal node does not have any children
        // Then what to do here? TBD
        if( listChildren.size() == 2 || listChildren.size() == 0  )
        {
//cout << "Simply go over the originals...\n";
            updatedLabels.push_back( listNodeLabels[i] );
            updatedPars.push_back( listParentNodePos[i] );  // do it for now, will update later
            updatedDist.push_back( listEdgeDist[i] );
        
            // record current position
            mapOldPosToNewPos[i] =  (int)updatedLabels.size() - 1;

            // now update its children's parent to this new location
            for( int jjj = 0; jjj < (int)listChildren.size(); ++jjj )
            {
                int oldpos = listChildren[jjj];
                int newpos = mapOldPosToNewPos[oldpos];
                updatedPars[newpos] = mapOldPosToNewPos[i];
            }

            continue;
        }
        if( listChildren.size() == 1 )
        {
            // we should remove this node
            int childOldPos = listChildren[0];
            // skip this node, but update the node
            // let its (only) child points to its parent
//cout << "childOldPos = " << childOldPos << endl;
            listParentNodePos[childOldPos] = listParentNodePos[i];
//cout << "childOldPos's parent set to  = " << listParentNodePos[i] << endl;

            
            // also update listChildren
            if( listParentNodePos[i] >= 0 )
            {
                int pppos = listParentNodePos[i] ;
                vector<int> listNewChildAtIParent;
                for( int ii=0; ii<(int)listDescendentsVec[pppos].size() ; ++ii )
                {
                    if( i != listDescendentsVec[pppos][ii] )
                    {
                        // do not append i anymore
                        listNewChildAtIParent.push_back(listDescendentsVec[pppos][ii] );
                    }
                }
                // 
                YW_ASSERT_INFO( (int)listNewChildAtIParent.size() == (int)listDescendentsVec[pppos].size()-1,
                    "Something wrong");
                // append a new thing
                listNewChildAtIParent.push_back( childOldPos );
                // update the orginal list
                listDescendentsVec[pppos] = listNewChildAtIParent;
            }
            else
            {
                int newpos = mapOldPosToNewPos[childOldPos];
                updatedPars[newpos] = -1;
                updatedDist[newpos] = 0.0;
            }
            continue;
        }

        // otherwise, we have to split the node
        for(int jjj = 0; jjj < (int) listChildren.size() - 2; ++jjj)
        {
            updatedLabels.push_back( labelNextToUse++ );  // new IN is assigned an arbitary label
            updatedPars.push_back( -1 );  // do it for now, will update later
            // for any new internal node, edge length (out of it) is 0
            updatedDist.push_back( 0.0 );

            // now update children
            int curINPos = (int)updatedLabels.size() - 1;
            if( jjj == 0 )
            {
                // Then we use the first original child
                int oldpos = listChildren[0];
                int newpos = mapOldPosToNewPos[oldpos];
                updatedPars[newpos] = curINPos;
            }
            else
            {
                // otherwise, we use the previous IN
                updatedPars[curINPos-1] = curINPos;
            }
            // the right branch is always an original branch
            int oldpos = listChildren[jjj+1];
            int newpos = mapOldPosToNewPos[oldpos];
            updatedPars[newpos] = curINPos;
        }
        // now we append the original internal node in
        updatedLabels.push_back( listNodeLabels[i] );
        updatedPars.push_back( listParentNodePos[i] );  // do it for now, will update later
        updatedDist.push_back( listEdgeDist[i] );
    
        // record current position
        mapOldPosToNewPos[i] =  (int)updatedLabels.size() - 1;

        // update its two children, one of them is the last new node to add
        updatedPars[ (int)updatedPars.size()-2 ] = mapOldPosToNewPos[i];
        int oldpos = listChildren[(int)listChildren.size() - 1];
        int newpos = mapOldPosToNewPos[oldpos];
        updatedPars[ newpos ] = mapOldPosToNewPos[i];
    }
    // finally, we update the mtree
    this->listNodeLabels = updatedLabels;
    this->listParentNodePos = updatedPars;
    this->listEdgeDist = updatedDist;

    // check to make sure this is indeed binary
    YW_ASSERT_INFO( this->listNodeLabels.size() == this->listParentNodePos.size(),
        "In binaralize: size wrong1");
    YW_ASSERT_INFO( this->listNodeLabels.size() == this->listEdgeDist.size(),
        "In binaralize: size wrong1");
    // now iterator the degree
#if 0
    vector<int> nodeOutDegrees;
    for(int i=0; i<(int)this->listNodeLabels.size(); ++i)
    {
        nodeOutDegrees.push_back( 0 );
    }
    for(int i=0; i<(int)this->listNodeLabels.size(); ++i)
    {
        int ppos = listParentNodePos[i] ;
        YW_ASSERT_INFO( ppos < (int)listParentNodePos.size(), "pos wrong" );
        if( ppos >= 0 )
        {
            nodeOutDegrees[ ppos ]++;
            if( nodeOutDegrees[ ppos ] >= 3 )
            {
                YW_ASSERT_INFO( false, "Error in binarinize." );
            }
        }
    }
#endif
//Dump();
}


void MarginalTree :: Consolidate()
{
//cout << "Before consolidate, tree = ";
//this->Dump();
    // Remove degree-2 intermediate nodes
    // first find out which nodes are those to be removed
    set<int> nodesToDel;
    // this is very simple: scan parent list
    // if a node (non-leaf) only appears at most once of them, then remove it
    vector<int> occurTimes;
    vector<bool> nodeVisitedFlags;
    for( int i=0; i<GetTotNodesNum(); ++i )
    {
        occurTimes.push_back( 0 );
        nodeVisitedFlags.push_back( false );
    }
    stack<int> nodesToExplore;
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        nodesToExplore.push(i);
    }
    while( nodesToExplore.empty() == false )
    {
        // find one node 
        int node = nodesToExplore.top();
        nodesToExplore.pop();

        // if this is already visited, skip
        if( nodeVisitedFlags[node] == true )
        {
            continue;
        }
        // this is a new node, so explore it
        nodeVisitedFlags[node] = true;
        int pp = GetParent(node);
        if( pp >= 0 )
        {
            nodesToExplore.push( pp );
            occurTimes[pp]++;
        }
    }
    // now figure out how many to remove up to a point
    vector<int> listNumDelItems;
    for( int i=0; i<GetNumLeaves(); ++i )
    {
        listNumDelItems.push_back( 0 );
    }
    int numToDelete = 0;
    for( int i = GetNumLeaves(); i<GetTotNodesNum(); ++i  )
    {
        if( occurTimes[i] <= 1  && i != GetTotNodesNum()-1 )
        {
            numToDelete ++;
        }
        listNumDelItems.push_back( numToDelete );
    }

    // now store a new set of items
    vector<int> listNodeLabelsNew;
    vector<int> listParentNodePosNew;
    vector< double > listEdgeDistNew;
    // now mark those with at most once to be deleted
    for(int i=0; i<GetTotNodesNum(); ++i)
    {
        // leaves and the root is always there
        if( occurTimes[i]  > 1  || i < GetNumLeaves()   || i == GetTotNodesNum()-1 )
        {
            listNodeLabelsNew.push_back( listNodeLabels[i] );

             // for parent, we trace upwards until either find a occur time > 1 or root
            double distNew = listEdgeDist[i];
            int parNew = GetParent( i ) ;
            // now trace back to see if we need them
            while( occurTimes[parNew] <= 1  && parNew >= 0 )
            {
                int parNext = GetParent( parNew );
                if( parNext < 0 )
                {
                    break;
                }
                distNew += listEdgeDist[parNew];
                parNew = parNext;
            }

            // save this (and make adjustment) 
            int parToSet = parNew - listNumDelItems[ parNew ] ;
            if( parToSet < 0 )
            {
                parToSet = -1;
            }
            listParentNodePosNew.push_back( parToSet  );
            listEdgeDistNew.push_back( distNew );
       }

    }

    // finally store this
    listNodeLabels = listNodeLabelsNew;
    listParentNodePos = listParentNodePosNew;
    listEdgeDist = listEdgeDistNew;

//cout << "After consolidate, tree = ";
//this->Dump();
}


double MarginalTree :: GetEdgeLen( int childNodeIndex ) const
{
    YW_ASSERT_INFO( childNodeIndex < (int)listEdgeDist.size(), "List overflow" );
    return listEdgeDist[childNodeIndex];
}

double MarginalTree :: GetTotEdgeLen() const
{
    //
    double res = 0.0;
    for(int i=0; i<GetTotNodesNum(); ++i)
    {
        if( i != GetRoot() )
        {
            res += GetEdgeLen(i);
        }
    }
    return res;
}

void MarginalTree :: ConsDecedentInfo( vector<vector<int> > &descNodes ) const
{
    descNodes.clear();
    int numNodes = GetTotNodesNum();
    //vector< vector<int> > listDescendents;
    for( int i=0; i<numNodes; ++i )
    {
        vector<int> emptyVec;
        descNodes.push_back( emptyVec );
    }
    for(int i=0; i<numNodes; ++i)
    {
        int parpos = listParentNodePos[i];
        if( parpos >= 0  )
        {
            descNodes[parpos].push_back( i );
        }
    }
//cout << "Descedents info:\n";
//for( unsigned int i=0; i<descNodes.size(); ++i)
//{
//DumpIntVec( descNodes[i] );
//}
}

void MarginalTree :: ConsAllDecedentInfo( vector<set<int> > &descNodes, bool fIncSelf ) const
{
    descNodes.clear();
    int numNodes = GetTotNodesNum();
    //vector< vector<int> > listDescendents;
    for( int i=0; i<numNodes; ++i )
    {
        set<int> emptySet;
        descNodes.push_back( emptySet );
    }
    for(int i=0; i<numNodes; ++i)
    {
        // Always contain itself if set
        if(  fIncSelf == true )
        {
            descNodes[i].insert( i );
        }

        int parpos = listParentNodePos[i];
        if( parpos >= 0  )
        {
            UnionSets( descNodes[parpos], descNodes[i] );
            if( fIncSelf == false)
            {
                // otherwise, we need to append this current node to
                descNodes[parpos].insert( i );
            }
        }

    }
}


void MarginalTree :: ConsDecedentLeavesInfo( vector<set<int> > &descLaves ) const
{
    descLaves.clear();
    //vector< vector<int> > listDescendents;
    int numNodes = GetTotNodesNum();
    for( int i=0; i<numNodes; ++i )
    {
        set<int> emptyVec;
        descLaves.push_back( emptyVec );
    }
    for(int i=0; i<numNodes; ++i)
    {
        // If this is a leave, push itself into 
        if( i < numLeaves )
        {
            descLaves[i].insert( i );
        }


        int parpos = listParentNodePos[i];
        if( parpos >= 0  )
        {
            UnionSets( descLaves[parpos], descLaves[i] );
        }
    }
//cout << "Descedents info:\n";
//for( unsigned int i=0; i<descLaves.size(); ++i)
//{
//DumpIntSet( descLaves[i] );
//}

}

void MarginalTree :: ConsDecedentLeavesInfo3( vector<set<int> > &descLaves ) const
{
    descLaves.clear();
    //vector< vector<int> > listDescendents;
    int numNodes = GetTotNodesNum();
    for( int i=0; i<numNodes; ++i )
    {
        set<int> emptyVec;
        descLaves.push_back( emptyVec );
    }
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        int nodecur = i;
        while( nodecur >= 0 )
        {
            descLaves[nodecur].insert(i);
            nodecur = GetParent(nodecur);
        }
    }
//cout << "Descedents info:\n";
//for( unsigned int i=0; i<descLaves.size(); ++i)
//{
//DumpIntSet( descLaves[i] );
//}

}

void MarginalTree :: ConsAllDescendantsInfo( vector<set<int> > &descNodes ) const
{
    descNodes.clear();
    //vector< vector<int> > listDescendents;
    int numNodes = GetTotNodesNum();
    for( int i=0; i<numNodes; ++i )
    {
        set<int> emptyVec;
        descNodes.push_back( emptyVec );
    }
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        int nodecur = i;
        
        while( nodecur >= 0 )
        {
            descNodes[nodecur].insert(nodecur);
            int nodepar = GetParent(nodecur);
            if( nodepar >= 0 )
            {
                UnionSets( descNodes[nodepar], descNodes[nodecur] );
            }
            nodecur = nodepar;
        }
    }
}


void MarginalTree :: ConsDecedentLeavesInfo2( map<set<int>, int> &mapSTToSTs ) const
{
    mapSTToSTs.clear();
    vector<set<int> > descLeaves;
    ConsDecedentLeavesInfo(descLeaves);
    for(unsigned int i=0; i<descLeaves.size(); ++i)
    {
        mapSTToSTs[ descLeaves[i] ] = i;
    }
}
void MarginalTree :: ConsDecedentLeavesInfo4( map<set<int>, int> &mapSTToSTs ) const
{
    mapSTToSTs.clear();
    vector<set<int> > descLeaves;
    ConsDecedentLeavesInfo3(descLeaves);
    for(unsigned int i=0; i<descLeaves.size(); ++i)
    {
        mapSTToSTs[ descLeaves[i] ] = i;
    }
}

void MarginalTree :: ConsDecedentLeavesInfoLabels( vector<set<int> > &leafNodeLabels ) const
{
    //
    leafNodeLabels.clear();
    vector< set<int> > leafNodePos;
    ConsDecedentLeavesInfo( leafNodePos );
    for(int i=0; i<(int)leafNodePos.size(); ++i)
    {
        set<int> ss;
        for(set<int> :: const_iterator it = leafNodePos[i].begin(); it != leafNodePos[i].end(); ++it)
        {
            ss.insert( GetLabel(*it) );
        }
        leafNodeLabels.push_back(ss);
    }
}

void MarginalTree :: FindAllSplits( vector<set<int> > &listSplits ) const
{
    //
    listSplits.clear();
    //vector< vector<int> > listDescendents;
    int numNodes = GetTotNodesNum();
    for( int i=0; i<numNodes; ++i )
    {
        set<int> emptyVec;
        listSplits.push_back( emptyVec );
    }
    for(int i=0; i<numNodes; ++i)
    {
        // If this is a leave, push itself into
        if( i < numLeaves )
        {
            listSplits[i].insert( GetLabel( i ) );
        }
        
        
        int parpos = listParentNodePos[i];
        if( parpos >= 0  )
        {
            UnionSets( listSplits[parpos], listSplits[i] );
        }
    }
}



int MarginalTree :: GetParent( int child ) const
{
if( child >= GetTotNodesNum() )
{
cout << "child = " << child << ", tot num of nodes = " << GetTotNodesNum() << endl;
}
    YW_ASSERT_INFO( child < GetTotNodesNum(), "Range bug" );
    return listParentNodePos[child];
}


void MarginalTree :: ConsHeightsInfo( vector<int> &nodesHt ) const
{
    nodesHt.clear();
    int numNodes = GetTotNodesNum();
    for( int i=0; i<numNodes; ++i )
    {
        nodesHt.push_back( 0 );
    }
    for( int i=0; i<numNodes; ++i )
    {
        // test whether the parent node should be updated its height
        int parpos = listParentNodePos[i];
        if( parpos >= 0 && nodesHt[parpos] <  nodesHt[i]+1 )
        {
            nodesHt[ parpos ] = nodesHt[i] + 1;
        }
    }
}

void MarginalTree :: Dump() const
{
    // Output marginal tree states
    cout << "Tree: number of leaves: " << numLeaves << endl;
    cout << "Node list = ";
    DumpIntVec( this->listNodeLabels );
    cout << "Parent list = ";
    DumpIntVec( this->listParentNodePos );
    cout << "Tree dist = ";
    DumpDoubleVec(this->listEdgeDist);
    cout << "Left descendents = ";
    DumpIntVec(listLeftDescs);
    cout << "Right descendents = ";
    DumpIntVec(listRightDescs);
}

int MarginalTree :: GetPosForLabel(int lbl) const
{
    //
    int res = -1;
    for(int i=0; i<(int)listNodeLabels.size(); ++i)
    {
        if( listNodeLabels[i] == lbl)
        {
            res = i;
            break;
        }
    }
    return res;
}

int MarginalTree :: GetMRCA(int v1, int v2) const
{
    // retrieve MRCA from it
//cout << "v1 = " << v1 << ", v2= " << v2 << endl;
    int n1=v1, n2 = v2;
    while( n1 != n2 )
    {
        // we alternatively move up, depend on which one is smaller
        if( n1 < n2  )
        {
            // move n1
            n1 = GetParent(n1);
        }
        else
        {
            // move n2
            n2 = GetParent(n2);
        }
//cout << "GetMRCA1: n1 = " << n1 << ", n2 = " << n2 << endl;
    }
    // n1 (or n2) is the result)
    return n1;
}

void MarginalTree :: GetChildren(int node, set<int> &listChildren) const
{
    listChildren.clear();

    // we just search parent list to see who has entry equal to node
    for( int i=0; i<(int)listParentNodePos.size(); ++i )
    {
        if( listParentNodePos[i] == node )
        {
            listChildren.insert(i);
        }
    }
}

int MarginalTree :: GetMaxHt() const
{
    vector<int> heights;
    ConsHeightsInfo( heights );
    int maxHt = 0;
    for(  int i=0; i<(int) heights.size(); ++i )
    {
        if( maxHt < heights[i] )
        {
            maxHt = heights[i];
        }
    }
    return maxHt;
}

double MarginalTree :: GetHeight() const
{
	int root = GetRoot();
	return GetHeightOfNode(root);
}
double MarginalTree :: GetHeightOfNode(int node) const
{
	// get descendent
	int lchild = GetLeftDescendant(node);
	int rchild = GetRightDescendant(node);
	if( lchild < 0 || rchild < 0)
	{
		return 0.0;
	}
	return max( GetEdgeLen(lchild)+GetHeightOfNode(lchild), GetEdgeLen(rchild)+GetHeightOfNode(rchild) );
}

void MarginalTree :: RemoveLeafNodeFromBinaryTree( int lfn )
{
    YW_ASSERT_INFO( IsLeaf(lfn) == true, "Not a leaf" );
    // rmeove a leaf node (and suppress the degree-2 node if so
    // first fill in leaves
    vector<int> listNodeLabelsNew;
    vector<int> listParentNodePosNew;
    int pp = GetParent( lfn );
    for( int i=0; i< GetTotNodesNum(); ++i )
    {
        if( i != lfn  && i != pp )
        {
            listNodeLabelsNew.push_back( this->listNodeLabels[i] );

            int parNew;
            int oldPar = GetParent( i );
            if( oldPar < pp )
            {
                // just minus 1
                parNew = oldPar - 1;
            }
            else if( oldPar > pp )
            {
                // otherwise, we lost two
                parNew = oldPar - 2;
            }
            else
            {
                // In this case, we are pointing to pp, since pp is removed, we need to move up by one
                parNew = GetParent( pp) -2;
            }
            if( parNew < 0 )
            {
                parNew = -1;
            }
            listParentNodePosNew.push_back(parNew);
        }
    }
    //
    this->listNodeLabels = listNodeLabelsNew;
    this->listParentNodePos = listParentNodePosNew;

    this->numLeaves --;
}

bool MarginalTree :: AreTwoPathsDisjoint( int sn1, int en1, int sn2, int en2 ) const
{
    // test whether two path (sn1, en1) and (sn2, en2) are (vertex) disjoint
    // note that for binary tree, this is also checking for edge disjoint
    // we use a dumb method here
    set<int> nodesVisitedTree1;

    int n1=sn1, n2 = en1;
    nodesVisitedTree1.insert( n1 );
    nodesVisitedTree1.insert(n2);
    while( n1 != n2 )
    {
        // we alternatively move up, depend on which one is smaller
        int nodeNew;
        if( n1 < n2  )
        {
            // move n1
            n1 = GetParent(n1);
            nodeNew = n1;
        }
        else
        {
            // move n2
            n2 = GetParent(n2);
            nodeNew = n2;
        }

        // 
        nodesVisitedTree1.insert( nodeNew );
    }
//cout << "Path 1=";
//DumpIntSet( nodesVisitedTree1 );
    // now we move on to the next pair
    n1= sn2;
    n2 = en2;
    if( nodesVisitedTree1.find( n1 ) != nodesVisitedTree1.end()  || 
        nodesVisitedTree1.find(n2) != nodesVisitedTree1.end() )
    {
        return false;
    }
    while( n1 != n2 )
    {
        // we alternatively move up, depend on which one is smaller
        int nodeNew;
        if( n1 < n2  )
        {
            // move n1
            n1 = GetParent(n1);
            nodeNew = n1;
        }
        else
        {
            // move n2
            n2 = GetParent(n2);
            nodeNew = n2;
        }

        // 
        if( nodesVisitedTree1.find( nodeNew ) != nodesVisitedTree1.end() )
        {
            return false;
        }
    }

    return true;
}

int MarginalTree :: GetPath( int sn, int en, set<int> &edgesOnPath ) const
{
    // find edges on the path, and return the MRCA
    int n1=sn, n2 = en;
    edgesOnPath.insert( n1 );
    edgesOnPath.insert(n2);
    while( n1 != n2 )
    {
        // we alternatively move up, depend on which one is smaller
        int nodeNew;
        if( n1 < n2  )
        {
            // move n1
            n1 = GetParent(n1);
            nodeNew = n1;
        }
        else
        {
            // move n2
            n2 = GetParent(n2);
            nodeNew = n2;
        }

        // 
        edgesOnPath.insert( nodeNew );
    }
    // remove MRCA from result
    YW_ASSERT_INFO( edgesOnPath.find( n1 ) != edgesOnPath.end(), "wrong2" );
    edgesOnPath.erase( n1 );

    return n1;
}

double MarginalTree :: GetPathLen(int sn, int en)
{
    // get the branch lenggth on the path
    double res = 0.0;
    
    set<int> edgesOnPath;
    int mrca = GetPath( sn, en, edgesOnPath );
    YW_ASSERT_INFO(edgesOnPath.find(mrca) == edgesOnPath.end(), "Fail to find");
    for(set<int> :: iterator it = edgesOnPath.begin(); it != edgesOnPath.end(); ++it)
    {
        res += GetEdgeLen(*it);
    }
    return res;
}

void MarginalTree :: OutputGML( const char *fileName ) const
{
	// Now output a file in GML format
	// First create a new name
	string name = fileName;
//cout << "num edges = " << listEdges.size() << endl;

	DEBUG("FileName=");
	DEBUG(name);
	DEBUG("\n");
	// Now open file to write out
	ofstream outFile( name.c_str() );

	// First output some header info
	outFile << "graph [\n"; 
	outFile << "comment ";
	OutputQuotedString(outFile, "Automatically generated by Graphing tool");
	outFile << "\ndirected  1\n";
	outFile << "id  1\n";
	outFile << "label ";
	OutputQuotedString ( outFile, "Marginal Tree....\n");

	// Now output all the vertices
//	int i;

//cout << "a.1.1\n";
	for(  int i=0; i<(int)listNodeLabels.size(); ++i  )
	{
		outFile << "node [\n";

		outFile << "id " <<  i  << endl;
		outFile << "label ";
        char buf[80];
//        sprintf(buf, "n%d",  listNodeLabels[i]  );
        snprintf(buf, sizeof(buf), "n%d",  i  );

        OutputQuotedString (outFile,  buf  ); 
		outFile << endl;

        // See if we need special shape here
	    outFile << "defaultAtrribute   1\n";
        
		outFile << "]\n";

	}
//cout << "a.1.3\n";

	// Now output all the edges, by again starting from root and output all nodes
    for( int i=0; i<(int)listParentNodePos.size(); ++i  )
	{
        int parpos = listParentNodePos[i];
        
//cout << "Output an edge \n"; 
		outFile << "edge [\n";
		outFile << "source " << parpos << endl; 
		outFile << "target  " <<  i  << endl; 
		outFile << "label " ;
		OutputQuotedString( outFile,  ""  );
		outFile << "\n";
		outFile << "]\n";

	}


	// Finally quite after closing file
	outFile << "\n]\n";
	outFile.close();    
}

string MarginalTree :: GetNewick() const
{
	// return the newick format of the tree (with length)
	// method: just get the newick at the root node
	return GetNewickAt( GetTotNodesNum()-1 );
}
string MarginalTree :: GetNewickSorted(bool fLen) const
{
    //
    return GetNewickAt( GetTotNodesNum()-1, true, fLen );
}

string MarginalTree :: GetNewickAt(int node, bool fSort, bool fLen) const
{
	// find its descendents
	string res;
	int childLeft = GetLeftDescendant(node);
	int childRight = GetRightDescendant(node);
	if( childLeft < 0 )
	{
		// must be leaf
		YW_ASSERT_INFO( IsLeaf(node) == true, "Wrong node in MT" );
		// for leaf, only ouput its label together with its length
		char buf[100];
        if( fLen == true )
        {
            snprintf(buf, sizeof(buf), "%d:%0.4f", GetLabel(node), GetEdgeLen(node) );
        }
        else
        {
            snprintf(buf, sizeof(buf), "%d", GetLabel(node) );
        }
		res = buf;
	}
	else
	{
		// append two children's
        if( childRight < 0 )
        {
            Dump();
        }
		YW_ASSERT_INFO( childRight >= 0, "Left/right mismatch" );
		res = "(";
		//res += GetNewickAt(childLeft);
		//res +=",";
		//res += GetNewickAt(childRight);
        string strPart1 = GetNewickAt( childLeft, fSort, fLen);
        string strPart2 = GetNewickAt( childRight, fSort, fLen);
        string strToAdd;
        if( fSort == false || strPart1 <= strPart2 )
        {
            res += strPart1;
            res += ",";
            res += strPart2;
        }
        else
        {
            res += strPart2;
            res += ",";
            res += strPart1;
        }
        res += strToAdd;
		res += ")";
		if( fLen == true && node <  GetTotNodesNum()-1  )
		{
			char buf[100];
			snprintf(buf, sizeof(buf), ":%0.4f", GetEdgeLen(node) );
			res += buf;
		}
	}
	return res;
}



void MarginalTree :: GetLeavesUnder( int nn, set<int> &leavesUnder) const
{
    //
    if( IsLeaf( nn ) == true  )
    {
        leavesUnder.insert(nn);
    }
    else
    {
        set<int> listChildren;
        GetChildren( nn,  listChildren);
        for( set<int> :: iterator it = listChildren.begin(); it != listChildren.end(); ++it )
        {
            GetLeavesUnder( *it, leavesUnder );
        }
    }
}

void MarginalTree ::  GetlabelsFor(const set<int> &setPos, set<int> &setLbls) const
{
    //
    setLbls.clear();
    for(set<int> :: const_iterator it = setPos.begin(); it != setPos.end(); ++it)
    {
        setLbls.insert( GetLabel(*it) );
    }
}

void MarginalTree :: GetLeafSetsForCuts( const vector<int> &listCuts, vector< set<int> > &listLeafSets )  const
{
    // this function finds the cutted subtrees' leaf sets for the given set of cut edges
    listLeafSets.clear();

    // we first create a map of whether an edge mutate or not
    vector<bool> mapEdgeMutFlags;
    for( int i=0; i<this->GetTotNodesNum(); ++i )
    {
        mapEdgeMutFlags.push_back(false);
    }
    for( int i=0; i<(int) listCuts.size(); ++i )
    {
        mapEdgeMutFlags[ listCuts[i] ] = true;
    }

    // we start by bottom up way to traversal all nodes
    vector< set<int> > nodesLeaves( this->GetTotNodesNum()  );
    for(int i=0; i<this->GetNumLeaves(); ++i)
    {
        // all leave nodes are trivial
        nodesLeaves[i].insert( i );
    }
    // test for all nodes
    for(int i=0; i<this->GetTotNodesNum(); ++i)
    {
        // if the edge is cut, we have found an partition or it is a root
        if( mapEdgeMutFlags[i] == true  || i == this->GetTotNodesNum() -1  )
        {
            if( nodesLeaves[i].size() > 0 )
            {
//cout << "Found one partition: ";
//DumpIntSet( nodesLeaves[i] );
                listLeafSets.push_back( nodesLeaves[i]  );
            }
        }
        else
        {
            // otherwise propagate to above
            UnionSets(  nodesLeaves[ this->GetParent(i) ], nodesLeaves[i] );
        }
    }
}

int MarginalTree :: GetMRCAForNodes( const set<int> & listNodes ) const
{
    // find mrca of a list of nodes
    // we use a priority queue, each time, we try to find
    priority_queue<int> queueNodesToCheck;
    set<int> nodesVisited;

    for( set<int> :: iterator it = listNodes.begin(); it != listNodes.end(); ++it )
    {
        queueNodesToCheck.push( (*it) * (-1) );
    }
    while( queueNodesToCheck.size() > 1 )
    {
        int curn = -queueNodesToCheck.top();
        queueNodesToCheck.pop();
        
        // in case there are duplicate ones, remove these duplicate copies
        // this can happen if one node is another node's parent
        if(  -queueNodesToCheck.top() == curn )
        {
            // don't work on this, wait for the next one
            continue;
        }
        
        
        
        // is this visited
        int pp = this->GetParent( curn );
//cout << "Processing curn: " << curn << ", parent: " << pp << endl;
        if( nodesVisited.find( pp ) == nodesVisited.end() )
        {
            // new node
            nodesVisited.insert( pp );
            // push to queue
            queueNodesToCheck.push(-1*pp);
        }
    }
    int res = -1*queueNodesToCheck.top();
    return res;
}

bool MarginalTree :: IsNodeUnder( int nn, int ancesNode) const
{
    //
    if( nn > ancesNode )
    {
        return false;
    }
    int curn = nn;
    while(  curn < ancesNode && curn >= 0  )
    {
        curn = this->GetParent( curn );
    }
    if( curn == ancesNode )
    {
        return true;
    }
    else
    {
        return false;
    }
}

void MarginalTree :: RandPermuateLeaves()
{
    // randomly permuate the leaves of the tree
    // we do this by shuffeling the parent of the leaves
    vector<int> parentsNewIndices;
    GetRandVector( parentsNewIndices, 0, GetNumLeaves()-1 );
//cout << "Dump Random vector: ";
//DumpIntVec( parentsNewIndices );
    // now shuffling it
    vector<int> leavesParNew;
    for( int i=0; i<(int)parentsNewIndices.size(); ++i )
    {
        leavesParNew.push_back( GetParent(parentsNewIndices[i])   ) ;
    }
    // now assign it
    for( int i=0; i<(int)parentsNewIndices.size(); ++i )
    {
        SetParent( i, leavesParNew[i]   ) ;
    }

}

int MarginalTree :: GetTriple(int i, int j, int k) const
{
	// ensure order of a,b,c first
	OrderInt(i,j);
	OrderInt(i,k);
	OrderInt(j,k);

	//
	// is these have different triples on T1 and T2?
	// we do this by getting MRCA for all pairs of MRCAs
	int mrcaij1 = GetMRCA( i, j );
	int mrcajk1 = GetMRCA( j, k );
	int mrcaik1 = GetMRCA( i, k );

	// now just test exhustively
	if( mrcaij1 == mrcajk1 )
	{
		return 3;
	}
	else if( mrcaij1 == mrcaik1 )
	{
		return 2;
	}
	else 
	{
		return 1;
	}
}

int MarginalTree :: GetSibling(int a) const
{
    if( a == GetRoot() )
    {
        return -1;  // root has no sibling
    }
	// get sibling of the node (leaf or non-leaf)
	int par = GetParent(a);
	int lc = GetLeftDescendant(par);
	int rc = GetRightDescendant(par);

	YW_ASSERT_INFO( a == lc || a == rc, "Very wrong" );
	if( a == lc )
	{
		return rc;
	}
	else
	{
		return lc;
	}
}

int MarginalTree :: GetSiblingFromDescInfo(int a, const vector<vector<int> > &descNodeInfo) const
{
    // get sibling of the node (leaf or non-leaf)
    int par = GetParent(a);
    int sib = -1;

    if( par < 0 )
    {
        return -1;
    }
    YW_ASSERT_INFO( par<(int) descNodeInfo.size(), "Wrong in GetSiblingFromDescInfo" );
    
    bool fFound = false;
    for(unsigned int ii=0; ii<descNodeInfo[par].size(); ++ii)
    {
        if( descNodeInfo[par][ii] != a)
        {
            sib = descNodeInfo[par][ii];
        }
        else
        {
            fFound = true;
        }
    }
    YW_ASSERT_INFO( fFound == true, "GetSiblingFromDescInfo: wrong" );
    return sib;
}

bool MarginalTree :: AreNodesSibling(int a, int b) const
{
    //
    return GetSibling(a) == b;
}

void MarginalTree :: SortByLeafId()
{
	// sort based on leaf id. That is, leaf ids = 0,1,2,3,.. in the list
    vector<int> listNodeLabelsNew(this->listNodeLabels.size());
    vector<int> listParentNodePosNew(this->listParentNodePos.size() );
    vector< double > listEdgeDistNew(this->listEdgeDist.size() );

    listNodeLabelsNew = listNodeLabels;
    listParentNodePosNew = listParentNodePos;
    listEdgeDistNew = listEdgeDist;
    
    // now sort and swap the leaf part
    // collect leaves
    vector<int> listLeafIds;
    for(int i=0; i<GetNumLeaves(); ++i )
    {
        listLeafIds.push_back(listNodeLabels[i]);
    }
    //vector<int> listLeafIdsOld = listLeafIds;
    SortIntVec(listLeafIds);
//cout << "listLeafIds = ";
//DumpIntVec( listLeafIds );
    // create a map
    //map<int,int> mapLeafIdToOldPos;
    //for( int i=0; i<(int)listLeafIdsOld.size(); ++i )
    //{
    //    mapLeafIdToOldPos.insert(map<int,int> :: value_type(listLeafIdsOld[i],i) );
    //}
    map<int,int> mapLeafIdToNewPos;
    for( int i=0; i<(int)listLeafIds.size(); ++i )
    {
//cout << "Set map from id " << listLeafIds[i] << " to position " << i << endl;
        mapLeafIdToNewPos.insert(map<int,int> :: value_type(listLeafIds[i],i) );
    }
    // now swap the info in each in the old list
    for( int i=0;i<(int)GetNumLeaves(); ++i )
    {
        int vid = listNodeLabels[i];
        YW_ASSERT_INFO( mapLeafIdToNewPos.find(vid) != mapLeafIdToNewPos.end(), "FAIL to find" );
        int posNew = mapLeafIdToNewPos[vid];
//cout << "vid = " << vid << ", Set " << posNew << " to position " << i << endl;
        listNodeLabelsNew[posNew] = vid;
        listParentNodePosNew[posNew] = listParentNodePos[i];
        listEdgeDistNew[posNew] = listEdgeDist[i];
    }

    
#if 0       // there is some issues with this piece of code: namely, it can not deal with non-distinct id in trees properly. Although ids are expected to be distinct but sometime they are not; so change it on 8/13/13
	// list leaf in order
	vector<int> listLeaves = this->listNodeLabels;
	SortIntVec(listLeaves);
cout << "after sorting, leaf list = ";
DumpIntVec( listLeaves);
	for(int i=0; i<(int)this->listNodeLabels.size(); ++i)
	{
		// keep a sorted list
		int lvid = this->listNodeLabels[i];
		// find which leaf is it
		int lvpos = GetItemIndexInVec(listLeaves, lvid);
		YW_ASSERT_INFO(lvpos >= 0, "FATAL ERROR10");
		listNodeLabelsNew[ lvpos ] = lvid;
		listParentNodePosNew[ lvpos ] = listParentNodePos[i];
		if( lvpos < (int) listEdgeDistNew.size() )
		{
			listEdgeDistNew[lvpos] = listEdgeDist[i];
		}
	}
#endif
	// now write back
    this->listNodeLabels = listNodeLabelsNew;
    this->listParentNodePos = listParentNodePosNew;
    this->listEdgeDist = listEdgeDistNew;
    
    // redo the descendents
    BuildDescendantInfo();
}

void MarginalTree :: FixDupIds()
{
	// remove redundent ids with something new
	// sort based on leaf id. That is, leaf ids = 0,1,2,3,.. in the list
	// also, keep the leaf and internal nodes id separated
    vector<int> listNodeLabelsNew(this->listNodeLabels.size());
	int numLeaves = GetNumLeaves();

	// list leaf in order
	set<int> setNids;
	PopulateSetByVec(setNids, this->listNodeLabels);
	int idNext = *( setNids.rbegin() )+1;

	set<int> idsSeenBefore;

	for(int i=0; i<(int)this->listNodeLabels.size(); ++i)
	{
		// keep a sorted list
		int lvid = this->listNodeLabels[i];
		if( idsSeenBefore.find( lvid ) != idsSeenBefore.end() )
		{
			lvid = idNext++;
		}
		listNodeLabelsNew[i] = lvid;
		idsSeenBefore.insert(lvid);
	}

	// now inc the id of the internal nodes
	for(int i=numLeaves; i<(int)listNodeLabelsNew.size(); ++i )
	{
		listNodeLabelsNew[i] += 3*numLeaves;
	}

	// now write back
    this->listNodeLabels = listNodeLabelsNew;
}

void MarginalTree :: RearrangeParIncOrder()
{
//cout << "--RearrangeParIncOrder:\n";
	// sometimes the parent position is out of order, say 1,3,3,2,2,1,...
	// we can rearrange the internal node so that it becomes 1,2,2,3,3,1...
	// check the order of the appreance of the parent node
	// CAUTION: after this, need to perform descendent list rebuilt

//#if 0
	int curParOrderIndex = GetNumLeaves();
	map<int,int> mapCurParPosToNewParPos;
	set<int> setSeePars;
	queue<int> nodesToProc;
	// add in the leaves first
	//vector<int> parposListNew(listParentNodePos.size() );				// the new par and dist list
	//parposListNew[ parposListNew.size()-1 ] = -1;			// the last one is always -1
	//vector<double> distListNew(listEdgeDist.size() );
	for(int i=0; i<GetNumLeaves(); ++i)
	{
		nodesToProc.push(i);
	}
	vector<int> listNewParsInOrder;
	while(nodesToProc.empty() == false)
	{
		int nodeCur = nodesToProc.front();
		nodesToProc.pop();
		int parpos = GetParent(nodeCur);
		if( parpos < 0)
		{
			// root, do nothing
			continue;
		}
//cout << "nodecur = " << nodeCur << ", parpos = " << parpos << endl;
		if( setSeePars.find(parpos) == setSeePars.end() )
		{
			// add to set and move to next
			setSeePars.insert(parpos);
		}
		else
		{
			// have seen before, so record the mapping
			YW_ASSERT_INFO(mapCurParPosToNewParPos.find(parpos) == mapCurParPosToNewParPos.end(), "Should not be here");;
			mapCurParPosToNewParPos.insert( map<int,int> :: value_type(parpos, curParOrderIndex) );
//cout << "map old pos " << parpos << " to " << curParOrderIndex << endl;
			++curParOrderIndex;
			// when a parent node is done, process it
			nodesToProc.push(parpos);

			listNewParsInOrder.push_back(parpos);
		}
	}

		
	// now swap the par positions
	vector<int> parposListNew;
	vector<double> distListNew = listEdgeDist;
	for( int ii=0; ii<(int)GetNumLeaves(); ++ii )
	{
		YW_ASSERT_INFO( mapCurParPosToNewParPos.find( listParentNodePos[ii] ) != mapCurParPosToNewParPos.end(), "False" );
		parposListNew.push_back( mapCurParPosToNewParPos[ listParentNodePos[ii] ] );
	}
	// then output the internal node in the given order
	for(int ii=0; ii<(int)listNewParsInOrder.size(); ++ii)
	{
		int nindex = listNewParsInOrder[ii];
		if( mapCurParPosToNewParPos.find( listParentNodePos[nindex] ) != mapCurParPosToNewParPos.end() )
		{
			parposListNew.push_back( mapCurParPosToNewParPos[ listParentNodePos[nindex] ] );
//cout << "set dist of edge " << nindex << " (old dist " << distListNew[nindex] << " to node " << mapCurParPosToNewParPos[ nindex ] << " w/ dist ";
//cout << listEdgeDist[ mapCurParPosToNewParPos[ nindex ] ] << endl;
			distListNew[nindex] = listEdgeDist[ mapCurParPosToNewParPos[ nindex ] ];
		}
		else
		{
			parposListNew.push_back(-1);
		}
	}

	// finally set up the new lists
	this->listParentNodePos = parposListNew;
	this->listEdgeDist = distListNew;
//#endif
}

string MarginalTree :: GetNewickNoBrLen() const
{
	// get the newick format w/o branch length
	string strCurr = this->GetNewick();
	PhylogenyTreeBasic trPhy;
	trPhy.ConsOnNewick(strCurr);
	trPhy.Order();
	string res;
	trPhy.ConsNewick(res);
	return res;
}

string MarginalTree :: GetNewickNoBrLen2() const
{
    //
    return GetNewickAt( GetTotNodesNum()-1, true, false );
}

void MarginalTree :: RemapLeafLabels(const map<int,int> &mapLeafLblsToNew)
{
#if 0
cout << "RemapLeafLabels: ";
this->Dump();
cout << "mapLeafLblsToNew: ";
for(map<int,int> :: const_iterator it = mapLeafLblsToNew.begin(); it != mapLeafLblsToNew.end(); ++it)
{
cout << "[" << it->first << "," << it->second << "]  ";
}
cout << endl;
this->Dump();
#endif
    // convert each existing labels to consecutive labels e.g. 0, 1, 2, ...
    for(int i=0;i<(int)listNodeLabels.size(); ++i)
    {
        int lblCur = listNodeLabels[i];
//cout << "lblCur: " << lblCur << endl;
        YW_ASSERT_INFO( lblCur < 0 || mapLeafLblsToNew.find(lblCur) != mapLeafLblsToNew.end(), "Fail to find123");
        if( lblCur >= 0 )
        {
            listNodeLabels[i] = (*(mapLeafLblsToNew.find(lblCur))).second;
        }
    }
    // rebuild descendent info
    BuildDescendantInfo();
}

void MarginalTree :: MapLeafLblConsecutiveOrder( vector<int> &listLeafLblsOld )
{
    listLeafLblsOld.clear();
    int idNext = 0;
    MapLeafLblConsecutiveOrderAt( this->GetRoot(), idNext, listLeafLblsOld );
    // adding the remaining internal nodes
    for(int i=GetNumLeaves(); i<GetTotNodesNum(); ++i)
    {
        listLeafLblsOld.push_back( GetLabel(i) );
        SetLabel(i, idNext);
        ++idNext;
    }
}

void MarginalTree :: MapLeafLblConsecutiveOrderAt(int rootST, int &idNext, vector<int> &listLeafLblsOld )
{
    if( IsLeaf(rootST))
    {
        listLeafLblsOld.push_back( GetLabel(rootST) );
        SetLabel( rootST, idNext );
        ++idNext;
    }
    else
    {
        MapLeafLblConsecutiveOrderAt( GetLeftDescendant(rootST), idNext, listLeafLblsOld );
        MapLeafLblConsecutiveOrderAt( GetRightDescendant(rootST), idNext, listLeafLblsOld );
    }
}

void MarginalTree :: ResetIncLabel()
{
    //
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        listNodeLabels[i] = i;
    }
}

void MarginalTree :: IncLabels()
{
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        ++listNodeLabels[i];
    }
}

void MarginalTree :: FindSibLeafPairs( vector<pair<int,int> > &listSibPairs ) const
{
//cout << "FindSibLeafPairs:\n";
//Dump();
    // find leaves that are siblings (return the index (note not label) of the sib pairs)
    for(int i=GetNumLeaves(); i<GetTotNodesNum(); ++i)
    {
        //
        int nvleft = GetLeftDescendant(i);
        int nvRight = GetRightDescendant(i);
        if( IsLeaf(nvleft) == true && IsLeaf(nvRight) == true )
        {
            pair<int,int> pp(nvleft, nvRight);
            listSibPairs.push_back(pp);
        }
    }
    YW_ASSERT_INFO(listSibPairs.size() > 0, "Must have at least one pair");
}

void MarginalTree :: MakeLeafSubtreeOfTwo( int posLeaf, int lblChild1, int lblChild2, double len1, double len2 )
{
//cout << "MakeLeafSubtreeOfTwo: posLeaf: " << posLeaf << ", child1:" << lblChild1 << ", child2:" << lblChild2 << ", len1:" << len1 << ", len2:" << len2 << endl;
    // add two new leaves below a leaf (here, the two new leaves are located at the end of leaves; and the new internal (original elaf)
    // is right next to these new leaves)
    // also clean up the tree a bit (set labels of internal nodes to be -1)
    vector<int> listNodeLabelsNew;
    vector<int> listParentNodePosNew;
    vector<double> listEdgeDistNew;
    
    //
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        if( i != posLeaf )
        {
            listNodeLabelsNew.push_back( GetLabel(i) );
            listParentNodePosNew.push_back( GetParent(i)+2 );
            listEdgeDistNew.push_back( GetEdgeLen(i) );
        }
    }
    // add the two new leaves
    listNodeLabelsNew.push_back(lblChild1);
    listNodeLabelsNew.push_back(lblChild2);
    int posCur = (int)listNodeLabelsNew.size();
    listNodeLabelsNew.push_back(-1);
    listParentNodePosNew.push_back(posCur);
    listParentNodePosNew.push_back(posCur);
    listParentNodePosNew.push_back( GetParent(posLeaf)+2 );
    listEdgeDistNew.push_back(len1);
    listEdgeDistNew.push_back(len2);
    listEdgeDistNew.push_back( GetEdgeLen(posLeaf) );
    for(int i=GetNumLeaves(); i< GetTotNodesNum(); ++i)
    {
        listNodeLabelsNew.push_back( -1 );
        if( GetParent(i) >= 0 )
        {
            listParentNodePosNew.push_back( GetParent(i)+2 );
        }
        else
        {
            listParentNodePosNew.push_back(-1);
        }
        listEdgeDistNew.push_back( GetEdgeLen(i) );
    }
    
    // now update the info
    ++this->numLeaves;
    this->listNodeLabels = listNodeLabelsNew;
    this->listParentNodePos = listParentNodePosNew;
    this->listEdgeDist = listEdgeDistNew;
    listLeftDescs.clear();
    listRightDescs.clear();
    BuildDescendantInfo();
}

void MarginalTree :: GetLabelListForLeaf( vector<int> &listLbls) const
{
    //
    listLbls.clear();
    for(int i=0; i<GetNumLeaves(); ++i)
    {
        listLbls.push_back( GetLabel(i) );
    }
}

void MarginalTree :: FindDiffSubtreesFrom( const MarginalTree &mtreeRef, set<int> &setDiffBrs, set<int> &setDiffRefMissed ) const
{
    // find all branches (subtrees below them) that are not in the reference tree
    // setDiffBrs: in this tree but not in reference tree
    // setDiffRefMissed: in reference tree but not in this tree
    vector< set<int> > listSubtreesRef;
    mtreeRef.ConsDecedentLeavesInfoLabels( listSubtreesRef );
    vector< set<int> > listSubtreesThis;
    ConsDecedentLeavesInfoLabels( listSubtreesThis );
    set< set<int> > setSubtreesRef;
    PopulateSetByVecGen( setSubtreesRef, listSubtreesRef );
    set< set<int> > setSubtreesThis;
    PopulateSetByVecGen( setSubtreesThis, listSubtreesThis );
    //
    setDiffBrs.clear();
    for(int i=0; i<(int)listSubtreesThis.size(); ++i)
    {
        if( setSubtreesRef.find( listSubtreesThis[i] ) == setSubtreesRef.end()  )
        {
            setDiffBrs.insert(i);
            
#if 0
            // alsoinsert any ancestral edge into it
            set<int> nodesAnces;
            GetPath( i, GetRoot(), nodesAnces );
            UnionSets( setDiffBrs, nodesAnces );
            
            // and also all the silbings
            for( set<int> :: iterator itg = nodesAnces.begin(); itg != nodesAnces.end(); ++itg )
            {
                if( IsLeaf(*itg) == false )
                {
                    setDiffBrs.insert( GetLeftDescendant(*itg) );
                    setDiffBrs.insert( GetRightDescendant(*itg) );
                }
            }
#endif
        }
    }
    setDiffRefMissed.clear();
    for(int i=0; i<(int)listSubtreesRef.size(); ++i)
    {
        if( setSubtreesThis.find( listSubtreesRef[i] ) == setSubtreesThis.end()  )
        {
            setDiffRefMissed.insert(i);
        }
    }
}

bool MarginalTree :: IsOutgroup(int lvid) const
{
//cout << "IsOutgroup: lvid = " << lvid << ", tree is: ";
//Dump();
    int rtn = GetRoot();
    // check two children of root
    int lc = GetLeftDescendant(rtn);
    if( IsLeaf(lc) )
    {
        if( GetLabel(lc) == lvid )
        {
//cout << "good OG\n";
            return true;
        }
    }
    int rc = GetRightDescendant(rtn);
    if( IsLeaf(rc) )
    {
        if( GetLabel(rc) == lvid )
        {
//cout << "good OG\n";
            return true;
        }
    }
//cout << "BAD OG\n";
    return false;
}

void MarginalTree :: CollectNumEdgesToAnces(std::map<int, std::map<int, int> > &mapNodeDistToAnces) const
{
    // return the number of edges from node to nodeAnces (its ancestor)
    // if nodeAnces is not the ancestor node of node, return -1
    mapNodeDistToAnces.clear();
    
    // init the root
    int root = GetRoot();
    mapNodeDistToAnces[root][root] = 0;
    
    // start from root and walk the way down
    for(int t=GetTotNodesNum()-1; t>=0; --t)
    {
        // get the children
        set<int> listChildren;
        GetChildren(t, listChildren);
        map<int,map<int,int> > :: iterator itPar = mapNodeDistToAnces.find(t);
        YW_ASSERT_INFO(itPar != mapNodeDistToAnces.end(), "Fail to find334");
        for(set<int> :: iterator it = listChildren.begin(); it != listChildren.end(); ++it)
        {
            int nodeChild = *it;
            mapNodeDistToAnces[nodeChild][nodeChild] = 0;
            // deal with all the ancestor of parent and plus one
            for(map<int,int> :: iterator itPar2 = itPar->second.begin(); itPar2 != itPar->second.end(); ++itPar2)
            {
                mapNodeDistToAnces[nodeChild][itPar2->first] = itPar2->second+1;
            }
        }
    }
}

void MarginalTree :: GetAllNodesBottomUpFrom(int nodeRoot, vector<int> &listNodes) const
{
    // get all nodes in the bottom up order, starting from the nodeRoot
    if( IsLeaf(nodeRoot) == false  )
    {
        GetAllNodesBottomUpFrom(GetLeftDescendant(nodeRoot), listNodes);
        GetAllNodesBottomUpFrom(GetRightDescendant(nodeRoot), listNodes);
    }
    listNodes.push_back(nodeRoot);
}
