<commit_before>/*
 * substitutionMap.cpp
 *
 */

#include "SubstitutionMap.h"
#include "simplifier.h"
#include "../AST/ArrayTransformer.h"

namespace BEEV
{

SubstitutionMap::~SubstitutionMap()
{
	delete SolverMap;
}

// if false. Don't simplify while creating the substitution map.
// This makes it easier to spot how long is spent in the simplifier.
const bool simplify_during_create_subM = false;

//if a is READ(Arr,const) and b is BVCONST then return 1.
//if a is a symbol SYMBOL, return 1.
//if b is READ(Arr,const) and a is BVCONST then return -1
// if b is a symbol return -1.
//
//else return 0 by default
int TermOrder(const ASTNode& a, const ASTNode& b)
{
  const Kind k1 = a.GetKind();
  const Kind k2 = b.GetKind();

  if (k1 == SYMBOL)
  	return 1;

  if (k2 == SYMBOL)
  	return -1;

  //a is of the form READ(Arr,const), and b is const, or
  if ((k1 == READ
       && a[0].GetKind() == SYMBOL
       && a[1].GetKind() == BVCONST
       && (k2 == BVCONST)))
    return 1;

  //b is of the form READ(Arr,const), and a is const, or
  //b is of the form var, and a is const
  if ((k1 == BVCONST)
      && ((k2 == READ
           && b[0].GetKind() == SYMBOL
           && b[1].GetKind() == BVCONST)))
    return -1;

  return 0;
} //End of TermOrder()

//This finds conjuncts which are one of: (= SYMBOL BVCONST), (= BVCONST (READ SYMBOL BVCONST)),
// (IFF SYMBOL TRUE), (IFF SYMBOL FALSE), (IFF SYMBOL SYMBOL), (=SYMBOL SYMBOL)
// or (=SYMBOL BVCONST).
// It tries to remove the conjunct, storing it in the substitutionmap. It replaces it in the
// formula by true.
// The bvsolver puts expressions of the form (= SYMBOL TERM) into the solverMap.

ASTNode SubstitutionMap::CreateSubstitutionMap(const ASTNode& a,  ArrayTransformer*at)
{
  if (!bm->UserFlags.wordlevel_solve_flag)
    return a;

  ASTNode output = a;
  //if the variable has been solved for, then simply return it
  if (CheckSubstitutionMap(a, output))
    return output;

  if (!alreadyVisited.insert(a.GetNodeNum()).second)
  {
    return a;
  }

  //traverse a and populate the SubstitutionMap
  const Kind k = a.GetKind();
  if (SYMBOL == k && BOOLEAN_TYPE == a.GetType())
    {
      bool updated = UpdateSubstitutionMap(a, ASTTrue);
      output = updated ? ASTTrue : a;
      return output;
    }
  if (NOT == k && SYMBOL == a[0].GetKind())
    {
      bool updated = UpdateSubstitutionMap(a[0], ASTFalse);
      output = updated ? ASTTrue : a;
      return output;
    }

  if (IFF == k || EQ == k)
    {
	  ASTVec c = a.GetChildren();
	  SortByArith(c);

	  if (c[0] == c[1])
		  return ASTTrue;

      ASTNode c1;
      if (EQ == k)
    	  c1 = simplify_during_create_subM ? simp->SimplifyTerm(c[1]) : c[1];
      else// (IFF == k )
    	  c1= simplify_during_create_subM ?  simp->SimplifyFormula_NoRemoveWrites(c[1], false) : c[1];

      bool updated = UpdateSubstitutionMap(c[0], c1);
      output = updated ? ASTTrue : a;

      if (updated)
        {
          //fill the arrayname readindices vector if e0 is a
          //READ(Arr,index) and index is a BVCONST
          int to;
          if ((to =TermOrder(c[0],c1))==1 && c[0].GetKind() == READ)
              at->FillUp_ArrReadIndex_Vec(c[0], c1);
          else if (to ==-1 && c1.GetKind() == READ)
                    at->FillUp_ArrReadIndex_Vec(c1,c[0]);
        }

      return output;
    }

  if (XOR == k)
  {
	  if (a.Degree() !=2)
		  return output;

	  int to = TermOrder(a[0],a[1]);
	  if (0 == to)
		  return output;

	  ASTNode symbol,rhs;
	  if (to==1)
	  {
		  symbol = a[0];
		  rhs = a[1];
	  }
	  else
	  {
		  symbol = a[0];
		  rhs = a[1];
	  }

	  assert(symbol.GetKind() == SYMBOL);

	  // If either side is already solved for.
	  if (CheckSubstitutionMap(symbol) || CheckSubstitutionMap(rhs))
		  return output;

	  if (UpdateSolverMap(symbol, bm->CreateNode(NOT, rhs)))
			return ASTTrue;
		else
			return output;
  }

  if (AND == k)
    {
      ASTVec o;
      ASTVec c = a.GetChildren();
      for (ASTVec::iterator
             it = c.begin(), itend = c.end();
           it != itend; it++)
        {
          simp->UpdateAlwaysTrueFormSet(*it);
          ASTNode aaa = CreateSubstitutionMap(*it,at);

          if (ASTTrue != aaa)
            {
              if (ASTFalse == aaa)
                return ASTFalse;
              else
                o.push_back(aaa);
            }
        }
      if (o.size() == 0)
        return ASTTrue;

      if (o.size() == 1)
        return o[0];

      return bm->CreateNode(AND, o);
    }

  //printf("I gave up on kind: %d node: %d\n", k, a.GetNodeNum());
  return output;
} //end of CreateSubstitutionMap()

ASTNode SubstitutionMap::applySubstitutionMap(const ASTNode& n)
{
	bm->GetRunTimes()->start(RunTimes::ApplyingSubstitutions);
	ASTNodeMap cache;
	ASTNode result =  replace(n,*SolverMap,cache,bm->defaultNodeFactory, false);
	bm->GetRunTimes()->stop(RunTimes::ApplyingSubstitutions);
	return result;
}

ASTNode SubstitutionMap::applySubstitutionMapUntilArrays(const ASTNode& n)
{
	bm->GetRunTimes()->start(RunTimes::ApplyingSubstitutions);
	ASTNodeMap cache;
	ASTNode result = replace(n,*SolverMap,cache,bm->defaultNodeFactory, true);
	bm->GetRunTimes()->stop(RunTimes::ApplyingSubstitutions);
	return result;
}

ASTNode SubstitutionMap::replace(const ASTNode& n, ASTNodeMap& fromTo,
                ASTNodeMap& cache, NodeFactory * nf)
{
    return replace(n,fromTo,cache,nf,false);
}

// NOTE the fromTo map is changed as we traverse downwards.
// We call replace on each of the things in the fromTo map aswell.
// This is in case we have a fromTo map: (x maps to y), (y maps to 5),
// and pass the replace() function the node "x" to replace, then it
// will return 5, rather than y.

ASTNode SubstitutionMap::replace(const ASTNode& n, ASTNodeMap& fromTo,
		ASTNodeMap& cache, NodeFactory * nf, bool stopAtArrays)
{

        ASTNodeMap::const_iterator it;
	if ((it = cache.find(n)) != cache.end())
		return it->second;

	if ((it = fromTo.find(n)) != fromTo.end())
	{
		if (n.GetIndexWidth() != 0)
		{
			const ASTNode& r = it->second;
			assert(r.GetIndexWidth() == n.GetIndexWidth());
			assert(BVTypeCheck(r));
			ASTNode replaced = replace(r, fromTo, cache,nf,stopAtArrays);
			if (replaced != r)
				fromTo[n] = replaced;

			return replaced;
		}
		ASTNode replaced = replace(it->second, fromTo, cache,nf,stopAtArrays);
		if (replaced != it->second)
			fromTo[n] = replaced;

		return replaced;
	}

	// These can't be created like regular nodes are. Skip 'em for now.
	if (n.isConstant() || n.GetKind() == SYMBOL /*|| n.GetKind() == WRITE*/)
		return n;

         if (stopAtArrays && n.GetType() == ARRAY_TYPE)
            {
           return n;
            }

	ASTVec children;
	children.reserve(n.GetChildren().size());
	for (unsigned i = 0; i < n.GetChildren().size(); i++)
	{
		children.push_back(replace(n[i], fromTo, cache,nf,stopAtArrays));
	}

	// This code short-cuts if the children are the same. Nodes with the same children,
	// won't have necessarily given the same node if the simplifyingNodeFactory is enabled
	// now, but wasn't enabled when the node was created. Shortcutting saves lots of time.

	ASTNode result;
	if (n.GetType() == BOOLEAN_TYPE)
	{
		assert(children.size() > 0);
		if (children != n.GetChildren()) // short-cut.
		{
			result = nf->CreateNode(n.GetKind(), children);
		}
		else
			result = n;
	}
	else
	{
		assert(children.size() > 0);
		if (children != n.GetChildren()) // short-cut.
		{
			// If the index and value width aren't saved, they are reset sometimes (??)
			const unsigned int indexWidth = n.GetIndexWidth();
			const unsigned int valueWidth = n.GetValueWidth();
			result = nf->CreateArrayTerm(n.GetKind(),indexWidth, n.GetValueWidth(),
					children);
			assert(result.GetValueWidth() == valueWidth);

		}
		else
			result = n;
	}

	if (n != result)
	{
		assert(BVTypeCheck(result));
		assert(result.GetValueWidth() == n.GetValueWidth());
		assert(result.GetIndexWidth() == n.GetIndexWidth());
	}

	cache[n] = result;
	return result;
}

// Adds to the dependency graph that n0 depends on the variables in n1.
// It's not the transitive closure of the dependencies. Just the variables in the expression "n1".
// This is only needed as long as all the substitution rules haven't been written through.
void SubstitutionMap::buildDepends(const ASTNode& n0, const ASTNode& n1)
{
	if (n0.GetKind() != SYMBOL)
		return;

	if (n1.isConstant())
		return;

	vector<Symbols*> av;
	vars.VarSeenInTerm(vars.getSymbol(n1), rhs_visited, rhs, av);

	sort(av.begin(), av.end());
	for (int i =0; i < av.size();i++)
	{
		if (i!=0 && av[i] == av[i-1])
			continue; // Treat it like a set of Symbol* in effect.

		ASTNodeSet* sym = (vars.TermsAlreadySeenMap.find(av[i])->second);
		if(rhsAlreadyAdded.find(sym) != rhsAlreadyAdded.end())
			continue;
		rhsAlreadyAdded.insert(sym);

		//cout << loopCount++ << " ";
		//cout << "initial" << rhs.size() << " Adding: " <<sym->size();
		rhs.insert(sym->begin(), sym->end());
		//cout << "final:" << rhs.size();
		//cout << "added:" << sym << endl;

	}

	assert (dependsOn.find(n0) == dependsOn.end());
	dependsOn.insert(make_pair(n0,vars.getSymbol(n1)));
}

// Take the transitive closure of the varsToCheck. Storing the result in visited.
void SubstitutionMap::loops_helper(const set<ASTNode>& varsToCheck, set<ASTNode>& visited)
{
	set<ASTNode>::const_iterator visitedIt = visited.begin();

	set<ASTNode> toVisit;
	vector<ASTNode> visitedN;

	// for each variable.
	for (set<ASTNode>::const_iterator varIt = varsToCheck.begin(); varIt != varsToCheck.end(); varIt++)
	{
		while (*visitedIt < *varIt && visitedIt != visited.end())
			visitedIt++;

		if (*visitedIt == *varIt)
			continue;

		visitedN.push_back(*varIt);
		DependsType::iterator it;
		if ((it = dependsOn.find(*varIt)) != dependsOn.end())
		{
			Symbols* s= it->second;
			bool destruct;
			ASTNodeSet* varsSeen = vars.SetofVarsSeenInTerm(s,destruct);
			toVisit.insert(varsSeen->begin(), varsSeen->end());

			if (destruct)
				delete varsSeen;
		}
	}

	visited.insert(visitedN.begin(), visitedN.end());

	visitedN.clear();

	if (toVisit.size() !=0)
		loops_helper(toVisit, visited);
}

// If n0 is replaced by n1 in the substitution map. Will it cause a loop?
// i.e. will the dependency graph be an acyclic graph still.
// For example, if we have x = F(y,z,w), it would make the substitutionMap loop
// if there's already z = F(x).
bool SubstitutionMap::loops(const ASTNode& n0, const ASTNode& n1)
	{
	if (n0.GetKind() != SYMBOL)
		return false; // sometimes this function is called with constants on the lhs.

	if (n1.isConstant())
		return false; // constants contain no variables. Can't loop.

	// We are adding an edge FROM n0, so unless there is already an edge TO n0,
	// there is no change it can loop. Unless adding this would add a TO and FROM edge.
	if (rhs.find(n0) == rhs.end())
	{
		return vars.VarSeenInTerm(n0,n1);
	}

	if (n1.GetKind() == SYMBOL && dependsOn.find(n1) == dependsOn.end() )
		return false; // The rhs is a symbol and doesn't appear.

	if (debug_substn)
		cout << loopCount++  << endl;

	bool destruct = true;
	ASTNodeSet* dependN = vars.SetofVarsSeenInTerm(n1,destruct);

	if (debug_substn)
	{
		cout << n0 << " " <<  n1.GetNodeNum(); //<< " Expression size:" << bm->NodeSize(n1,true);
		cout << "Variables in expression: "<< dependN->size() << endl;
	}

	set<ASTNode> depend(dependN->begin(), dependN->end());

	if (destruct)
		delete dependN;

	set<ASTNode> visited;
	loops_helper(depend,visited);

	bool loops = visited.find(n0) != visited.end();

	if (debug_substn)
		cout << "Visited:" << visited.size() << "Loops:" << loops << endl;

	return (loops);
	}

};
<commit_msg>Refactor. Neaten up the CreateSubstitutionMap function.<commit_after>/*
 * substitutionMap.cpp
 *
 */

