int main( )
{
  FPGroup G;
  cout << "Enter nilpotentcy class first then group ";
  int NilpotentcyClass;
  cin >> NilpotentcyClass;
  Chars errMsg = cin >> G;
  if (errMsg.length()>0) 
    return 1;
 
  NilpotentGroup ng(G.namesOfGenerators(),
		    NilpotentcyClass,makeVectorOf(G.getRelators()));
  ng.initialize();
  VectorOf<Word> vw;
  Word w;
  for (int i=1;true;i++){
    cout << endl << "Enter the "<<i<<" generator of subgroup"<< endl;
    cout << "Empty word to finish: ";
    w = G.readWord(cin,errMsg);
    if (w.length()==0)
      break;
    if (errMsg.length()>0) 
      return 1;
    vw.append(w);
  }
  SGOfNilpotentGroup sg(ng,vw);
  sg.initBasis();
  sg.printBasis(cout);
  cout << "The Hirsch number :" << sg.theHirschNumber() << endl;
  cout << "Index in parent group :" << sg.index() << endl;
  cout << "Is Trivial :" << sg.isTrivial() << endl;
  cout << "Is Central :" << sg.isCentral() << endl;
  cout << "Is normal :" << sg.isNormal() << endl;
  cout << "Is abelian :" << sg.isAbelian() << endl;
  cout << "Subgroup class :" << sg.subgroupClass() << endl;
  cout << "Generators of normal closure :" << endl;
  vw = sg.normalClosureGens();
  for (int i=0;i<vw.length();i++){
    G.printWord(cout,vw[i]);
    cout << endl;
  }
  PresentationForSNG sgp = sg.makePresentation();
  cout << "Presentation of subgroup :" << endl;
  sgp.print(cout);
  cout << endl << "Enter the word :";
  errMsg = "";
  w = G.readWord(cin,errMsg);
  if (errMsg.length()>0){ 
    cout << errMsg;
    return 1; 
  }
  cout << endl << "Does subgroup contain the word :" << sg.contains(w) << endl;
  PolyWord result;
  if (sg.decompose(ng.decompose(w),result))
    cout << "Decomposition in sg basis :" << sg.asDecomposition(result);
  else
    cout << "Subgroup does not contain this word";

}

// can be invoced only after the cyclic decomposition has been computed
bool AbelianEquationsSolver::root( Word& w , int n ) const
{
  AbelianGroup A1 = A.getCanonicalSmithPresentation();
  FPGroup G = A1.getFPGroup(); 
  
  AbelianWord nw = A.oldInAbelianForm( w );
  nw = A.oldToNewGens( nw );
  Word ans;
  
  VectorOf<Integer> powers = nw.getPowers();
  
  for( int i = 0 ; i < powers.length() ; i++ )
    {
      Generator g( i + 1 );
      
      int ord = A1.orderOfElt( g ).as_long();
      int pow = powers[i].as_long();
      
      if( pow )
	if( !ord )
	  {
	    if( pow % n )
	      return no;
	    else
	      for( int j = 0 ; j < abs(pow / n) ; j++ )
		ans *= ( (pow / n) > 0 ) ? g : inv(g);
	  }
	else
	  if( pow % n )
	    {
	      ord = ( ord > 0 ) ? ord : -ord;
	      
	      int x;
	      for( x = -ord ; x < ord ; x++ )
		if( ! ( (n * x - pow) % ord ) )
		  break;
	      
	      for( int j = 0 ; j < abs( x ) ; j++ )
		ans *= ( x > 0 ) ? g : inv(g);
	    }
	  else
	    for( int j = 0 ; j < abs(pow / n) ; j++ )
	      ans *= ( (pow / n) > 0 ) ? g : inv(g);
    }
  
  nw = AbelianWord( G.numberOfGenerators() , ans );
  nw = A.newToOldGens( nw );
  w = nw.getWord();
  
  return yes;
}