#include "SubstitutionMap.h"
#include "simplifier.h"
#include "../AST/ArrayTransformer.h"

namespace BEEV
{

SubstitutionMap::~SubstitutionMap()
{
	delete SolverMap;
}

// if false. Don't simplify while creating the substitution map.
// This makes it easier to spot how long is spent in the simplifier.
const bool simplify_during_create_subM = false;

//if a is READ(Arr,const) and b is BVCONST then return 1.
//if a is a symbol SYMBOL, return 1.
//if b is READ(Arr,const) and a is BVCONST then return -1
// if b is a symbol return -1.
//
//else return 0 by default
int TermOrder(const ASTNode& a, const ASTNode& b)
{
  const Kind k1 = a.GetKind();
  const Kind k2 = b.GetKind();

  if (k1 == SYMBOL)
  	return 1;

  if (k2 == SYMBOL)
  	return -1;

  //a is of the form READ(Arr,const), and b is const, or
  if ((k1 == READ
       && a[0].GetKind() == SYMBOL
       && a[1].GetKind() == BVCONST
       && (k2 == BVCONST)))
    return 1;

  //b is of the form READ(Arr,const), and a is const, or
  //b is of the form var, and a is const
  if ((k1 == BVCONST)
      && ((k2 == READ
           && b[0].GetKind() == SYMBOL
           && b[1].GetKind() == BVCONST)))
    return -1;

  return 0;
} //End of TermOrder()

//This finds conjuncts which are one of: (= SYMBOL BVCONST), (= BVCONST (READ SYMBOL BVCONST)),
// (IFF SYMBOL TRUE), (IFF SYMBOL FALSE), (IFF SYMBOL SYMBOL), (=SYMBOL SYMBOL)
// or (=SYMBOL BVCONST).
// It tries to remove the conjunct, storing it in the substitutionmap. It replaces it in the
// formula by true.
// The bvsolver puts expressions of the form (= SYMBOL TERM) into the solverMap.

ASTNode SubstitutionMap::CreateSubstitutionMap(const ASTNode& a,  ArrayTransformer*at)
{
  if (!bm->UserFlags.wordlevel_solve_flag)
    return a;

  ASTNode output;
  //if the variable has been solved for, then simply return it
  if (CheckSubstitutionMap(a, output))
    return output;

  if (!alreadyVisited.insert(a.GetNodeNum()).second)
  {
    return a;
  }

  output = a;

  //traverse a and populate the SubstitutionMap
  const Kind k = a.GetKind();
  if (SYMBOL == k && BOOLEAN_TYPE == a.GetType())
    {
      bool updated = UpdateSubstitutionMap(a, ASTTrue);
      output = updated ? ASTTrue : a;
    }
  else if (NOT == k && SYMBOL == a[0].GetKind())
    {
      bool updated = UpdateSubstitutionMap(a[0], ASTFalse);
      output = updated ? ASTTrue : a;
    }
  else if (IFF == k || EQ == k)
    {
	  ASTVec c = a.GetChildren();

	  if (simplify_during_create_subM)
		  SortByArith(c);

	  if (c[0] == c[1])
		  return ASTTrue;

      ASTNode c1;
      if (EQ == k)
    	  c1 = simplify_during_create_subM ? simp->SimplifyTerm(c[1]) : c[1];
      else// (IFF == k )
    	  c1= simplify_during_create_subM ?  simp->SimplifyFormula_NoRemoveWrites(c[1], false) : c[1];

      bool updated = UpdateSubstitutionMap(c[0], c1);
      output = updated ? ASTTrue : a;

      if (updated)
        {
          //fill the arrayname readindices vector if e0 is a
          //READ(Arr,index) and index is a BVCONST
          int to;
          if ((to =TermOrder(c[0],c1))==1 && c[0].GetKind() == READ)
              at->FillUp_ArrReadIndex_Vec(c[0], c1);
          else if (to ==-1 && c1.GetKind() == READ)
                    at->FillUp_ArrReadIndex_Vec(c1,c[0]);
        }
    }
  else if (XOR == k)
  {
	  if (a.Degree() !=2)
		  return output;

	  int to = TermOrder(a[0],a[1]);
	  if (0 == to)
		  return output;

	  ASTNode symbol,rhs;
	  if (to==1)
	  {
		  symbol = a[0];
		  rhs = a[1];
	  }
	  else
	  {
		  symbol = a[0];
		  rhs = a[1];
	  }

	  assert(symbol.GetKind() == SYMBOL);

	  // If either side is already solved for.
	  if (CheckSubstitutionMap(symbol) || CheckSubstitutionMap(rhs))
	  {}
	  else if (UpdateSolverMap(symbol, bm->CreateNode(NOT, rhs)))
			output =  ASTTrue;
  }
  else if (AND == k)
    {
	  const ASTVec& c = a.GetChildren();
	  ASTVec o;
      o.reserve(c.size());

      for (ASTVec::const_iterator
             it = c.begin(), itend = c.end();
           it != itend; it++)
        {
          simp->UpdateAlwaysTrueFormSet(*it);
          ASTNode aaa = CreateSubstitutionMap(*it,at);

          if (ASTTrue != aaa)
            {
              if (ASTFalse == aaa)
                return ASTFalse;
              else
                o.push_back(aaa);
            }
        }
      if (o.size() == 0)
        output =  ASTTrue;
      else if (o.size() == 1)
        output = o[0];
      else if (o != c)
    	output = bm->CreateNode(AND, o);
      else
    	output = a;
    }

  return output;
} //end of CreateSubstitutionMap()

ASTNode SubstitutionMap::applySubstitutionMap(const ASTNode& n)
{
	bm->GetRunTimes()->start(RunTimes::ApplyingSubstitutions);
	ASTNodeMap cache;
	ASTNode result =  replace(n,*SolverMap,cache,bm->defaultNodeFactory, false);
	bm->GetRunTimes()->stop(RunTimes::ApplyingSubstitutions);
	return result;
}

ASTNode SubstitutionMap::applySubstitutionMapUntilArrays(const ASTNode& n)
{
	bm->GetRunTimes()->start(RunTimes::ApplyingSubstitutions);
	ASTNodeMap cache;
	ASTNode result = replace(n,*SolverMap,cache,bm->defaultNodeFactory, true);
	bm->GetRunTimes()->stop(RunTimes::ApplyingSubstitutions);
	return result;
}

ASTNode SubstitutionMap::replace(const ASTNode& n, ASTNodeMap& fromTo,
                ASTNodeMap& cache, NodeFactory * nf)
{
    return replace(n,fromTo,cache,nf,false);
}

// NOTE the fromTo map is changed as we traverse downwards.
// We call replace on each of the things in the fromTo map aswell.
// This is in case we have a fromTo map: (x maps to y), (y maps to 5),
// and pass the replace() function the node "x" to replace, then it
// will return 5, rather than y.

ASTNode SubstitutionMap::replace(const ASTNode& n, ASTNodeMap& fromTo,
		ASTNodeMap& cache, NodeFactory * nf, bool stopAtArrays)
{

        ASTNodeMap::const_iterator it;
	if ((it = cache.find(n)) != cache.end())
		return it->second;

	if ((it = fromTo.find(n)) != fromTo.end())
	{
		if (n.GetIndexWidth() != 0)
		{
			const ASTNode& r = it->second;
			assert(r.GetIndexWidth() == n.GetIndexWidth());
			assert(BVTypeCheck(r));
			ASTNode replaced = replace(r, fromTo, cache,nf,stopAtArrays);
			if (replaced != r)
				fromTo[n] = replaced;

			return replaced;
		}
		ASTNode replaced = replace(it->second, fromTo, cache,nf,stopAtArrays);
		if (replaced != it->second)
			fromTo[n] = replaced;

		return replaced;
	}

	// These can't be created like regular nodes are. Skip 'em for now.
	if (n.isConstant() || n.GetKind() == SYMBOL /*|| n.GetKind() == WRITE*/)
		return n;

         if (stopAtArrays && n.GetType() == ARRAY_TYPE)
            {
           return n;
            }

	ASTVec children;
	children.reserve(n.GetChildren().size());
	for (unsigned i = 0; i < n.GetChildren().size(); i++)
	{
		children.push_back(replace(n[i], fromTo, cache,nf,stopAtArrays));
	}

	// This code short-cuts if the children are the same. Nodes with the same children,
	// won't have necessarily given the same node if the simplifyingNodeFactory is enabled
	// now, but wasn't enabled when the node was created. Shortcutting saves lots of time.

	ASTNode result;
	if (n.GetType() == BOOLEAN_TYPE)
	{
		assert(children.size() > 0);
		if (children != n.GetChildren()) // short-cut.
		{
			result = nf->CreateNode(n.GetKind(), children);
		}
		else
			result = n;
	}
	else
	{
		assert(children.size() > 0);
		if (children != n.GetChildren()) // short-cut.
		{
			// If the index and value width aren't saved, they are reset sometimes (??)
			const unsigned int indexWidth = n.GetIndexWidth();
			const unsigned int valueWidth = n.GetValueWidth();
			result = nf->CreateArrayTerm(n.GetKind(),indexWidth, n.GetValueWidth(),
					children);
			assert(result.GetValueWidth() == valueWidth);

		}
		else
			result = n;
	}

	if (n != result)
	{
		assert(BVTypeCheck(result));
		assert(result.GetValueWidth() == n.GetValueWidth());
		assert(result.GetIndexWidth() == n.GetIndexWidth());
	}

	cache[n] = result;
	return result;
}

// Adds to the dependency graph that n0 depends on the variables in n1.
// It's not the transitive closure of the dependencies. Just the variables in the expression "n1".
// This is only needed as long as all the substitution rules haven't been written through.
void SubstitutionMap::buildDepends(const ASTNode& n0, const ASTNode& n1)
{
	if (n0.GetKind() != SYMBOL)
		return;

	if (n1.isConstant())
		return;

	vector<Symbols*> av;
	vars.VarSeenInTerm(vars.getSymbol(n1), rhs_visited, rhs, av);

	sort(av.begin(), av.end());
	for (int i =0; i < av.size();i++)
	{
		if (i!=0 && av[i] == av[i-1])
			continue; // Treat it like a set of Symbol* in effect.

		ASTNodeSet* sym = (vars.TermsAlreadySeenMap.find(av[i])->second);
		if(rhsAlreadyAdded.find(sym) != rhsAlreadyAdded.end())
			continue;
		rhsAlreadyAdded.insert(sym);

		//cout << loopCount++ << " ";
		//cout << "initial" << rhs.size() << " Adding: " <<sym->size();
		rhs.insert(sym->begin(), sym->end());
		//cout << "final:" << rhs.size();
		//cout << "added:" << sym << endl;

	}

	assert (dependsOn.find(n0) == dependsOn.end());
	dependsOn.insert(make_pair(n0,vars.getSymbol(n1)));
}

// Take the transitive closure of the varsToCheck. Storing the result in visited.
void SubstitutionMap::loops_helper(const set<ASTNode>& varsToCheck, set<ASTNode>& visited)
{
	set<ASTNode>::const_iterator visitedIt = visited.begin();

	set<ASTNode> toVisit;
	vector<ASTNode> visitedN;

	// for each variable.
	for (set<ASTNode>::const_iterator varIt = varsToCheck.begin(); varIt != varsToCheck.end(); varIt++)
	{
		while (*visitedIt < *varIt && visitedIt != visited.end())
			visitedIt++;

		if (*visitedIt == *varIt)
			continue;

		visitedN.push_back(*varIt);
		DependsType::iterator it;
		if ((it = dependsOn.find(*varIt)) != dependsOn.end())
		{
			Symbols* s= it->second;
			bool destruct;
			ASTNodeSet* varsSeen = vars.SetofVarsSeenInTerm(s,destruct);
			toVisit.insert(varsSeen->begin(), varsSeen->end());

			if (destruct)
				delete varsSeen;
		}
	}

	visited.insert(visitedN.begin(), visitedN.end());

	visitedN.clear();

	if (toVisit.size() !=0)
		loops_helper(toVisit, visited);
}

// If n0 is replaced by n1 in the substitution map. Will it cause a loop?
// i.e. will the dependency graph be an acyclic graph still.
// For example, if we have x = F(y,z,w), it would make the substitutionMap loop
// if there's already z = F(x).
bool SubstitutionMap::loops(const ASTNode& n0, const ASTNode& n1)
	{
	if (n0.GetKind() != SYMBOL)
		return false; // sometimes this function is called with constants on the lhs.

	if (n1.isConstant())
		return false; // constants contain no variables. Can't loop.

	// We are adding an edge FROM n0, so unless there is already an edge TO n0,
	// there is no change it can loop. Unless adding this would add a TO and FROM edge.
	if (rhs.find(n0) == rhs.end())
	{
		return vars.VarSeenInTerm(n0,n1);
	}

	if (n1.GetKind() == SYMBOL && dependsOn.find(n1) == dependsOn.end() )
		return false; // The rhs is a symbol and doesn't appear.

	if (debug_substn)
		cout << loopCount++  << endl;

	bool destruct = true;
	ASTNodeSet* dependN = vars.SetofVarsSeenInTerm(n1,destruct);

	if (debug_substn)
	{
		cout << n0 << " " <<  n1.GetNodeNum(); //<< " Expression size:" << bm->NodeSize(n1,true);
		cout << "Variables in expression: "<< dependN->size() << endl;
	}

	set<ASTNode> depend(dependN->begin(), dependN->end());

	if (destruct)
		delete dependN;

	set<ASTNode> visited;
	loops_helper(depend,visited);

	bool loops = visited.find(n0) != visited.end();

	if (debug_substn)
		cout << "Visited:" << visited.size() << "Loops:" << loops << endl;

	return (loops);
	}

};
<|endoftext|>


<commit_before>/**
 * Created by Jian Chen
 * @since  2016.01.09
 * @author Jian Chen <admin@chensoft.com>
 * @link   http://chensoft.com
 */
#include "cmd.hpp"
#include "log.hpp"
#include "path.hpp"
#include <string.h>
#include <iostream>

using namespace chen;
using namespace chen::cmd;

// -----------------------------------------------------------------------------
// parser
parser::parser(const std::string &app)
: _app(app)
{
}

// parse
void parser::parse(int argc, const char *const argv[])
{
    // argc must greater than 1
    if (argc <= 1)
        return;

    // set app name if it's empty
    if (this->_app.empty())
        this->_app = path::basename(argv[0]);

    // find current action first
    int index = 1;
    std::string name;

    while (index < argc)
    {
        auto param = argv[index];

        if (param[0] != '-')
        {
            // sub-action is joined by dots
            std::string temp(!name.empty() ? name + "." + param : param);

            if (this->_define.find(temp) != this->_define.end())
                name = temp;
            else
                break;
        }
        else
        {
            break;
        }

        ++index;
    }

    // using empty action if not found
    auto it = this->_define.find(name);
    std::unique_ptr<chen::cmd::action> action(it != this->_define.end() ? new chen::cmd::action(it->second) : new chen::cmd::action);

    // parse the objects and options
    auto objects = action->objects();
    auto options = action->options();

    std::vector<chen::cmd::object>::iterator object = objects.begin();

    while (index < argc)
    {
        auto param = argv[index];

        if (param[0] == '-')
        {
            // handle option
            auto curr = (strlen(param) >= 2) && (param[1] == '-') ? std::string(param + 2) : std::string(param + 1);
            auto next = (index + 1 < argc) ? argv[index + 1] : nullptr;
            auto find = options.find(curr);

            if (find != options.end())
            {
                if (next && (next[0] != '-'))
                {
                    find->second.set(next);
                    index += 2;
                }
                else
                {
                    find->second.set();
                    ++index;
                }
            }
            else
            {
                break;
            }
        }
        else
        {
            // handle object
            if (object != objects.end())
            {
                auto len = object->size();
                auto cur = index;

                while ((index < argc) && (index - cur < len))
                {
                    auto temp = argv[index];

                    if (temp[0] != '-')
                        object->add(temp);
                    else
                        break;

                    ++index;
                }

                // restore if the object is not reach len
                auto size = object->val().size();

                if (len && (size < len))
                {
                    // length is error
                    index = cur;
                    object->clear();
                    break;
                }
                else
                {
                    ++object;
                }
            }
            else
            {
                break;
            }
        }
    }

    // the rest unresolved params
    this->_rest.clear();

    for (; index < argc; ++index)
        this->_rest.push_back(argv[index]);

    // store the current action
    this->_action.swap(action);
}

// action
void parser::action(const std::string &name,
                    const std::string &desc,
                    std::function<void (const chen::cmd::parser &parser)> bind)
{
    this->_define.emplace(name, chen::cmd::action(name, desc, bind));
}

// object
void parser::object(const std::string &action,
                    const std::string &name,
                    std::size_t size)
{
    auto it = this->_define.find(action);

    if (it != this->_define.end())
        it->second.add(chen::cmd::object(name, size));
    else
        throw chen::cmd::error_action("cmd action not found");
}

// option
void parser::option(const std::string &action,
                    const std::string &name,
                    const std::string &tiny,
                    const std::string &desc,
                    const any &def,
                    const any &pre)
{
    if (tiny.size() > 1)
        throw chen::cmd::error_option("cmd tiny name can be only one character");

    auto it = this->_define.find(action);

    if (it != this->_define.end())
        it->second.add(name, chen::cmd::option(name, tiny, desc, def, pre));
    else
        throw chen::cmd::error_action("cmd action not found");
}

// current
std::string parser::current() const
{
    if (this->_action)
        return this->_action->name();
    else
        throw chen::cmd::error_action("cmd current action not found");
}