AbelianEquationsSolver::AbelianEquationsSolver( const AbelianGroup& a , 
						const VectorOf<Word>& v ,
						int numOfVar )
  : rawA( a ),
    A( FPGroup() ),
    rawSystem( v ),
    system( v.length() ),
    b( v.length() ),
    x( numOfVar ),
    torsion( numOfVar ),
    params( numOfVar ),
    numberOfVariables( numOfVar ),
    sysRank( 0 ),
    haveSol( -1 )
{
  FPGroup G( a.getFPGroup() );
  VectorOf<Chars> q( G.numberOfGenerators() - numberOfVariables );
  
  for( int i = numberOfVariables ; i < G.numberOfGenerators() ; i++ )
    q[ i - numberOfVariables ] = (G.namesOfGenerators())[i];
  
  if( G.getRelators().cardinality() )
    {
      SetOf<Word> s = G.getRelators();
      SetIterator<Word> I(s);
      SetOf<Word> news;
      
      while( !I.done() )
	{
	  Word w = I.value();
	  for( int j = 0 ; j < w.length() ; j++ )
	    {
	      int p = Generator( w[j] ).hash();
	      if( p > 0 )
		w[j] = Generator( p - numberOfVariables );
	      else
		w[j] = Generator( p + numberOfVariables );
	    }
	  
	  news.adjoinElement( w );
	  I.next();
	}
      
      FPGroup G1( q , news );
      
      A = AbelianGroup( G1 );
    }
  else
    A = AbelianGroup( FPGroup(q) );
  
}

VectorOf<Chars> PresentationParser::parseGeneratorList( Chars& errMesg )
{
  VectorOf<Chars> result;

  if ( curToken == INIT ) getToken();

  while ( curToken == GENERATOR ) {

	 Chars name(tokenName);

    // Check for duplication and presence of inverses.
	 if ( result.indexOf(name) >= 0 ) {
		parseError("Duplicate generator");
		errMesg = parseErrorMessage;
		return result;
	 } else {
		invertName(tokenName);
		if ( result.indexOf(Chars(tokenName)) >= 0 ) {
		  parseError("Duplicate generator: formal inverse");
		  errMesg = parseErrorMessage;
		  return result;
		}
	 }

	 result.append(name); // Ok, it's inefficient, but time is $$

	 getToken();

	 if ( curToken == COMMA ) {
	   getToken();
	   if ( curToken == DOT ) {
	     if (!getGeneratorRange(name,result,errMesg))
	       return result;
	   } else {
	     if ( curToken != GENERATOR ) {
	       parseError("Expected a generator here");
	       errMesg = parseErrorMessage;
	       return result;
	     }
	   }
	 }
  }

  return result;
}

Subgroup& Subgroup::deleteGenerator( const Word& w ) {
  VectorOf<Word> theGenerators = look()->theGenerators;
  int i = theGenerators.indexOf(w);
  if ( i == -1 )
	 error("attempt to delete nonexistent word in Subgroup::deleteGenerator");
  else {
	 int len = theGenerators.length();
	 VectorOf<Word> temp(len - 1);
	 int k = 0;
	 for( int j = 0; j < len; ++j )
		if ( j != i ) temp[k++] = theGenerators[j];
	 change()->theGenerators = temp;
	 
	 enhance()->theOrder = -1;
	 // @stc not yet correct: the order does not necessarily become unknown
	 return *this;
  }
}