// object value
std::vector<std::string> parser::objVal(const std::string &object) const
{
    if (this->_action)
    {
        auto temp = this->_action->objects();

        for (auto &obj : temp)
        {
            if (obj.name() == object)
            {
                return obj.val();
            }
        }

        return std::vector<std::string>();
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

// option value
bool parser::boolVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<bool>(find->second.val()) : false;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::int32_t parser::intVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<std::int32_t>(find->second.val()) : 0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::string parser::strVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? find->second.val() : "";
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::int64_t parser::int64Val(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<std::int64_t>(find->second.val()) : 0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

double parser::doubleVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<double>(find->second.val()) : 0.0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

// rest
const std::vector<std::string>& parser::rest() const
{
    return this->_rest;
}

// usage
void parser::usage() const
{
    if (!this->_prefix.empty())
        this->output(this->_prefix);

    // todo
    this->output("xxx");

    if (!this->_suffix.empty())
        this->output(this->_suffix);
}

void parser::prefix(const std::string &text)
{
    this->_prefix = text;
}

void parser::suffix(const std::string &text)
{
    this->_suffix = text;
}

void parser::suggest(const std::string &alias, const std::string &action)
{
    this->_suggest[alias] = action;
}

// output
void parser::output(const std::string &text) const
{
    std::cout << text << std::endl;
}

// -----------------------------------------------------------------------------
// action
action::action(const std::string &name,
               const std::string &desc,
               std::function<void (const chen::cmd::parser &parser)> bind)
: _name(name)
, _desc(desc)
, _bind(bind)
{
}

void action::add(const chen::cmd::object &object)
{
    this->_objects.push_back(object);
}

void action::add(const std::string &name, const chen::cmd::option &option)
{
    this->_options.emplace(name, option);
}

const std::string& action::name() const
{
    return this->_name;
}

const std::string& action::desc() const
{
    return this->_desc;
}

const std::function<void (const chen::cmd::parser &parser)>& action::bind() const
{
    return this->_bind;
}

const std::vector<chen::cmd::object>& action::objects() const
{
    return this->_objects;
}

const std::map<std::string, chen::cmd::option>& action::options() const
{
    return this->_options;
}

// -----------------------------------------------------------------------------
// object
object::object(const std::string &name, std::size_t size)
: _name(name)
, _size(size)
{
}

void object::add(const std::string &val)
{
    this->_val.push_back(val);
}

void object::clear()
{
    this->_val.clear();
}

const std::string& object::name() const
{
    return this->_name;
}

std::size_t object::size() const
{
    return this->_size;
}

const std::vector<std::string>& object::val() const
{
    return this->_val;
}

// -----------------------------------------------------------------------------
// option
option::option(const std::string &name,
               const std::string &tiny,
               const std::string &desc,
               const chen::any &def,
               const chen::any &pre)
: _name(name)
, _tiny(tiny)
, _desc(desc)
, _def(def)
, _pre(pre)
{
}

void option::set()
{
    this->_set = true;
}

void option::set(const chen::any &val)
{
    this->_set = true;
    this->_val = val;
}

void option::clear()
{
    this->_set = false;
    this->_val = chen::any();
}

const std::string& option::name() const
{
    return this->_name;
}

const std::string& option::tiny() const
{
    return this->_tiny;
}

const std::string& option::desc() const
{
    return this->_desc;
}

const chen::any& option::val() const
{
    return this->_val;
}

const chen::any& option::def() const
{
    return this->_def;
}

const chen::any& option::pre() const
{
    return this->_pre;
}

const bool option::valid() const
{
    return this->_set;
}<commit_msg>cmd: should throw error if no action found<commit_after>/**
 * Created by Jian Chen
 * @since  2016.01.09
 * @author Jian Chen <admin@chensoft.com>
 * @link   http://chensoft.com
 */
#include "cmd.hpp"
#include "log.hpp"
#include "path.hpp"
#include <string.h>
#include <iostream>

using namespace chen;
using namespace chen::cmd;

// -----------------------------------------------------------------------------
// parser
parser::parser(const std::string &app)
: _app(app)
{
}

// parse
void parser::parse(int argc, const char *const argv[])
{
    // argc must greater than 1
    if (argc <= 1)
        return;

    // set app name if it's empty
    if (this->_app.empty())
        this->_app = path::basename(argv[0]);

    // find current action first
    int index = 1;
    std::string name;

    while (index < argc)
    {
        auto param = argv[index];

        if (param[0] != '-')
        {
            // sub-action is joined by dots
            std::string temp(!name.empty() ? name + "." + param : param);

            if (this->_define.find(temp) != this->_define.end())
                name = temp;
            else
                break;
        }
        else
        {
            break;
        }

        ++index;
    }

    // using empty action if not found
    auto it = this->_define.find(name);
    if (it == this->_define.end())
        throw chen::cmd::error_action("cmd can not find current action");

    std::unique_ptr<chen::cmd::action> action(new chen::cmd::action(it->second));

    // parse the objects and options
    auto objects = action->objects();
    auto options = action->options();

    std::vector<chen::cmd::object>::iterator object = objects.begin();

    while (index < argc)
    {
        auto param = argv[index];

        if (param[0] == '-')
        {
            // handle option
            auto curr = (strlen(param) >= 2) && (param[1] == '-') ? std::string(param + 2) : std::string(param + 1);
            auto next = (index + 1 < argc) ? argv[index + 1] : nullptr;
            auto find = options.find(curr);

            if (find != options.end())
            {
                if (next && (next[0] != '-'))
                {
                    find->second.set(next);
                    index += 2;
                }
                else
                {
                    find->second.set();
                    ++index;
                }
            }
            else
            {
                break;
            }
        }
        else
        {
            // handle object
            if (object != objects.end())
            {
                auto len = object->size();
                auto cur = index;

                while ((index < argc) && (index - cur < len))
                {
                    auto temp = argv[index];

                    if (temp[0] != '-')
                        object->add(temp);
                    else
                        break;

                    ++index;
                }

                // restore if the object is not reach len
                if (len && (index - cur < len))
                {
                    // length is error
                    index = cur;
                    object->clear();
                    break;
                }
                else
                {
                    ++object;
                }
            }
            else
            {
                break;
            }
        }
    }

    // the rest unresolved params
    this->_rest.clear();

    for (; index < argc; ++index)
        this->_rest.push_back(argv[index]);

    // store the current action
    this->_action.swap(action);
}

// action
void parser::action(const std::string &name,
                    const std::string &desc,
                    std::function<void (const chen::cmd::parser &parser)> bind)
{
    this->_define.emplace(name, chen::cmd::action(name, desc, bind));
}

// object
void parser::object(const std::string &action,
                    const std::string &name,
                    std::size_t size)
{
    auto it = this->_define.find(action);

    if (it != this->_define.end())
        it->second.add(chen::cmd::object(name, size));
    else
        throw chen::cmd::error_action("cmd action not found");
}

// option
void parser::option(const std::string &action,
                    const std::string &name,
                    const std::string &tiny,
                    const std::string &desc,
                    const any &def,
                    const any &pre)
{
    if (tiny.size() > 1)
        throw chen::cmd::error_option("cmd tiny name can be only one character");

    auto it = this->_define.find(action);

    if (it != this->_define.end())
        it->second.add(name, chen::cmd::option(name, tiny, desc, def, pre));
    else
        throw chen::cmd::error_action("cmd action not found");
}

// current
std::string parser::current() const
{
    if (this->_action)
        return this->_action->name();
    else
        throw chen::cmd::error_action("cmd current action not found");
}

// object value
std::vector<std::string> parser::objVal(const std::string &object) const
{
    if (this->_action)
    {
        auto temp = this->_action->objects();

        for (auto &obj : temp)
        {
            if (obj.name() == object)
            {
                return obj.val();
            }
        }

        return std::vector<std::string>();
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

// option value
bool parser::boolVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<bool>(find->second.val()) : false;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::int32_t parser::intVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<std::int32_t>(find->second.val()) : 0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::string parser::strVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? find->second.val() : "";
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

std::int64_t parser::int64Val(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<std::int64_t>(find->second.val()) : 0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

double parser::doubleVal(const std::string &option) const
{
    if (this->_action)
    {
        auto temp = this->_action->options();
        auto find = temp.find(option);
        return find != temp.end() ? static_cast<double>(find->second.val()) : 0.0;
    }
    else
    {
        throw chen::cmd::error_action("cmd current action not found");
    }
}

// rest
const std::vector<std::string>& parser::rest() const
{
    return this->_rest;
}

// usage
void parser::usage() const
{
    if (!this->_prefix.empty())
        this->output(this->_prefix);

    // todo
    this->output("xxx");

    if (!this->_suffix.empty())
        this->output(this->_suffix);
}

void parser::prefix(const std::string &text)
{
    this->_prefix = text;
}

void parser::suffix(const std::string &text)
{
    this->_suffix = text;
}

void parser::suggest(const std::string &alias, const std::string &action)
{
    this->_suggest[alias] = action;
}

// output
void parser::output(const std::string &text) const
{
    std::cout << text << std::endl;
}

// -----------------------------------------------------------------------------
// action
action::action(const std::string &name,
               const std::string &desc,
               std::function<void (const chen::cmd::parser &parser)> bind)
: _name(name)
, _desc(desc)
, _bind(bind)
{
}

void action::add(const chen::cmd::object &object)
{
    this->_objects.push_back(object);
}

void action::add(const std::string &name, const chen::cmd::option &option)
{
    this->_options.emplace(name, option);
}

const std::string& action::name() const
{
    return this->_name;
}

const std::string& action::desc() const
{
    return this->_desc;
}

const std::function<void (const chen::cmd::parser &parser)>& action::bind() const
{
    return this->_bind;
}

const std::vector<chen::cmd::object>& action::objects() const
{
    return this->_objects;
}

const std::map<std::string, chen::cmd::option>& action::options() const
{
    return this->_options;
}

// -----------------------------------------------------------------------------
// object
object::object(const std::string &name, std::size_t size)
: _name(name)
, _size(size)
{
}

void object::add(const std::string &val)
{
    this->_val.push_back(val);
}

void object::clear()
{
    this->_val.clear();
}

const std::string& object::name() const
{
    return this->_name;
}

std::size_t object::size() const
{
    return this->_size;
}

const std::vector<std::string>& object::val() const
{
    return this->_val;
}

// -----------------------------------------------------------------------------
// option
option::option(const std::string &name,
               const std::string &tiny,
               const std::string &desc,
               const chen::any &def,
               const chen::any &pre)
: _name(name)
, _tiny(tiny)
, _desc(desc)
, _def(def)
, _pre(pre)
{
}

void option::set()
{
    this->_set = true;
}

void option::set(const chen::any &val)
{
    this->_set = true;
    this->_val = val;
}

void option::clear()
{
    this->_set = false;
    this->_val = chen::any();
}

const std::string& option::name() const
{
    return this->_name;
}

const std::string& option::tiny() const
{
    return this->_tiny;
}

const std::string& option::desc() const
{
    return this->_desc;
}

const chen::any& option::val() const
{
    return this->_val;
}

const chen::any& option::def() const
{
    return this->_def;
}

const chen::any& option::pre() const
{
    return this->_pre;
}

const bool option::valid() const
{
    return this->_set;
}<|endoftext|>


<commit_before>/*******************************************************************************
    Copyright (c) The Taichi Authors (2016- ). All Rights Reserved.
    The use of this software is governed by the LICENSE file.
*******************************************************************************/

#include <condition_variable>
#include <taichi/system/threading.h>
#include <thread>

TC_NAMESPACE_BEGIN

class ThreadPool {
public:
  std::vector<std::thread> threads;
  std::condition_variable cv;
  std::condition_variable master_cv;
  std::mutex mutex;
  int task_head;
  int task_tail;
  int running_threads;
  uint64 timestamp;
  bool started;
  bool exiting;

  void do_task(int i) {
    double ret = 0.0;
    for (int t = 0; t < 100000000; t++) {
      ret += t * 1e-20;
    }
    TC_P(int(i + ret));
  }

  void target() {
    uint64 last_timestamp = 0;
    while (true) {
      {
        std::unique_lock<std::mutex> lock(mutex);
        cv.wait(lock, [this, &last_timestamp] {
          return (timestamp > last_timestamp) || this->exiting;
        });
        last_timestamp = timestamp;
        if (exiting) {
          break;
        } else {
          started = true;
          running_threads++;
        }
      }

      while (true) {
        // For a single parallel task
        int task_id;
        {
          std::unique_lock<std::mutex> lock(mutex);
          task_id = task_head;
          if (task_id >= task_tail)
            break;
          task_head++;
        }
        do_task(task_id);
      }

      {
        std::unique_lock<std::mutex> lock(mutex);
        running_threads--;
        if (running_threads == 0)
          master_cv.notify_one();
      }
    }
  }

  ThreadPool(int num_threads) {
    exiting = false;
    started = false;
    running_threads = 0;
    threads.resize((std::size_t)num_threads);
    for (int i = 0; i < num_threads; i++) {
      threads[i] = std::thread([this] { this->target(); });
    }
  }

  void run(int tail) {
    started = false;
    task_head = 0;
    task_tail = tail;
    timestamp++;
    cv.notify_all();
    {
      std::unique_lock<std::mutex> lock(mutex);
      master_cv.wait(lock, [this] { return started && running_threads == 0; });
    }
    TC_ASSERT(task_head == task_tail);
  }

  ~ThreadPool() {
    {
      std::lock_guard<std::mutex> lg(mutex);
      exiting = true;
    }
    cv.notify_all();
    for (int i = 0; i < threads.size(); i++) {
      threads[i].join();
    }
  }
};

bool test_threading() {
  auto tp = ThreadPool(10);
  for (int i = 0; i < 10; i++) {
    tp.run(10);
  }
  return true;
}

TC_NAMESPACE_END
<commit_msg>pass in context and func to thread pool<commit_after>/*******************************************************************************
    Copyright (c) The Taichi Authors (2016- ). All Rights Reserved.
    The use of this software is governed by the LICENSE file.
*******************************************************************************/

#include <condition_variable>
#include <taichi/system/threading.h>
#include <thread>

TC_NAMESPACE_BEGIN

using CPUTaskFunc = void (*)(void *, int i);

class ThreadPool {
public:
  std::vector<std::thread> threads;
  std::condition_variable slave_cv;
  std::condition_variable master_cv;
  std::mutex mutex;
  int task_head;
  int task_tail;
  int running_threads;
  uint64 timestamp;
  bool started;
  bool exiting;
  CPUTaskFunc func;
  void *context;

  void target() {
    uint64 last_timestamp = 0;
    while (true) {
      {
        std::unique_lock<std::mutex> lock(mutex);
        slave_cv.wait(lock, [this, &last_timestamp] {
          return (timestamp > last_timestamp) || this->exiting;
        });
        last_timestamp = timestamp;
        if (exiting) {
          break;
        } else {
          started = true;
          running_threads++;
        }
      }

      while (true) {
        // For a single parallel task
        int task_id;
        {
          std::unique_lock<std::mutex> lock(mutex);
          task_id = task_head;
          if (task_id >= task_tail)
            break;
          task_head++;
        }
        func(context, task_id);
      }

      {
        std::unique_lock<std::mutex> lock(mutex);
        running_threads--;
        if (running_threads == 0)
          master_cv.notify_one();
      }
    }
  }

  ThreadPool(int num_threads) {
    exiting = false;
    started = false;
    running_threads = 0;
    timestamp = 0;
    task_head = 0;
    task_tail = 0;
    threads.resize((std::size_t)num_threads);
    for (int i = 0; i < num_threads; i++) {
      threads[i] = std::thread([this] { this->target(); });
    }
  }

  void run(int tail, void *context, CPUTaskFunc func) {
    this->context = context;
    this->func = func;
    started = false;
    task_head = 0;
    task_tail = tail;
    timestamp++;

    // wake all slaves
    slave_cv.notify_all();
    {
      std::unique_lock<std::mutex> lock(mutex);
      master_cv.wait(lock, [this] { return started && running_threads == 0; });
    }
    TC_ASSERT(task_head == task_tail);
  }

  ~ThreadPool() {
    {
      std::lock_guard<std::mutex> lg(mutex);
      exiting = true;
    }
    slave_cv.notify_all();
    for (int i = 0; i < threads.size(); i++) {
      threads[i].join();
    }
  }
};

bool test_threading() {
  auto tp = ThreadPool(10);
  for (int j = 0; j < 10; j++) {
    tp.run(10, &j, [](void *j, int i){
      double ret = 0.0;
      for (int t = 0; t < 100000000; t++) {
        ret += t * 1e-20;
      }
      TC_P(int(i + ret + 10 * *(int *)j));
    });
  }
  return true;
}

TC_NAMESPACE_END
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Silvio Traversaro, CoDyCo project
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Silvio Traversaro */

#include "kdl_export/kdl_export.hpp"
#include <urdf_model/model.h>
#include <console_bridge/console.h>
#include <iostream>

using namespace std;

namespace kdl_export{

// construct vector
urdf::Vector3 toUrdf(const KDL::Vector & v)
{
    return urdf::Vector3(v.x(), v.y(), v.z());
}

// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
    double x,y,z,w;
    r.GetQuaternion(x,y,z,w);
    return urdf::Rotation(x,y,z,w);
}

// construct pose
urdf::Pose toUrdf(const KDL::Frame & p)
{
    urdf::Pose ret;
    ret.rotation = toUrdf(p.M);
    ret.position = toUrdf(p.p);
    return ret;
}

// construct joint
urdf::Joint toUrdf(const KDL::Joint & jnt)
{
    urdf::Joint ret;
    ret.name = jnt.getName();
    switch(jnt.getType())
    {
        case KDL::Joint::RotAxis:
        case KDL::Joint::RotX:
        case KDL::Joint::RotY:
        case KDL::Joint::RotZ:
            ret.type = urdf::Joint::REVOLUTE;
            ret.parent_to_joint_origin_transform = toUrdf(KDL::Frame(KDL::Rotation::Identity(),jnt.JointOrigin()));
            ret.axis = toUrdf(jnt.JointAxis());
        break;
        case KDL::Joint::TransAxis:
        case KDL::Joint::TransX:
        case KDL::Joint::TransY:
        case KDL::Joint::TransZ:
            ret.type = urdf::Joint::PRISMATIC;
            ret.parent_to_joint_origin_transform = toUrdf(KDL::Frame(KDL::Rotation::Identity(),jnt.JointOrigin()));
            ret.axis = toUrdf(jnt.JointAxis());
        break;
        default:
            logWarn("Converting unknown joint type of joint '%s' into a fixed joint",jnt.getTypeName().c_str());
        case KDL::Joint::None:
            ret.type = urdf::Joint::FIXED;
    }
    return ret;
}

// construct inertia
urdf::Inertial toUrdf(KDL::RigidBodyInertia i)
{
  urdf::Inertial ret;
  ret.mass = i.getMass();
  ret.origin = toUrdf(KDL::Frame(KDL::Rotation::Identity(),i.getCOG()));
  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
  // however the kdl RigidBodyInertia constructor take the inertia with the COG as reference point,
  // but the getInertia
  KDL::RotationalInertia Ic;
  Ic = i.RefPoint(i.getCOG()).getRotationalInertia();
  ret.ixx = Ic.data[0];
  ret.ixy = Ic.data[1];
  ret.ixz = Ic.data[2];
  ret.iyy = Ic.data[4];
  ret.iyz = Ic.data[5];
  ret.izz = Ic.data[8];
  return ret;
}

/*
bool treeFromFile(const string& file, Tree& tree)
{
  TiXmlDocument urdf_xml;
  urdf_xml.LoadFile(file);
  return treeFromXml(&urdf_xml, tree);
}

bool treeFromParam(const string& param, Tree& tree)
{
  urdf::Model robot_model;
  if (!robot_model.initParam(param)){
    ROS_ERROR("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}

bool treeFromString(const string& xml, Tree& tree)
{
  TiXmlDocument urdf_xml;
  urdf_xml.Parse(xml.c_str());
  return treeFromXml(&urdf_xml, tree);
}

bool treeToXml(TiXmlDocument *xml_doc, Tree& tree)
{
    urdf::Model robot_model;
    treeToUrdfModel(
  return treeFromUrdfModel(robot_model, tree);
}
*/

bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
    unsigned int i = 0;
    robot_model.clear();
    robot_model.name_ = robot_name;

    //Add all links
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    KDL::SegmentMap::const_iterator root_seg;
    tree.getRootSegment(root_seg);
    tree.getSegments(segs);
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            logError("link '%s' is not unique.",  seg->first.c_str());
            robot_model.clear();
            return false;
        }
        else
        {
            boost::shared_ptr<urdf::Link> link;
            link.reset(new urdf::Link);

            //add name
            link->name = seg->first;

            //add inertial
            link->inertial.reset(new urdf::Inertial());
            *(link->inertial) = toUrdf(seg->second.segment.getInertia());

            //insert link
            robot_model.links_.insert(make_pair(seg->first,link));
            logDebug("successfully added a new link '%s'", link->name.c_str());
        }
    }

    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        //The fake root segment has no joint to add
        if( seg->first != root_seg->first ) {
            KDL::Joint jnt;
            jnt = seg->second.segment.getJoint();
            if (robot_model.getJoint(jnt.getName()))
            {
                logError("joint '%s' is not unique.", jnt.getName().c_str());
                robot_model.clear();
                return false;
            }
            else
            {
                boost::shared_ptr<urdf::Joint> joint;
                joint.reset(new urdf::Joint());

                //convert joint
                *joint = toUrdf(jnt);

                //insert parent
                joint->parent_link_name = seg->second.parent->first;

                //insert child
                joint->child_link_name = seg->first;

                //insert joint
                robot_model.joints_.insert(make_pair(seg->first,joint));
                logDebug("successfully added a new joint '%s'", jnt.getName().c_str());
            }
        }
    }

    // every link has children links and joints, but no parents, so we create a
    // local convenience data structure for keeping child->parent relations
    std::map<std::string, std::string> parent_link_tree;
    parent_link_tree.clear();

    // building tree: name mapping
    //try
    //{
        robot_model.initTree(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to build tree: %s", e.what());
        robot_model.clear();
        return false;
    }*/

    // find the root link
    //try
    //{
        robot_model.initRoot(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to find root link: %s", e.what());
        robot_model.reset();
        return false;
    }
    */
    return true;
}

//update topology and parameters
bool treeUpdateUrdfModel(const KDL::Tree& tree, urdf::ModelInterface& robot_model)
{
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    tree.getSegments(segs);

    //Update all links
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            boost::shared_ptr<urdf::Link> link = robot_model.links_[seg->first];
            //update inertial
            link->inertial.reset(new urdf::Inertial());
            *(link->inertial) = toUrdf(seg->second.segment.getInertia());

            logDebug("successfully updated link '%s'", link->name.c_str());
        }
        else
        {
            logError("link '%s' not found.",  seg->first.c_str());
            return false;
        }
    }

    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        KDL::Joint jnt;

        urdf::Joint new_joint;
        jnt = seg->second.segment.getJoint();
        if (robot_model.getJoint(jnt.getName()))
        {
            boost::shared_ptr<urdf::Joint> joint;
            joint = robot_model.joints_[jnt.getName()];

            //convert joint
            new_joint = toUrdf(jnt);

            //update existing joint
            joint->type = new_joint.type;
            joint->axis = new_joint.axis;
            joint->parent_to_joint_origin_transform = new_joint.parent_to_joint_origin_transform;

            logDebug("successfully updated joint '%s'", jnt.getName().c_str());
        }
        else
        {
            logError("joint '%s' not found.",  jnt.getName().c_str());
            return false;
        }
    }
}

}

<commit_msg>If no joint limits are specified, urdf::Joint::CONTINUOUS joint should be used<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Silvio Traversaro, CoDyCo project
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Silvio Traversaro */

#include "kdl_export/kdl_export.hpp"
#include <urdf_model/model.h>
#include <console_bridge/console.h>
#include <iostream>

using namespace std;

namespace kdl_export{

// construct vector
urdf::Vector3 toUrdf(const KDL::Vector & v)
{
    return urdf::Vector3(v.x(), v.y(), v.z());
}

// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
    double x,y,z,w;
    r.GetQuaternion(x,y,z,w);
    return urdf::Rotation(x,y,z,w);
}

// construct pose
urdf::Pose toUrdf(const KDL::Frame & p)
{
    urdf::Pose ret;
    ret.rotation = toUrdf(p.M);
    ret.position = toUrdf(p.p);
    return ret;
}

// construct joint
urdf::Joint toUrdf(const KDL::Joint & jnt)
{
    urdf::Joint ret;
    ret.name = jnt.getName();
    switch(jnt.getType())
    {
        case KDL::Joint::RotAxis:
        case KDL::Joint::RotX:
        case KDL::Joint::RotY:
        case KDL::Joint::RotZ:
            //using continuos if no joint limits are specified
            ret.type = urdf::Joint::CONTINUOUS;
            ret.parent_to_joint_origin_transform = toUrdf(KDL::Frame(KDL::Rotation::Identity(),jnt.JointOrigin()));
            ret.axis = toUrdf(jnt.JointAxis());
        break;
        case KDL::Joint::TransAxis:
        case KDL::Joint::TransX:
        case KDL::Joint::TransY:
        case KDL::Joint::TransZ:
            ret.type = urdf::Joint::PRISMATIC;
            ret.parent_to_joint_origin_transform = toUrdf(KDL::Frame(KDL::Rotation::Identity(),jnt.JointOrigin()));
            ret.axis = toUrdf(jnt.JointAxis());
        break;
        default:
            logWarn("Converting unknown joint type of joint '%s' into a fixed joint",jnt.getTypeName().c_str());
        case KDL::Joint::None:
            ret.type = urdf::Joint::FIXED;
    }
    return ret;
}