bool imuIdentifierThread::goHome()
{
    setHeadCtrlModes("position");
    // Vector pos0(6,0.0);
    // iposH -> positionMove(pos0.data());

    VectorOf<int> jointsToSet;
    jointsToSet.push_back(0);
    jointsToSet.push_back(1);
    jointsToSet.push_back(2);
    Vector poss(3,0.0);

    iposH -> positionMove(jointsToSet.size(),
                          jointsToSet.getFirst(),
                          poss.data());

    return true;
}

    void testToString()
    {
        {
            report(0, "testing toString int");
            bool ok = true;
            VectorOf<int> vec;
            std::string strToCheck = "0 1 2 3 4 5 6 7 8 9";
            for (size_t i=0; i<10; i++)
            {
                vec.push_back(i);
            }

            ok = vec.toString() == strToCheck;
            checkTrue(ok, "string correctly formatted");
        }

        {
            report(0, "testing toString double");
            bool ok = true;
            VectorOf<double> vec;
            std::string strToCheck = " 0.000000\t 1.000000\t 2.000000\t 3.000000\t 4.000000\t"
                                     " 5.000000\t 6.000000\t 7.000000\t 8.000000\t 9.000000";
            for (size_t i=0; i<10; i++)
            {
                vec.push_back(i);
            }

            ok = vec.toString() == strToCheck;
            checkTrue(ok, "string correctly formatted");
        }

    }

void Controller::setJointsCtrlMode()
{
    if (jointsToSet.size()==0)
        return;

    VectorOf<int> modes;
    for (size_t i=0; i<jointsToSet.size(); i++)
    {
        if (jointsToSet[i]<eyesJoints[0])
        {
            if (neckPosCtrlOn)
                modes.push_back(VOCAB_CM_POSITION_DIRECT);
            else
                modes.push_back(VOCAB_CM_VELOCITY);
        }
        else
            modes.push_back(VOCAB_CM_MIXED);
    }

    modHead->setControlModes(jointsToSet.size(),jointsToSet.getFirst(),
                             modes.getFirst());
}

void AbelianSGPresentationRep::makeFile( )
{
  File file;
  VectorOf<AbelianWord> cyclicGens = newToOldGens();
  int numofnewgens = cyclicGens.length();
  int rank = rankOfFreeAbelianFactor();
  char s[10];
  Word tmpWord;
  file.setColor(titleColor);
  file << "New generators of the subgroup expressed in terms of the given generators:" << endl;  
  file.setColor(mainColor);
  for( int i = 0; i < rank; ++i ) {
    file << "f" << i+1 << " = ";
    tmpWord =  fromSGPGensToSGGens(cyclicGens[i].getWord());
    theParent.getFPGroup().printWord(file,theParent.oldInAbelianForm(tmpWord).getWord() );
    file  << endl;
  }
  for( int i = rank; i < numofnewgens; ++i ) {
    file << "t" << i - rank + 1 << " = ";
    tmpWord =  fromSGPGensToSGGens(cyclicGens[i].getWord());
    theParent.getFPGroup().printWord(file,theParent.oldInAbelianForm(tmpWord).getWord() );
    file << endl;
  }
  file << endl;
  file.setColor(titleColor);
  file << "Given generators expressed in terms of the generators for the canonical decomposition of the subgroup:" << endl;  
  file.setColor(mainColor);
  for( int i = 0; i < theSGGens.length(); ++i ) {
    theParent.getFPGroup().printWord( file,theSGGens[i] );
    file << " = ";
    if (fromSGGensToSGPGens(theSGGens[i],tmpWord))
      printWordInNewGens(file,oldToNewGens(oldInAbelianForm(tmpWord)));   
    file << endl;
  } 
  file << end;
  theFileName = file.getFileName();
}

VectorOf<Word> SGOfFreeNilpotentGroupRep::normalClosureBasis() const {

  //@ep I have found that the process is slower with the full basis,
  // so I form it from the scratch.

  MalcevSet newBasis(theGenerators, theParentGroup.collector());
  newBasis.makeNormalClosure();

  // The basis is found. Now we try to reduce it to make further
  // processing easier.

  VectorOf<Word> basis = newBasis.getCommutatorWords();

  // First, we reduce words in terms of basic commutators.
  // We could proceed without this step, but it helps
  // to reduce the second one greatly.
  
  FreeGroup F( theParentGroup.theHirschNumber() );  
  basis = F.nielsenBasis(basis);
  
  // Convert words in terms of basic commutators to group
  // generators.

  for(int i = 0; i < basis.length(); i++) {
    PolyWord pw = basis.val(i);
    basis[i] = theParentGroup.commutators().wordForm().toWord(pw);
  }

  // Now reduce this vector

  FreeGroup F1( theParentGroup.numberOfGenerators() );
  basis = F1.nielsenBasis(basis);
  

  return basis;
}