// construct inertia
urdf::Inertial toUrdf(KDL::RigidBodyInertia i)
{
  urdf::Inertial ret;
  ret.mass = i.getMass();
  ret.origin = toUrdf(KDL::Frame(KDL::Rotation::Identity(),i.getCOG()));
  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
  // however the kdl RigidBodyInertia constructor take the inertia with the COG as reference point,
  // but the getInertia
  KDL::RotationalInertia Ic;
  Ic = i.RefPoint(i.getCOG()).getRotationalInertia();
  ret.ixx = Ic.data[0];
  ret.ixy = Ic.data[1];
  ret.ixz = Ic.data[2];
  ret.iyy = Ic.data[4];
  ret.iyz = Ic.data[5];
  ret.izz = Ic.data[8];
  return ret;
}

/*
bool treeFromFile(const string& file, Tree& tree)
{
  TiXmlDocument urdf_xml;
  urdf_xml.LoadFile(file);
  return treeFromXml(&urdf_xml, tree);
}

bool treeFromParam(const string& param, Tree& tree)
{
  urdf::Model robot_model;
  if (!robot_model.initParam(param)){
    ROS_ERROR("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}

bool treeFromString(const string& xml, Tree& tree)
{
  TiXmlDocument urdf_xml;
  urdf_xml.Parse(xml.c_str());
  return treeFromXml(&urdf_xml, tree);
}

bool treeToXml(TiXmlDocument *xml_doc, Tree& tree)
{
    urdf::Model robot_model;
    treeToUrdfModel(
  return treeFromUrdfModel(robot_model, tree);
}
*/

bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
    unsigned int i = 0;
    robot_model.clear();
    robot_model.name_ = robot_name;

    //Add all links
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    KDL::SegmentMap::const_iterator root_seg;
    tree.getRootSegment(root_seg);
    tree.getSegments(segs);
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            logError("link '%s' is not unique.",  seg->first.c_str());
            robot_model.clear();
            return false;
        }
        else
        {
            boost::shared_ptr<urdf::Link> link;
            link.reset(new urdf::Link);

            //add name
            link->name = seg->first;

            //add inertial
            link->inertial.reset(new urdf::Inertial());
            *(link->inertial) = toUrdf(seg->second.segment.getInertia());

            //insert link
            robot_model.links_.insert(make_pair(seg->first,link));
            logDebug("successfully added a new link '%s'", link->name.c_str());
        }
    }

    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        //The fake root segment has no joint to add
        if( seg->first != root_seg->first ) {
            KDL::Joint jnt;
            jnt = seg->second.segment.getJoint();
            if (robot_model.getJoint(jnt.getName()))
            {
                logError("joint '%s' is not unique.", jnt.getName().c_str());
                robot_model.clear();
                return false;
            }
            else
            {
                boost::shared_ptr<urdf::Joint> joint;
                joint.reset(new urdf::Joint());

                //convert joint
                *joint = toUrdf(jnt);

                //insert parent
                joint->parent_link_name = seg->second.parent->first;

                //insert child
                joint->child_link_name = seg->first;

                //insert joint
                robot_model.joints_.insert(make_pair(seg->first,joint));
                logDebug("successfully added a new joint '%s'", jnt.getName().c_str());
            }
        }
    }

    // every link has children links and joints, but no parents, so we create a
    // local convenience data structure for keeping child->parent relations
    std::map<std::string, std::string> parent_link_tree;
    parent_link_tree.clear();

    // building tree: name mapping
    //try
    //{
        robot_model.initTree(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to build tree: %s", e.what());
        robot_model.clear();
        return false;
    }*/

    // find the root link
    //try
    //{
        robot_model.initRoot(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to find root link: %s", e.what());
        robot_model.reset();
        return false;
    }
    */
    return true;
}

//update topology and parameters
bool treeUpdateUrdfModel(const KDL::Tree& tree, urdf::ModelInterface& robot_model)
{
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    tree.getSegments(segs);

    //Update all links
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            boost::shared_ptr<urdf::Link> link = robot_model.links_[seg->first];
            //update inertial
            link->inertial.reset(new urdf::Inertial());
            *(link->inertial) = toUrdf(seg->second.segment.getInertia());

            logDebug("successfully updated link '%s'", link->name.c_str());
        }
        else
        {
            logError("link '%s' not found.",  seg->first.c_str());
            return false;
        }
    }

    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        KDL::Joint jnt;

        urdf::Joint new_joint;
        jnt = seg->second.segment.getJoint();
        if (robot_model.getJoint(jnt.getName()))
        {
            boost::shared_ptr<urdf::Joint> joint;
            joint = robot_model.joints_[jnt.getName()];

            //convert joint
            new_joint = toUrdf(jnt);

            //update existing joint
            joint->type = new_joint.type;
            joint->axis = new_joint.axis;
            joint->parent_to_joint_origin_transform = new_joint.parent_to_joint_origin_transform;

            logDebug("successfully updated joint '%s'", jnt.getName().c_str());
        }
        else
        {
            logError("joint '%s' not found.",  jnt.getName().c_str());
            return false;
        }
    }
}

}

<|endoftext|>


<commit_before>/*
 * File:  parserutils.cpp
 * Copyright (C) 2004 The Institute for System Programming of the Russian Academy of Sciences (ISP RAS)
 */

#include "sedna.h"
#include <string>
#include <stdlib.h>
#include <string.h>
#include "utf8.h"

using namespace std;

bool check_char_wellformedness(int c)
{
  if (c == 0x9 || c == 0xA || (c >= 0x20 && c <= 0xd7ff) || (c >= 0xe000 && c <= 0xfffd) || (c >= 0x1000 && c <= 0x10ffff))
     return true;
  else return false;
}

string erase_doublequot(char* lex_text)
{
  string lex = lex_text;
  if (lex.size() >= 3)
  {
    string find_text = string(1, lex[0]) + string(1, lex[0]);
    string repl_text = string("\\") + string(1, lex[0]);
    string text = lex.substr(1, lex.size()-2);//erase first and last characters
    string::size_type pos;
    for(;;)
    {
      pos = text.find(find_text);
      if (pos == string::npos) break;

      text = text.replace(pos, 2, repl_text);
    }

    //replase " with \" for Scheme part
    find_text = "\"";
    repl_text = "\\\"";

    pos = 0;

    for(;;)
    {
      pos = text.find(find_text, pos);
      if (pos == string::npos) break;

      if (pos == 0)
         text = text.replace(pos, 1, repl_text);
      else
      {
         if (text[pos-1] != '\\')
             text = text.replace(pos, 1, repl_text);
      }

      pos +=find_text.size() + 1;
    }

    lex = string("\"") + text + "\"";
  }
  else
  {
    lex[0] = '\"';
    lex[lex.size()-1] = '\"';
  }

  return  lex;

//  printf("lext_text=%s\n", lex_text);

}

string escape_quot(string text)
{
    //replase " with \" for Scheme part
    int pos;
    string find_text = "\"";
    string repl_text = "\\\"";

    pos = 0;

    for(;;)
    {
      pos = text.find(find_text, pos);
      if (pos == string::npos) break;

      if (pos == 0)
         text = text.replace(pos, 1, repl_text);
      else
      {
         if (text[pos-1] != '\\')
             text = text.replace(pos, 1, repl_text);
      }

      pos +=find_text.size() + 1;
    }

    return text;
}

string replace_entity(char* lex_text, string find_ent, string replc_ent)
{
  string lex = lex_text;
  string::size_type pos;

  for(;;)
  {
    pos = lex.find(find_ent);
    if (pos == string::npos) break;

    lex = lex.replace(pos, find_ent.size(), replc_ent);
  }

  strcpy(lex_text, lex.c_str());

  return lex;
}

string replace_charref(char* lex_text)
{
   string text = std::string(lex_text);
   string::size_type posb = 0, pose = 0;
   string char_ref;
   int char_code;
   string str_char_code;

   //replace hex char ref
   for (;;)
   {
      posb = text.find("&#x", posb);
      if (posb == string::npos) break;

      pose = text.find(";", posb);
      if (pose == string::npos) break;

      char_ref =  text.substr(posb+3, pose - (posb+3));

      char_code = strtol(char_ref.c_str(), NULL, 16);

      if (!check_char_wellformedness(char_code))
	throw USER_EXCEPTION(XQST0090);

//      printf("char_code=%d\n", char_code);
      str_char_code = string(utf8_encode_char(char_code));

      text = text.replace(posb, (pose-posb)+1, str_char_code.c_str(), 0, str_char_code.size());
   }

   posb=0;
   pose=0;

   //replace dec char ref
   for (;;)
   {
      posb = text.find("&#", posb);
      if (posb == string::npos) break;

      pose = text.find(";", posb);
      if (pose == string::npos) break;

      char_ref =  text.substr(posb+2, pose - (posb+2));

      char_code = atoi(char_ref.c_str());

      if (!check_char_wellformedness(char_code))
	throw USER_EXCEPTION(XQST0090);

//      printf("char_code=%d\n", char_code);
      str_char_code = string(utf8_encode_char(char_code));

      text = text.replace(posb, (pose-posb)+1, str_char_code.c_str(), 0, str_char_code.size());
   }

   return text;
}

/*
enum pragma_states {START_WS, P_PREFIX, P_LOC_NAME, NEXT_WS, PRCONTENT};

void * extract_pragma_content(char* text, char* qname, char* pr_content)
{
  state = START_WS;
  string loc_name, prefix;

  for (int i=0; i<strlen(text); i++)
  {
     if (state == START_WS)
     {
         if (text[i] == ' ' || text[i] == '\t' || text[i] == '\r' || text[i] == '\n') continue;
         else state = P_PREFIX;
     }

     if (state == P_PREFIX)
     {
        if ((text[i] >= 'a' && text[i] <= 'z') || (text[i] >= 'A' && text[i] <= 'Z')  || (text[i] >= '0' && text[i] <= '9') ||
	     text[i] == '_' || text[i] == '-' || text[i] == '.')
        {
             prefix +=text[i];
             continue;
        }
        else
        if (text[i] == ':')
        {
             state = P_LOC_NAME;
             continue;
        }
        else
        if (text[i] == )
     }
  }
}
*/

<commit_msg>entity ref replacement bug fix<commit_after>/*
 * File:  parserutils.cpp
 * Copyright (C) 2004 The Institute for System Programming of the Russian Academy of Sciences (ISP RAS)
 */

#include "sedna.h"
#include <string>
#include <stdlib.h>
#include <string.h>
#include "utf8.h"

using namespace std;

bool check_char_wellformedness(int c)
{
  if (c == 0x9 || c == 0xA || (c >= 0x20 && c <= 0xd7ff) || (c >= 0xe000 && c <= 0xfffd) || (c >= 0x1000 && c <= 0x10ffff))
     return true;
  else return false;
}

string erase_doublequot(char* lex_text)
{
  string lex = lex_text;
  if (lex.size() >= 3)
  {
    string find_text = string(1, lex[0]) + string(1, lex[0]);
    string repl_text = string("\\") + string(1, lex[0]);
    string text = lex.substr(1, lex.size()-2);//erase first and last characters
    string::size_type pos;
    for(;;)
    {
      pos = text.find(find_text);
      if (pos == string::npos) break;

      text = text.replace(pos, 2, repl_text);
    }

    //replase " with \" for Scheme part
    find_text = "\"";
    repl_text = "\\\"";

    pos = 0;

    for(;;)
    {
      pos = text.find(find_text, pos);
      if (pos == string::npos) break;

      if (pos == 0)
         text = text.replace(pos, 1, repl_text);
      else
      {
         if (text[pos-1] != '\\')
             text = text.replace(pos, 1, repl_text);
      }

      pos +=find_text.size() + 1;
    }

    lex = string("\"") + text + "\"";
  }
  else
  {
    lex[0] = '\"';
    lex[lex.size()-1] = '\"';
  }

  return  lex;

//  printf("lext_text=%s\n", lex_text);

}

string escape_quot(string text)
{
    //replase " with \" for Scheme part
    int pos;
    string find_text = "\"";
    string repl_text = "\\\"";

    pos = 0;

    for(;;)
    {
      pos = text.find(find_text, pos);
      if (pos == string::npos) break;

      if (pos == 0)
         text = text.replace(pos, 1, repl_text);
      else
      {
         if (text[pos-1] != '\\')
             text = text.replace(pos, 1, repl_text);
      }

      pos +=find_text.size() + 1;
    }

    return text;
}

string replace_entity(char* lex_text, string find_ent, string replc_ent)
{
  string lex = lex_text;
  string::size_type pos = 0;

  for(;;)
  {
    pos = lex.find(find_ent, pos);
    if (pos == string::npos) break;

    lex = lex.replace(pos, find_ent.size(), replc_ent);
    pos++;
  }

  strcpy(lex_text, lex.c_str());

  return lex;
}

string replace_charref(char* lex_text)
{
   string text = std::string(lex_text);
   string::size_type posb = 0, pose = 0;
   string char_ref;
   int char_code;
   string str_char_code;

   //replace hex char ref
   for (;;)
   {
      posb = text.find("&#x", posb);
      if (posb == string::npos) break;

      pose = text.find(";", posb);
      if (pose == string::npos) break;

      char_ref =  text.substr(posb+3, pose - (posb+3));

      char_code = strtol(char_ref.c_str(), NULL, 16);

      if (!check_char_wellformedness(char_code))
	throw USER_EXCEPTION(XQST0090);

//      printf("char_code=%d\n", char_code);
      str_char_code = string(utf8_encode_char(char_code));

      text = text.replace(posb, (pose-posb)+1, str_char_code.c_str(), 0, str_char_code.size());
   }

   posb=0;
   pose=0;

   //replace dec char ref
   for (;;)
   {
      posb = text.find("&#", posb);
      if (posb == string::npos) break;

      pose = text.find(";", posb);
      if (pose == string::npos) break;

      char_ref =  text.substr(posb+2, pose - (posb+2));

      char_code = atoi(char_ref.c_str());

      if (!check_char_wellformedness(char_code))
	throw USER_EXCEPTION(XQST0090);

//      printf("char_code=%d\n", char_code);
      str_char_code = string(utf8_encode_char(char_code));

      text = text.replace(posb, (pose-posb)+1, str_char_code.c_str(), 0, str_char_code.size());
   }

   return text;
}

/*
enum pragma_states {START_WS, P_PREFIX, P_LOC_NAME, NEXT_WS, PRCONTENT};

void * extract_pragma_content(char* text, char* qname, char* pr_content)
{
  state = START_WS;
  string loc_name, prefix;

  for (int i=0; i<strlen(text); i++)
  {
     if (state == START_WS)
     {
         if (text[i] == ' ' || text[i] == '\t' || text[i] == '\r' || text[i] == '\n') continue;
         else state = P_PREFIX;
     }

     if (state == P_PREFIX)
     {
        if ((text[i] >= 'a' && text[i] <= 'z') || (text[i] >= 'A' && text[i] <= 'Z')  || (text[i] >= '0' && text[i] <= '9') ||
	     text[i] == '_' || text[i] == '-' || text[i] == '.')
        {
             prefix +=text[i];
             continue;
        }
        else
        if (text[i] == ':')
        {
             state = P_LOC_NAME;
             continue;
        }
        else
        if (text[i] == )
     }
  }
}
*/

<|endoftext|>


<commit_before>class Solution {
public:
    /**
     * @param s input string
     * @return the longest palindromic substring
     */
    string longestPalindrome(string& s) {
        // Write your code here
        int i,j,k,lenS = s.size();
        int tmp_len;
        string rtLongPalin;
        int last_len = -1 , left_b = 0 ,right_b = 0 , max_len = 0;

        for (i=0;i<lenS;i++){
            if (i-last_len>=0)
                if (s[i] == s[i-last_len])

            else
            if (i-last_len-1>=0)
                if ( s[i] == s[i-last_len-1]){
                    last_len += 2;
                }
                else {
                    last_len = -1;
                }

            if (last_len > max_len){
                max_len = last_len;
                right_b=i;
                left_b=i-last_len+1;
            }

        }

        for (i=left_b;i<=right_b;i++)
            rtLongPalin += s[i];

        return rtLongPalin;
    }
};<commit_msg>Consideer 2 conditions seperately<commit_after>class Solution {
public:
    /**
     * @param s input string
     * @return the longest palindromic substring
     */
    string longestPalindrome(string& s) {
        // Write your code here
        int i,j,k,lenS = s.size();
        int tmp_len;
        string rtLongPalin;
        int last_len = -1 , left_b = 0 ,right_b = 0 , max_len = 0;

        // scan all odd palindrome
        for (i=0;i<lenS;i++){
            if (i-last_len-1>=0)
                if ( s[i] == s[i-last_len-1]){
                    last_len += 2;
                }
                else {
                    last_len = -1;
                }

            if (last_len > max_len){
                max_len = last_len;
                right_b=i;
                left_b=i-last_len+1;
            }
        }

        //scan all even palindrome

        last_len

        for (i=left_b;i<=right_b;i++)
            rtLongPalin += s[i];

        return rtLongPalin;
    }
};<|endoftext|>


<commit_before>#include <cstdio>

#define VERTEX_COUNT 100000
#define EDGE_COUNT 100000

using namespace std;

struct vertex
{
	int first_edge;
	bool vis;
}V[VERTEX_COUNT];

struct edge
{
	int endp, next;
}E[EDGE_COUNT];

int ec = 1;

int Q[VERTEX_COUNT];
int hp = 0, tp = 0;

void enqueue(int x)
{
	Q[tp++] = x;
}

int dequeue()
{
	return Q[hp++];
}

bool queue_empty()
{
	return hp == tp;
}

void add_edge(int u, int v)
{
	E[ec].next = V[u].first_edge;
	V[u].first_edge = ec;
	E[ec].endp = v;
	ec++;
}

void BFS()
{
	while (!queue_empty())
	{
		int u = dequeue();

		for (int cur = V[u].first_edge; cur != 0; cur++)
		{
			if (V[E[cur].endp].vis == false)
			{
				V[E[cur].endp].vis = true;
				enqueue(E[cur].endp);
			}
		}
	}
}

int main()
{
	int ivc, iec;
	scanf("%d%d", &ivc, &iec);

	for (int i = 0; i < iec; i++)
	{
		int u, v;
		scanf("%d%d", &u, &v);

		add_edge(u, v);
	}

	BFS();

	return 0;
}
<commit_msg>更正了根节点未入队以及查找子边时循环递增语句的错误<commit_after>#include <cstdio>

#define VERTEX_COUNT 100000
#define EDGE_COUNT 100000

using namespace std;

struct vertex
{
	int first_edge;
	bool vis;
}V[VERTEX_COUNT];

struct edge
{
	int endp, next;
}E[EDGE_COUNT];

int ec = 1;

int Q[VERTEX_COUNT];
int hp = 0, tp = 0;

void enqueue(int x)
{
	Q[tp++] = x;
}

int dequeue()
{
	return Q[hp++];
}

bool queue_empty()
{
	return hp == tp;
}

void add_edge(int u, int v)
{
	E[ec].next = V[u].first_edge;
	V[u].first_edge = ec;
	E[ec].endp = v;
	ec++;
}

void BFS()
{
	while (!queue_empty())
	{
		int u = dequeue();

		for (int cur = V[u].first_edge; cur != 0; cur = E[cur].next)
		{
			if (V[E[cur].endp].vis == false)
			{
				V[E[cur].endp].vis = true;
				enqueue(E[cur].endp);
			}
		}
	}
}

int main()
{
	int ivc, iec;
	scanf("%d%d", &ivc, &iec);

	for (int i = 0; i < iec; i++)
	{
		int u, v;
		scanf("%d%d", &u, &v);

		add_edge(u, v);
	}

	enqueue(1);

	BFS();

	return 0;
}
<|endoftext|>


<commit_before><commit_msg>implemented consistency check in dumptree_RD - it is checked if "Tag" contains the "prodName"<commit_after><|endoftext|>


<commit_before>void GLWidget::mouseMoveEvent(QMouseEvent *event)
{
  if (event->buttons() & Qt::LeftButton) {
    switch (activeViewport) { // Move cameras in viewports accordingly
      case ISO:   isoCamera->setDeltaTheta(toRadians(xClick - event->x() + (width / 2)));
                  isoCamera->setDeltaPhi(toRadians(event->y() - yClick));
                  break;
    }
  }
}