bool reactCtrlThread::areJointsHealthyAndSet(VectorOf<int> &jointsToSet,
                                             const string &_p, const string &_s)
{
    VectorOf<int> modes;
    if (_p=="arm")
    {
        modes=encsA->size();
        imodA->getControlModes(modes.getFirst());
    }
    else if (_p=="torso")
    {
        modes=encsT->size();
        imodT->getControlModes(modes.getFirst());
    }
    else
        return false;

    for (size_t i=0; i<modes.size(); i++)
    {
        if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
            return false;

        if (_s=="velocity")
        {
            if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
                jointsToSet.push_back(i);
        }
        else if (_s=="position")
        {
            if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
                jointsToSet.push_back(i);
        }

    }

    return true;
}

// put the words to the set
MalcevSet::MalcevSet(const VectorOf<Word>& v, const NGCollector& nc)
  : isBasis(false), isNormal(dontknow), theCollector(nc)
{
  for(int i = 0; i < v.length(); i++)
    addWord( v.val(i) );
}

//@njz:default value removed. defined in .h file
//void AbelianEquationsSolver::findSolutions( File& file , bool out = true )
void AbelianEquationsSolver::findSolutions( File& file , bool out)
//
{
  haveSol = 0;
  
  if( !A.haveCyclicDecomposition() )
    A.computeCyclicDecomposition();
  
  if( !A.havePrimaryDecomposition() )
    A.findPrimaryBasis();
  
  if( !rawA.haveCyclicDecomposition() )
    rawA.computeCyclicDecomposition();
  
  if( !rawA.havePrimaryDecomposition() )
    rawA.findPrimaryBasis();
  
  makeSystem();

  if( out )
   {
     if( system.length() > 1 )
       file << "The system of equations: " << endl << endl;
     else
       file << "The equation: " << endl << endl;
     
     printRawSystem( file );

     file << endl << "can be transformed to the one: " << endl << endl;

     printSystem( file );
  
     if( system.length() > 1 )
       file << endl << "Finding solutions of this system ..." << endl << endl;
     else
       file << endl << "Finding solutions of this equation ..." << endl << endl;
   }

  //@njz
  //  int **matrix = new (int*)[ system.length() ];
  int **matrix = new int*[ system.length() ];
  //
  
  int i,j,k,i1,j1;
  
  for( i = 0 ; i < system.length() ; i++ )
   {
     matrix[i] = new int[ numberOfVariables ];
     for( j = 0 ; j < numberOfVariables ; j++ )
       {
	 Integer u = (AbelianWord( numberOfVariables , system[i] ))[j];
	 matrix[i][j] = u.as_long();
       }
   }

  VectorOf< VectorOf<int> > transform;
  VectorOf<int> trans(3);

  // diagonalization of the matrix
  
  sysRank = 0;
  
  for( i = 0 ; ( i < numberOfVariables && i < system.length() ) ; i++ )
    {
      bool flag = false;
      
      for( j = i ; j < system.length() ; j++ )
	{
	  for( k = i ; k < numberOfVariables ; k++ )
	    if( matrix[j][k] )
	      {
		flag = true;
		break;
	      }
	  
	  if( flag ) break;
	}
      
      if( k == numberOfVariables && j == system.length() )
	{    
	  if( sysRank )
	    break;
	  
	  int q;
	  for( q = 0 ; q < b.length() ; q++ )
	    if( !A.isTrivial( Word( b[q] ) ) )
	      {
		haveSol = -1;
		
		if( out )
		  if( system.length() > 1 )
		    file << "while computing the canonical form of the system it was found that this system has no solutions." << endl;
		  else
		    file << "while computing the canonical form of the equation it was found that this equation has no solutions." << endl;
		
		return;
	      }
	  
//.........这里部分代码省略.........

    void checkSendReceiveInt()
    {
        report(0, "check VectorO<int> send receive");

        {
            Port portIn;
            Port portOut;

            portOut.open("/harness_sig/vtest/o");
            portIn.open("/harness_sig/vtest/i");

            Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");

            portOut.enableBackgroundWrite(true);


            VectorOf<int> vector;
            vector.resize(10);
            for (unsigned int k = 0; k < vector.size(); k++)
            {
                vector[k] = k;
            }

            bool success = true;
            portOut.write(vector);

            VectorOf<int> tmp;
            portIn.read(tmp);

            //compare vector and tmp
            if (tmp.size() != vector.size())
            {
                success = false;
            }
            else
            {
                for (unsigned int k = 0; k < vector.size(); k++)
                {
                    if (tmp[k] != vector[k])
                        success = false;
                }
            }

            checkTrue(success, "VectorOf<int> was sent and received correctly");
            portOut.interrupt();
            portOut.close();
            portIn.interrupt();
            portIn.close();
        }

        report(0, "check VectorOf<int> bottle compatibility");
        {
            //write the same vector again and receive it as a bottle
            Port portIn;
            Port portOut;
            bool success = true;

            portOut.open("/harness_sig/vtest/o");
            portIn.open("/harness_sig/vtest/i");

            Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");

            portOut.enableBackgroundWrite(true);


            VectorOf<int> vector;
            vector.resize(10);
            for (unsigned int k = 0; k < vector.size(); k++)
            {
                vector[k] = k;
            }
            portOut.write(vector);
            Bottle tmp2;
            success = portIn.read(tmp2);
            checkTrue(success,"correctly read from the port");

            //compare vector and tmp
            success = true;
            if (tmp2.size() != vector.size())
            {
                success = false;
            }
            else
            {
                for (unsigned int k = 0; k < vector.size(); k++)
                {
                    if (tmp2.get(k).asInt32() != vector[k])
                        success = false;
                }
            }

            checkTrue(success, "VectorOf<int> was received correctly in a Bottle");
            portOut.interrupt();
            portOut.close();
            portIn.interrupt();
            portIn.close();
        }
    }

main()
{
  FPGroup G;
  VectorOf<int> order;
  cin >> G;
  char ch;
  cin >> ch;
  if (ch != '[') { cerr << "Unexpected input, aborted"<<endl; exit(1);}
  do {
    int i;
    cin >> i;
    order.append(i);
    cin>> ch;
    if (ch==']') break;
    else if (ch!=','){ cerr << "Unexpected input, aborted"<<endl; exit(1);}
  } while (ch==',');
  WordOrder word_order("ShortLex",order);
  KBmagPackage kbmag(G.namesOfGenerators(),G.getRelators(),word_order,20);
  if ( !kbmag.sanityCheck() ) {
    error("kbmag failed sanity check.\n");
  }
  if (kbmag.autgroup()==yes){
    cout << "Group "<< G << " is proved shortlex automatic"<<endl;
    GroupDFSA WA = kbmag.wordAcceptor();
    WA.setName("Word_acceptor");
    WA.printOn();
    GenMult GM = kbmag.generalMultiplier();
    GM.setName("General_multiplier");
    GM.printOn();
    DiffMachine D1 = kbmag.differenceMachine(1);
    D1.setName("1stDiffMachine");
    D1.printOn();
    DiffMachine D2 = kbmag.differenceMachine(2);
    D2.setName("2ndDiffMachine");
    D2.printOn();
  }
  else 
    cout << "Group "<< G << " is not proved shortlex automatic"<<endl;
  cout << endl;
  
  FPGroup G2;
  cin >> G2;
  KBmagPackage kbmag2(G2.namesOfGenerators(),G2.getRelators());
  if ( !kbmag2.sanityCheck() ) {
    error("kbmag2 failed sanity check.\n");
  }
  if (kbmag2.kbprog()==yes && kbmag2.gpmakefsa()==yes && kbmag2.gpaxioms()==yes){
    cout << "Group "<< G2 << " is proved shortlex automatic"<<endl;
    GroupDFSA WA = kbmag2.wordAcceptor();
    WA.setName("Word_acceptor");
    WA.printOn();
    GenMult GM = kbmag2.generalMultiplier();
    GM.setName("General_multiplier");
    GM.printOn();
    DiffMachine D1 = kbmag2.differenceMachine(1);
    D1.setName("1stDiffMachine");
    D1.printOn();
    DiffMachine D2 = kbmag2.differenceMachine(2);
    D2.setName("2ndDiffMachine");
    D2.printOn();
  }
  else 
    cout << "Group "<< G2 << " is not proved shortlex automatic"<<endl;
  cout << endl;
  FPGroup G3;
  cin >> G3;
  KBmagPackage kbmag3(G3.namesOfGenerators(),G3.getRelators());
  if ( !kbmag3.sanityCheck() ) {
    error("kbmag3 failed sanity check.\n");
  }
  if (kbmag3.kbprog()>0){
    Bool abort=NO;
    int loop=0;
    do {
      loop++;
      cout << "Trying to built automata."<< endl;
      if (loop>1) cout << "Pass no. " << loop <<" though loop."<< endl; 
      if (kbmag3.gpwa()!=yes || kbmag3.gpgenmult()!=yes) abort=YES; 
      if (abort){ cout << "Failed, giving up!"<< endl; break;}
      GroupDFSA WA = kbmag3.wordAcceptor();
      WA.setName("Word_acceptor");
      WA.printOn();
      GenMult GM = kbmag3.generalMultiplier();
      GM.setName("GeneralMultiplier");
      GM.printOn();
      DiffMachine D1 = kbmag3.differenceMachine(1);
      D1.setName("1stDiffMachine");
      D1.printOn();
      DiffMachineRep D2 = kbmag3.differenceMachineRep(2);
      D2.setName("2ndDiffMachine");
      D2.printOn();
    } while (kbmag3.gpcheckmult()==no);
    if (abort==NO && kbmag3.gpaxioms()==yes)
      cout << "Group "<< G3 << " is proved shortlex automatic"<<endl;
    else 
      cout << "Group "<< G3 << " is not proved shortlex automatic"<<endl;
   }
   GroupDFSARep WA2 = kbmag3.wordAcceptorRep();
   DiffMachineRep D3 = kbmag3.differenceMachineRep(2);
   KBmagPackage kbmag4(G3.namesOfGenerators(),G3.getRelators());
//.........这里部分代码省略.........

bool imuIdentifierThread::setHeadCtrlModes(const string &_s)
{
    printMessage(1,"Setting %s mode for head joints..\n",_s.c_str());

    if (_s!="position" && _s!="velocity")
        return false;

    VectorOf<int> jointsToSet;
    jointsToSet.push_back(0);
    jointsToSet.push_back(1);
    jointsToSet.push_back(2);
    VectorOf<int> modes;

    if (_s=="position")
    {
        modes.push_back(VOCAB_CM_POSITION);
        modes.push_back(VOCAB_CM_POSITION);
        modes.push_back(VOCAB_CM_POSITION);
    }
    else if (_s=="velocity")
    {
        modes.push_back(VOCAB_CM_VELOCITY);
        modes.push_back(VOCAB_CM_VELOCITY);
        modes.push_back(VOCAB_CM_VELOCITY);
    }

    imodH -> setControlModes(jointsToSet.size(),
                             jointsToSet.getFirst(),
                             modes.getFirst());

    Time::delay(0.1);

    return true;
}

bool reactCtrlThread::setCtrlModes(const VectorOf<int> &jointsToSet,
                                   const string &_p, const string &_s)
{
    if (_s!="position" && _s!="velocity")
        return false;

    if (jointsToSet.size()==0)
        return true;

    VectorOf<int> modes;
    for (size_t i=0; i<jointsToSet.size(); i++)
    {
        if (_s=="position")
        {
            modes.push_back(VOCAB_CM_POSITION);
        }
        else if (_s=="velocity")
        {
            modes.push_back(VOCAB_CM_VELOCITY);
        }
    }

    if (_p=="arm")
    {
        imodA->setControlModes(jointsToSet.size(),
                               jointsToSet.getFirst(),
                               modes.getFirst());
    }
    else if (_p=="torso")
    {
        imodT->setControlModes(jointsToSet.size(),
                               jointsToSet.getFirst(),
                               modes.getFirst());
    }
    else
        return false;

    return true;
}

bool PresentationParser::getGeneratorRange( const Chars& name,
					    VectorOf<Chars>& result,
					    Chars& errMesg )
{
  if ( curToken != DOT ) 
    error("bool PresentationParser::getGeneratorRange() "
	  "can't read the range");
  // Make sure, that we have 3 dots in a row
  for (int i = 0;i<2;i++){
    getToken();
    if ( curToken != DOT ) {
      parseError("Expected '.' here");
      errMesg = parseErrorMessage;
      return false;
    }   
  }
  getToken();
  
   // Supose to be ','
  if ( curToken != COMMA ) {
    parseError("Expected ',' here");
    errMesg = parseErrorMessage;
    return false;
  }    

  getToken();
  
  // Supose to be a generator
  if ( curToken != GENERATOR ) {
    parseError("Expected a generator here");
    errMesg = parseErrorMessage;
    return false;
  }    
  Chars name1;
  int beginRange;
  
  // Generators must be in the form: <name><index>, where
  // index is the first and the last generator index in a range.
  // 
  Chars rangeErrMsg = "When defining a set of generators using a range of "
    "subscripts, both the smallest subscript and the largest subscript"
    " must be given and the smallest subscript mast be less then the "
    "largest one";

  if ( !getRangeOf(name,name1,beginRange)) {
    parseError(rangeErrMsg);
    errMesg = parseErrorMessage;
    return false;    
  }
  Chars name2;
  int endRange;  
  if ( !getRangeOf(tokenName,name2,endRange)) {
    parseError(rangeErrMsg);
    errMesg = parseErrorMessage;
    return false;    
  }

  // The last index must be greater than the first one
  if ( endRange <= beginRange) {
    parseError(rangeErrMsg);
    errMesg = parseErrorMessage;
    return false;    
  }
 
  // Names gave to be equal
  if ( name2 != name1) {
    parseError(rangeErrMsg);
    errMesg = parseErrorMessage;
    return false;    
  }

  // Generate generators of type: name1<index>, where
  // beginRange < index < endRange
  int numberOfGens = endRange - beginRange - 1;
  if (numberOfGens > 0){
    VectorOf<Chars> gens(numberOfGens);
    
    for(int i = 0;i<numberOfGens;i++){
      gens[i] = name1+Chars(beginRange + 1 + i);
      // Check for duplication and presence of inverses.
      if ( result.indexOf(gens[i]) >= 0 ) {
	parseError("Duplicate generator");
	errMesg = parseErrorMessage;
	return false;
      } else {
	char str[100];
	strcpy(str,gens[i]);
	invertName(str);
	if ( result.indexOf(Chars(str)) >= 0 ) {
	  parseError("Duplicate generator: formal inverse");
	  errMesg = parseErrorMessage;
	  return false;
	}
      }
    }
    result = concatenate(result, gens);
  }
  return true;
}

bool imuIdentifierThread::processWayPoint()
{
    processIMU();
    
    if (wayPoints[currentWaypoint].name == "START     " ||
        wayPoints[currentWaypoint].name == "END       " ||
        wayPoints[currentWaypoint].name == "MIDDLE    " )
    {
        if (posCtrlFlag)
        {
            printMessage(1,"Putting head in home position..\n");
            goHome();
            posCtrlFlag = false;
        }

        if (yarp::os::Time::now() - timeNow > CTRL_PERIOD)
        {
            posCtrlFlag = true;
            return false;
        }
    }
    else
    {
        Vector jls = wayPoints[currentWaypoint].jntlims;
        Vector vls = wayPoints[currentWaypoint].vels;
        bool flag = false;

        iencsH->getEncoders(encsH->data());
        yarp::sig::Vector head = *encsH;

        // ivelH -> velocityMove(vls.data());

        VectorOf<int> jointsToSet;
        jointsToSet.push_back(0);
        jointsToSet.push_back(1);
        jointsToSet.push_back(2);
    
        ivelH -> velocityMove(jointsToSet.size(),
                              jointsToSet.getFirst(),
                              vls.data());

        for (int i = 0; i < 3; i++)
        {
            if      (vls(i) > 0.0)
            {
                if (jls(i) - head(i) > 0.0)
                {
                    flag = true;
                }
            }
            else if (vls(i) < 0.0)
            {
                if (jls(i) - head(i) < 0.0)
                {
                    flag = true;
                }
            }
        }

        return flag;
    }

    return true;
}

int main ( )
{
  Chars errMsg;
  GHNConfig c;
  //cout << c << endl;

  ifstream in("../wp.in");
  in >> c;
  cout << c << endl;

  int numOfVars;
  cout << "Enter the number of variables: ";
  cin >> numOfVars;
  cout << endl;
    
  FreeGroup G;
  cout << "Enter a free group: ";
  errMsg = cin >> G;
  if( errMsg.length() > 0 ) {
    cout << errMsg;
    exit(0);
  }
  cout << endl;
  
  VectorOf<Chars> v = G.namesOfGenerators();
  int numOfConst = v.length();
  int rLen = v.length() + numOfVars;
  VectorOf<Chars> r(rLen);
  int j;
  for( j = 0; j < v.length(); ++j )
    if( v[j][0] != 'x' && v[j][0] != 'X' )
      r[j] = v[j];
    else
      error("x is reserved for variables\n");

  char s[3] = {0,0,0};
  s[0] = 'x';
  
  for( int i = j; i < rLen; ++i ) {
    s[1] = i-j+1+48;
    r[i] = s;
  }

  FreeGroup F(r);
  cout << "Enter an equation (a word) with variables x1 ... x" 
       << numOfVars << ": ";
  Word w;
 
  w = F.readWord(cin,errMsg);
  if( errMsg.length() > 0 ) {
    cout << errMsg;
    exit(0);
  }
  cout << endl;

  VectorOf<Word> im(rLen);
  for( int i = 0; i < rLen; ++i )
    im[i] = Word(Generator(i+1));
  Map M(F,F,im);

  int popSize = c.populationSize();
  GAWord pop[numOfVars][popSize],newPop[numOfVars][popSize];
  
  for( int k = 0; k < numOfVars; ++k )
    for( int i = 0; i < popSize; ++i ) {
      pop[k][i] = GAWord(numOfConst,Word());
    }

  int fit[popSize];

  // the main loop

  int numOfGens = c.numOfGenerations();
  //bool bHaveFitnessScaling = c.haveFitnessScaling();
  float crossRate = c.chanceOfCrossover();
  float mutRate = c.chanceOfMutation();
  UniformRandom devgen;
  int max, min, minInd, g;
 
  // create the original random populations
  for( int k = 0; k < numOfVars; ++k )
    for( int i = 0; i < popSize; ++i ) {
      pop[k][i] = pop[k][i].randomWord();
    }
  
  for( g = 0; g < numOfGens || numOfGens == -1; ++g ) {
    
    min = MAXINT; max = 0;  minInd = -1;
    
    // compute fitness values

    for( int i = 0; i < popSize; ++i ) {

      for( int k = 0; k < numOfVars; ++k )
	M.setGeneratingImages(numOfConst+k,(pop[k][i]).getWord());
      fit[i] = M.imageOf(w).freelyReduce().length();
      
      if( fit[i] < min ) {
	min = fit[i];
	minInd = i;
//.........这里部分代码省略.........