void GLWidget::mouseReleaseEvent(QMouseEvent *event)
{
  if (event->button() == Qt::LeftButton) {
    switch (activeViewport) {
      case ISO:   isoCamera->updateTheta(isoCamera->getDeltaTheta());
                  isoCamera->updatePhi(isoCamera->getDeltaPhi());
                  break;
    }
  }
}<commit_msg>Refine notes more<commit_after>isoCamera->setDeltaTheta(toRadians(xClick - event->x());
isoCamera->setDeltaPhi(toRadians(event->y() - yClick));
isoCamera->updateTheta(dTheta);
isoCamera->updatePhi(dPhi);<|endoftext|>


<commit_before>/******************************************************************
  DHT Temperature & Humidity Sensor library for Arduino.

  Features:
  - Support for DHT11 and DHT22/AM2302/RHT03
  - Auto detect sensor model
  - Very low memory footprint
  - Very small code

  http://www.github.com/markruys/arduino-DHT

  Written by Mark Ruys, mark@paracas.nl.

  BSD license, check license.txt for more information.
  All text above must be included in any redistribution.

  Datasheets:
  - http://www.micro4you.com/files/sensor/DHT11.pdf
  - http://www.adafruit.com/datasheets/DHT22.pdf
  - http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Weather/RHT03.pdf
  - http://meteobox.tk/files/AM2302.pdf

  Changelog:
   2013-06-10: Initial version
   2013-06-12: Refactored code
 ******************************************************************/

#include "DHT.h"

void DHT::setup(int pin, DHT_MODEL_t model)
{
  DHT::pin = pin;
  DHT::model = model;
  DHT::lastReadTime = millis() - 3000; // Make sure we do read the sensor in the next readSensor()

  if ( model == AUTO_DETECT) {
    DHT::model = DHT22;
    if ( getStatus() == ERROR_TIMEOUT ) {
      DHT::model = DHT11;
      delay(18); // ignore bits we might get
    }
  }
}

DHT::DHT_MODEL_t DHT::getModel()
{
  return model;
}

int DHT::getMinimumSamplingPeriod()
{
  return model == DHT11 ? 1001 : 2001;
}

float DHT::getHumidity()
{
  readSensor();
  return humidity;
}

float DHT::getTemperature()
{
  readSensor();
  return temperature;
}

DHT::DHT_ERROR_t DHT::getStatus()
{
  readSensor();
  return error;
}

#ifndef OPTIMIZE_SRAM_SIZE

const char* DHT::getStatusString()
{
  switch ( error ) {
    case DHT::ERROR_TIMEOUT:
      return "TIMEOUT";

    case DHT::ERROR_CHECKSUM:
      return "CHECKSUM";

    default:
      return "OK";
  }
}

#else

// At the expense of 26 bytes of extra PROGMEM, we save 11 bytes of
// SRAM by using the following method:

prog_char P_OK[]       PROGMEM = "OK";
prog_char P_TIMEOUT[]  PROGMEM = "TIMEOUT";
prog_char P_CHECKSUM[] PROGMEM = "CHECKSUM";

const char *DHT::getStatusString() {
  prog_char *c;
  switch ( error ) {
    case DHT::ERROR_CHECKSUM:
      c = P_CHECKSUM; break;

    case DHT::ERROR_TIMEOUT:
      c = P_TIMEOUT; break;

    default:
      c = P_OK; break;
  }

  static char buffer[9];
  strcpy_P(buffer, c);

  return buffer;
}

#endif

void DHT::readSensor()
{
  // Make sure we don't poll the sensor too often
  // - Max sample rate DHT11 is 1 Hz   (duty cicle 1000 ms)
  // - Max sample rate DHT22 is 0.5 Hz (duty cicle 2000 ms)
  unsigned long startTime = millis();
  if ( (unsigned long)(startTime - lastReadTime) < (model == DHT11 ? 1000L : 2000L) ) {
    return;
  }
  lastReadTime = startTime;

  temperature = NAN;
  humidity = NAN;

  // Request sample

  digitalWrite(pin, LOW); // Send start signal
  pinMode(pin, OUTPUT);
  if ( model == DHT11 ) {
    delay(18);
  }
  else {
    // This will fail for a DHT11 - that's how we can detect such a device
    delayMicroseconds(800);
  }

  pinMode(pin, INPUT);
  digitalWrite(pin, HIGH); // Switch bus to receive data

  // We're going to read 83 edges:
  // - First a FALLING, RISING, and FALLING edge for the start bit
  // - Then 40 bits: RISING and then a FALLING edge per bit
  // To keep our code simple, we accept any HIGH or LOW reading if it's max 85 usecs long

  word rawHumidity;
  word rawTemperature;
  word data;

  for ( int8_t i = -3 ; i < 2 * 40; i++ ) {
    byte age;
    startTime = micros();

    do {
      age = (unsigned long)(micros() - startTime);
      if ( age > 90 ) {
        error = ERROR_TIMEOUT;
        return;
      }
    }
    while ( digitalRead(pin) == (i & 1) ? HIGH : LOW );

    if ( i >= 0 && (i & 1) ) {
      // Now we are being fed our 40 bits
      data <<= 1;

      // A zero max 30 usecs, a one at least 68 usecs.
      if ( age > 30 ) {
        data |= 1; // we got a one
      }
    }

    switch ( i ) {
      case 31:
        rawHumidity = data;
        break;
      case 63:
        rawTemperature = data;
        data = 0;
        break;
    }
  }

  // Verify checksum

  if ( (byte)(((byte)rawHumidity) + (rawHumidity >> 8) + ((byte)rawTemperature) + (rawTemperature >> 8)) != data ) {
    error = ERROR_CHECKSUM;
    return;
  }

  // Store readings

  if ( model == DHT11 ) {
    humidity = rawHumidity >> 8;
    temperature = rawTemperature >> 8;
  }
  else {
    humidity = rawHumidity * 0.1;

    if ( rawTemperature & 0x8000 ) {
      rawTemperature = -(int16_t)(rawTemperature & 0x7FFF);
    }
    temperature = ((int16_t)rawTemperature) * 0.1;
  }

  error = ERROR_NONE;
}

float DHT::toFahrenheit(float fromCelcius)
{
  return 1.8 * fromCelcius + 32.0;
}

float DHT::toCelsius(float fromFahrenheit)
{
  return 0.5555556 * (fromFahrenheit - 32.0);
}
<commit_msg>getStatus() should not read out sensor, just return last error value<commit_after>/******************************************************************
  DHT Temperature & Humidity Sensor library for Arduino.

  Features:
  - Support for DHT11 and DHT22/AM2302/RHT03
  - Auto detect sensor model
  - Very low memory footprint
  - Very small code

  http://www.github.com/markruys/arduino-DHT

  Written by Mark Ruys, mark@paracas.nl.

  BSD license, check license.txt for more information.
  All text above must be included in any redistribution.

  Datasheets:
  - http://www.micro4you.com/files/sensor/DHT11.pdf
  - http://www.adafruit.com/datasheets/DHT22.pdf
  - http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Weather/RHT03.pdf
  - http://meteobox.tk/files/AM2302.pdf

  Changelog:
   2013-06-10: Initial version
   2013-06-12: Refactored code
 ******************************************************************/

#include "DHT.h"

void DHT::setup(int pin, DHT_MODEL_t model)
{
  DHT::pin = pin;
  DHT::model = model;
  DHT::lastReadTime = millis() - 3000; // Make sure we do read the sensor in the next readSensor()

  if ( model == AUTO_DETECT) {
    DHT::model = DHT22;
    readSensor();
    if ( error == ERROR_TIMEOUT ) {
      DHT::model = DHT11;
      delay(18); // ignore bits we might get
    }
  }
}

DHT::DHT_MODEL_t DHT::getModel()
{
  return model;
}

int DHT::getMinimumSamplingPeriod()
{
  return model == DHT11 ? 1001 : 2001;
}

float DHT::getHumidity()
{
  readSensor();
  return humidity;
}

float DHT::getTemperature()
{
  readSensor();
  return temperature;
}

DHT::DHT_ERROR_t DHT::getStatus()
{
  return error;
}

#ifndef OPTIMIZE_SRAM_SIZE

const char* DHT::getStatusString()
{
  switch ( error ) {
    case DHT::ERROR_TIMEOUT:
      return "TIMEOUT";

    case DHT::ERROR_CHECKSUM:
      return "CHECKSUM";

    default:
      return "OK";
  }
}

#else

// At the expense of 26 bytes of extra PROGMEM, we save 11 bytes of
// SRAM by using the following method:

prog_char P_OK[]       PROGMEM = "OK";
prog_char P_TIMEOUT[]  PROGMEM = "TIMEOUT";
prog_char P_CHECKSUM[] PROGMEM = "CHECKSUM";

const char *DHT::getStatusString() {
  prog_char *c;
  switch ( error ) {
    case DHT::ERROR_CHECKSUM:
      c = P_CHECKSUM; break;

    case DHT::ERROR_TIMEOUT:
      c = P_TIMEOUT; break;

    default:
      c = P_OK; break;
  }

  static char buffer[9];
  strcpy_P(buffer, c);

  return buffer;
}

#endif

void DHT::readSensor()
{
  // Make sure we don't poll the sensor too often
  // - Max sample rate DHT11 is 1 Hz   (duty cicle 1000 ms)
  // - Max sample rate DHT22 is 0.5 Hz (duty cicle 2000 ms)
  unsigned long startTime = millis();
  if ( (unsigned long)(startTime - lastReadTime) < (model == DHT11 ? 1000L : 2000L) ) {
    return;
  }
  lastReadTime = startTime;

  temperature = NAN;
  humidity = NAN;

  // Request sample

  digitalWrite(pin, LOW); // Send start signal
  pinMode(pin, OUTPUT);
  if ( model == DHT11 ) {
    delay(18);
  }
  else {
    // This will fail for a DHT11 - that's how we can detect such a device
    delayMicroseconds(800);
  }

  pinMode(pin, INPUT);
  digitalWrite(pin, HIGH); // Switch bus to receive data

  // We're going to read 83 edges:
  // - First a FALLING, RISING, and FALLING edge for the start bit
  // - Then 40 bits: RISING and then a FALLING edge per bit
  // To keep our code simple, we accept any HIGH or LOW reading if it's max 85 usecs long

  word rawHumidity;
  word rawTemperature;
  word data;

  for ( int8_t i = -3 ; i < 2 * 40; i++ ) {
    byte age;
    startTime = micros();

    do {
      age = (unsigned long)(micros() - startTime);
      if ( age > 90 ) {
        error = ERROR_TIMEOUT;
        return;
      }
    }
    while ( digitalRead(pin) == (i & 1) ? HIGH : LOW );

    if ( i >= 0 && (i & 1) ) {
      // Now we are being fed our 40 bits
      data <<= 1;

      // A zero max 30 usecs, a one at least 68 usecs.
      if ( age > 30 ) {
        data |= 1; // we got a one
      }
    }

    switch ( i ) {
      case 31:
        rawHumidity = data;
        break;
      case 63:
        rawTemperature = data;
        data = 0;
        break;
    }
  }

  // Verify checksum

  if ( (byte)(((byte)rawHumidity) + (rawHumidity >> 8) + ((byte)rawTemperature) + (rawTemperature >> 8)) != data ) {
    error = ERROR_CHECKSUM;
    return;
  }

  // Store readings

  if ( model == DHT11 ) {
    humidity = rawHumidity >> 8;
    temperature = rawTemperature >> 8;
  }
  else {
    humidity = rawHumidity * 0.1;

    if ( rawTemperature & 0x8000 ) {
      rawTemperature = -(int16_t)(rawTemperature & 0x7FFF);
    }
    temperature = ((int16_t)rawTemperature) * 0.1;
  }

  error = ERROR_NONE;
}

float DHT::toFahrenheit(float fromCelcius)
{
  return 1.8 * fromCelcius + 32.0;
}

float DHT::toCelsius(float fromFahrenheit)
{
  return 0.5555556 * (fromFahrenheit - 32.0);
}
<|endoftext|>