worldfile.cc

Go to the documentation of this file.

/*
 *  Stage : a multi-robot simulator.
 *  Copyright (C) 2001-2011 Richard Vaughan, Andrew Howard and Brian Gerkey.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 * Desc: A class for reading in the world file.
 * Authors: Andrew Howard <ahoward@usc.edu>
 *          Richard Vaughan (rtv) <vaughan@sfu.ca>
 *          Douglas S. Blank <dblank@brynmawr.edu>
 *
 * Date: 15 Nov 2001
 * CVS info: $Id$
 */

#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <limits.h> // for PATH_MAX
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <stdarg.h>

//#define DEBUG

#include "replace.h" // for dirname(3)
#include "stage.hh"
#include "worldfile.hh"
using namespace Stg;

// the isblank() macro is not standard - it's a GNU extension
// and it doesn't work for me, so here's an implementation - rtv
#ifndef isblank
#define isblank(a) (a == ' ' || a == '\t')
#endif

// Useful macros for dumping parser errors
#define TOKEN_ERR(z, l)                         \
  PRINT_ERR2("%s:%d : " z, this->filename.c_str(), l)
#define PARSE_ERR(z, l)                         \
  PRINT_ERR2("%s:%d : " z, this->filename.c_str(), l)


// Default constructor
Worldfile::Worldfile() :
  tokens(),
  macros(),
  entities(),
        properties(),
  filename(),
  unit_length( 1.0 ),
  unit_angle( M_PI / 180.0 )
{
}


// Destructor
Worldfile::~Worldfile()
{
  ClearProperties();
  ClearMacros();
  ClearEntities();
  ClearTokens();
}

FILE *Worldfile::FileOpen(const std::string& filename, const char* method)
{
  FILE *fp = fopen(filename.c_str(), method);
  // if this opens, then we will go with it:
  if (fp) {
    PRINT_DEBUG1( "Loading: %s", filename.c_str());
    return fp;
  }
  // else, search other places, and set this->filename
  // accordingly if found:
  char *stagepath = getenv("STAGEPATH");
  char *token = strtok(stagepath, ":");
  char* fullpath = new char[PATH_MAX];
  char *tmp = strdup(filename.c_str());
  const char *base = basename(tmp);
  while (token != NULL) {
    // for each part of the path, try it:
    memset( fullpath, 0, PATH_MAX);
    strcat( fullpath, token);
    strcat( fullpath, "/" );
    strcat( fullpath, base);
    assert(strlen(fullpath) + 1 < PATH_MAX);
    fp = fopen(fullpath, method);
    if (fp) {
      this->filename = std::string(fullpath);
      PRINT_DEBUG1( "Loading: %s", filename.c_str());
      free(tmp);
      return fp;
    }
    token = strtok(NULL, ":");
  }
  if( tmp ) free(tmp);
  if( fullpath ) delete[] fullpath;
  return NULL;
}


// Load world from file
bool Worldfile::Load(const std::string& filename )
{
  this->filename = filename;

  // Open the file
  //FILE *file = fopen(this->filename, "r");
  FILE *file = FileOpen(this->filename.c_str(), "r");
  if (!file)
    {
      PRINT_ERR2("unable to open world file %s : %s",
                                          this->filename.c_str(), strerror(errno));
      return false;
    }

  ClearTokens();

  // Read tokens from the file
  if (!LoadTokens(file, 0))
    {
      //DumpTokens();
      fclose(file);
      return false;
    }

  fclose(file);

  // Parse the tokens to identify entities
  if (!ParseTokens())
    {
      //DumpTokens();
      return false;
    }

  // Dump contents and exit if this file is meant for debugging only.
  if (ReadInt(0, "test", 0) != 0)
    {
      PRINT_ERR("this is a test file; quitting");
      DumpTokens();
      DumpMacros();
      DumpEntities();
      DumpProperties();
      return false;
    }
  
  // Work out what the length units are
  const std::string& unitl = ReadString(0, "unit_length", "m");
  if(  unitl == "m")
    this->unit_length = 1.0;
  else if( unitl == "cm")
    this->unit_length = 0.01;
  else if( unitl == "mm") 
    this->unit_length = 0.001;
  
  // Work out what the angle units are
  const std::string& unita = ReadString(0, "unit_angle", "degrees");
  if(  unita == "degrees")
    this->unit_angle = M_PI / 180;
  else if( unita == "radians")
    this->unit_angle = 1;
  
  //printf( "f: %s\n", this->filename.c_str() );

  return true;
}


// Save world to file
bool Worldfile::Save(const std::string& filename )
{
  // Debugging
  //DumpProperties();

  // Open file
  FILE *file = fopen(filename.c_str(), "w+");
  //FILE *file = FileOpen(filename, "w+");
  if (!file)
    {
      PRINT_ERR2("unable to open world file %s : %s",
                                                                 filename.c_str(), strerror(errno));
      return false;
    }
        
  // TODO - make a backup before saving the file
        
  // Write the current set of tokens to the file
  if (!SaveTokens(file))
    {
      fclose(file);
      return false;
    }
        
  fclose(file);
  return true;
}


// Check for unused properties and print warnings
bool Worldfile::WarnUnused()
{
  bool unused = false;
  
  FOR_EACH( it, properties )
    {
      if( ! it->second->used )
        {
          PRINT_WARN3("worldfile %s:%d : property [%s] is defined but not used",
                      this->filename.c_str(), it->second->line, it->second->name.c_str());
          unused = true;
        }
    }
  
  return unused;
}


// Load tokens from a file.
bool Worldfile::LoadTokens(FILE *file, int include)
{
  int ch;
  int line;
  char token[256];

  line = 1;

  while (true)
    {
      ch = fgetc(file);
      if (ch == EOF)
        break;

      if ((char) ch == '#')
        {
          ungetc(ch, file);
          if (!LoadTokenComment(file, &line, include))
            return false;
        }
      else if (isalpha(ch))
        {
          ungetc(ch, file);
          if (!LoadTokenWord(file, &line, include))
            return false;
        }
      else if (strchr("+-.0123456789", ch))
        {
          ungetc(ch, file);
          if (!LoadTokenNum(file, &line, include))
            return false;
        }
      else if (isblank(ch))
        {
          ungetc(ch, file);
          if (!LoadTokenSpace(file, &line, include))
            return false;
        }
      else if (ch == '"')
        {
          ungetc(ch, file);
          if (!LoadTokenString(file, &line, include))
            return false;
        }
      else if (strchr("(", ch))
        {
          token[0] = ch;
          token[1] = 0;
          AddToken(TokenOpenEntity, token, include);
        }
      else if (strchr(")", ch))
        {
          token[0] = ch;
          token[1] = 0;
          AddToken(TokenCloseEntity, token, include);
        }
      else if (strchr("[", ch))
        {
          token[0] = ch;
          token[1] = 0;
          AddToken(TokenOpenTuple, token, include);
        }
      else if (strchr("]", ch))
        {
          token[0] = ch;
          token[1] = 0;
          AddToken(TokenCloseTuple, token, include);
        }
      else if ( 0x0d == ch )
        {
          ch = fgetc(file);
          if ( 0x0a != ch )
            ungetc(ch, file);
          line++;
          AddToken(TokenEOL, "\n", include);
        }
      else if ( 0x0a == ch )
        {
          ch = fgetc(file);
          if ( 0x0d != ch )
            ungetc(ch, file);
          line++;
          AddToken(TokenEOL, "\n", include);
        }
      else
        {
          TOKEN_ERR("syntax error", line);
          return false;
        }
    }

  return true;
}


// Read in a comment token
bool Worldfile::LoadTokenComment(FILE *file, int *line, int include)
{
  char token[256];
  int len;
  int ch;

  len = 0;
  memset(token, 0, sizeof(token));

  while (true)
    {
      ch = fgetc(file);

      if (ch == EOF)
        {
          AddToken(TokenComment, token, include);
          return true;
        }
      else if ( 0x0a == ch || 0x0d == ch )
        {
          ungetc(ch, file);
          AddToken(TokenComment, token, include);
          return true;
        }
      else
        token[len++] = ch;
    }
  return true;
}


// Read in a word token
bool Worldfile::LoadTokenWord(FILE *file, int *line, int include)
{
  char token[256];
  int len;
  int ch;

  len = 0;
  memset(token, 0, sizeof(token));

  while (true)
    {
      ch = fgetc(file);

      if (ch == EOF)
        {
          AddToken(TokenWord, token, include);
          return true;
        }
      else if (isalpha(ch) || isdigit(ch) || strchr(".-_[]", ch))
        {
          token[len++] = ch;
        }
      else
        {
          if (strcmp(token, "include") == 0)
            {
              ungetc(ch, file);
              AddToken(TokenWord, token, include);
              if (!LoadTokenInclude(file, line, include))
                return false;
            }
          else
            {
              ungetc(ch, file);
              AddToken(TokenWord, token, include);
            }
          return true;
        }
    }
  assert(false);
  return false;
}


// Load an include token; this will load the include file.
bool Worldfile::LoadTokenInclude(FILE *file, int *line, int include)
{
  int ch;
  const char *filename;
  char *fullpath;

  ch = fgetc(file);

  if (ch == EOF)
    {
      TOKEN_ERR("incomplete include statement", *line);
      return false;
    }
  else if (!isblank(ch))
    {
      TOKEN_ERR("syntax error in include statement", *line);
      return false;
    }

  ungetc(ch, file);
  if (!LoadTokenSpace(file, line, include))
    return false;

  ch = fgetc(file);

  if (ch == EOF)
    {
      TOKEN_ERR("incomplete include statement", *line);
      return false;
    }
  else if (ch != '"')
    {
      TOKEN_ERR("syntax error in include statement", *line);
      return false;
    }

  ungetc(ch, file);
  if (!LoadTokenString(file, line, include))
    return false;

  // This is the basic filename
  filename = GetTokenValue(this->tokens.size() - 1);

  // Now do some manipulation.  If its a relative path,
  // we append the path of the world file.
  if (filename[0] == '/' || filename[0] == '~')
    {
      fullpath = strdup(filename);
    }
  else if (this->filename[0] == '/' || this->filename[0] == '~')
    {
      // Note that dirname() modifies the contents, so
      // we need to make a copy of the filename.
      // There's no bounds-checking, but what the heck.
      char *tmp = strdup(this->filename.c_str());
      fullpath = new char[PATH_MAX];
      memset(fullpath, 0, PATH_MAX);
      strcat( fullpath, dirname(tmp));
      strcat( fullpath, "/" );
      strcat( fullpath, filename );
      assert(strlen(fullpath) + 1 < PATH_MAX);
      free(tmp);
    }
  else
    {
      // Note that dirname() modifies the contents, so
      // we need to make a copy of the filename.
      // There's no bounds-checking, but what the heck.
      char *tmp = strdup(this->filename.c_str());
      fullpath = new char[PATH_MAX];
      char* dummy = getcwd(fullpath, PATH_MAX);
      if (!dummy)
                  {
                         PRINT_ERR2("unable to get cwd %d: %s", errno, strerror(errno));
                         if(tmp) free(tmp);
                         delete[] fullpath;
                         return false;
      }
      strcat( fullpath, "/" );
      strcat( fullpath, dirname(tmp));
      strcat( fullpath, "/" );
      strcat( fullpath, filename );
      assert(strlen(fullpath) + 1 < PATH_MAX);
      free(tmp);
    }

  printf( "[Include %s]", filename );
  fflush( stdout );

  // Open the include file
  FILE *infile = FileOpen(fullpath, "r");
  if (!infile)
    {
      PRINT_ERR2("unable to open include file %s : %s",
                 fullpath, strerror(errno));
      //free(fullpath);
                delete[] fullpath;
      return false;
    }

  // Terminate the include line
  AddToken(TokenEOL, "\n", include);

  //DumpTokens();

  // Read tokens from the file
  if (!LoadTokens(infile, include + 1))
    {
                fclose( infile );
      //DumpTokens();
      //free(fullpath);
                delete[] fullpath;
      return false;
    }

  // done with the include file
  fclose( infile );

  // consume the rest of the include line XX a bit of a hack - assumes
  // that an include is the last thing on a line
  while ( ch != '\n' )
    ch = fgetc(file);

  delete[] fullpath;
  return true;
}


// Read in a number token
bool Worldfile::LoadTokenNum(FILE *file, int *line, int include)
{
  char token[256];
  int len;
  int ch;

  len = 0;
  memset(token, 0, sizeof(token));

  while (true)
    {
      ch = fgetc(file);

      if (ch == EOF)
        {
          AddToken(TokenNum, token, include);
          return true;
        }
      else if (strchr("+-.0123456789", ch))
        {
          token[len++] = ch;
        }
      else
        {
          AddToken(TokenNum, token, include);
          ungetc(ch, file);
          return true;
        }
    }
  assert(false);
  return false;
}


// Read in a string token
bool Worldfile::LoadTokenString(FILE *file, int *line, int include)
{
  int ch;
  int len;
  char token[256];

  len = 0;
  memset(token, 0, sizeof(token));

  ch = fgetc(file);

  while (true)
    {
      ch = fgetc(file);

      if ( EOF == ch || 0x0a == ch || 0x0d == ch )
        {
          TOKEN_ERR("unterminated string constant", *line);
          return false;
        }
      else if (ch == '"')
        {
          AddToken(TokenString, token, include);
          return true;
        }
      else
        {
          token[len++] = ch;
        }
    }
  assert(false);
  return false;
}


// Read in a whitespace token
bool Worldfile::LoadTokenSpace(FILE *file, int *line, int include)
{
  int ch;
  int len;
  char token[256];

  len = 0;
  memset(token, 0, sizeof(token));

  while (true)
    {
      ch = fgetc(file);

      if (ch == EOF)
        {
          AddToken(TokenSpace, token, include);
          return true;
        }
      else if (isblank(ch))
        {
          token[len++] = ch;
        }
      else
        {
          AddToken(TokenSpace, token, include);
          ungetc(ch, file);
          return true;
        }
    }
  assert(false);
  return false;
}


// Save tokens to a file.
bool Worldfile::SaveTokens(FILE *file)
{
  unsigned int i;
  CToken *token;

  for (i = 0; i < this->tokens.size(); i++)
    {
      token = &this->tokens[i];

      if (token->include > 0)
        continue;
      if (token->type == TokenString)
                                fprintf(file, "\"%s\"", token->value.c_str());
      else
                                fprintf(file, "%s", token->value.c_str());
    }
  return true;
}


// Clear the token list
void Worldfile::ClearTokens()
{
        tokens.clear();
}


// Add a token to the token list
bool Worldfile::AddToken(int type, const char *value, int include)
{
        tokens.push_back( CToken( include, type, value ));
  return true;
}


// Set a token value in the token list
bool Worldfile::SetTokenValue(int index, const char *value)
{
  assert(index >= 0 && index < (int)this->tokens.size() );
        tokens[index].value = value;
  return true;
}


// Get the value of a token
const char *Worldfile::GetTokenValue(int index)
{
  assert(index >= 0 && index < (int)this->tokens.size());
  return this->tokens[index].value.c_str();
}


// Dump the token list (for debugging).
void Worldfile::DumpTokens()
{
  int line;

  line = 1;
  printf("\n## begin tokens\n");
  printf("## %4d : ", line);

        FOR_EACH( it, tokens )
  //for (int i = 0; i < this->token_count; i++)
    {
      if ( it->value[0] == '\n')
                                printf("[\\n]\n## %4d : %02d ", ++line, it->include);
      else
                                printf("[%s] ", it->value.c_str());
    }
  printf("\n");
  printf("## end tokens\n");
}


// Parse tokens into entities and properties.
bool Worldfile::ParseTokens()
{
  int i;
  int entity;
  int line;
  CToken *token;

  ClearEntities();
  ClearProperties();

  // Add in the "global" entity.
  entity = AddEntity(-1, "");
  line = 1;

  for (i = 0; i < (int)this->tokens.size(); i++)
    {
      token = &this->tokens[0] + i;
                        
      switch (token->type)
                                {
                                case TokenWord:
                                        if ( token->value == "include") 
                                                {
                                                        if (!ParseTokenInclude(&i, &line))
                                                                return false;
                                                }
                                        else if ( token->value == "define" )
                                                {
                                                        if (!ParseTokenDefine(&i, &line))
                                                                return false;
                                                }
                                        else
                                                {
                                                        if (!ParseTokenWord(entity, &i, &line))
                                                                return false;
                                                }
                                        break;
                                case TokenComment:
                                        break;
                                case TokenSpace:
                                        break;
                                case TokenEOL:
                                        line++;
                                        break;
                                default:
                                        PARSE_ERR("syntax error 1", line);
                                        return false;
                                }
    }
  return true;
}


// Parse an include statement
bool Worldfile::ParseTokenInclude(int *index, int *line)
{
  int i;
  CToken *token;
        
  for (i = *index + 1; i < (int)this->tokens.size(); i++)
    {
      token = &this->tokens[i];
                        
      switch (token->type)
                                {
                                case TokenString:
                                        break;
                                case TokenSpace:
                                        break;
                                case TokenEOL:
                                        *index = i;
                                        (*line)++;
                                        return true;
                                default:
                                        PARSE_ERR("syntax error in include statement", *line);
                                        return false;
                                }
    }
  PARSE_ERR("incomplete include statement", *line);
  return false;
}


// Parse a macro definition
bool Worldfile::ParseTokenDefine(int *index, int *line)
{
  int i;
  int count;
  const char *macroname, *entityname;
  int starttoken;
  CToken *token;

  count = 0;
  macroname = NULL;
  entityname = NULL;
  starttoken = -1;

  for (i = *index + 1; i < (int)this->tokens.size(); i++)
    {
      token = &this->tokens[i];

      switch (token->type)
        {
        case TokenWord:
          if (count == 0)
            {
              if (macroname == NULL)
                macroname = GetTokenValue(i);
              else if (entityname == NULL)
                {
                  entityname = GetTokenValue(i);
                  starttoken = i;
                }
              else
                {
                  PARSE_ERR("extra tokens in macro definition", *line);
                  return false;
                }
            }
          else
            {
              if (macroname == NULL)
                {
                  PARSE_ERR("missing name in macro definition", *line);
                  return false;
                }
              if (entityname == NULL)
                {
                  PARSE_ERR("missing name in macro definition", *line);
                  return false;
                }
            }
          break;
        case TokenOpenEntity:
          count++;
          break;
        case TokenCloseEntity:
          count--;
          if (count == 0)
            {
              AddMacro(macroname, entityname, *line, starttoken, i);
              *index = i;
              return true;
            }
          if (count < 0)
            {
              PARSE_ERR("misplaced ')'", *line);
              return false;
            }
          break;
        default:
          break;
        }
    }
  PARSE_ERR("missing ')'", *line);
  return false;
}


// Parse something starting with a word; could be a entity or an property.
bool Worldfile::ParseTokenWord(int entity, int *index, int *line)
{
  int i;
  CToken *token;

  for (i = *index + 1; i < (int)this->tokens.size(); i++)
    {
      token = &this->tokens[i];

      switch (token->type)
        {
        case TokenComment:
          break;
        case TokenSpace:
          break;
        case TokenEOL:
          (*line)++;
          break;
        case TokenOpenEntity:
          return ParseTokenEntity(entity, index, line);
        case TokenNum:
        case TokenString:
        case TokenOpenTuple:
          return ParseTokenProperty(entity, index, line);
        default:
          PARSE_ERR("syntax error 2", *line);
          return false;
        }
    }

  return false;
}


// Parse a entity from the token list.
bool Worldfile::ParseTokenEntity(int entity, int *index, int *line)
{
  int i;
  //int macro;
  int name;
  CToken *token;

  name = *index;
  CMacro* macro = LookupMacro(GetTokenValue(name));

  // If the entity name is a macro...
  if (macro )
    {
      int nentity = this->entities.size();
      int mindex = macro->starttoken;
      int mline = macro->line;
      if (!ParseTokenEntity(entity, &mindex, &mline))
                                return false;
      entity = nentity;

      for (i = *index + 1; i < (int)this->tokens.size(); i++)
                                {
                                        token = &this->tokens[i];

          switch (token->type)
            {
            case TokenOpenEntity:
              break;
            case TokenWord:
              if (!ParseTokenWord(entity, &i, line))
                return false;
              break;
            case TokenCloseEntity:
              *index = i;
              return true;
            case TokenComment:
              break;
            case TokenSpace:
              break;
            case TokenEOL:
              (*line)++;
              break;
            default:
              PARSE_ERR("syntax error 3", *line);
              return false;
            }
        }
      PARSE_ERR("missing ')'", *line);
    }

  // If the entity name is not a macro...
  else
    {
      for (i = *index + 1; i < (int)this->tokens.size(); i++)
        {
          token = &this->tokens[i];

          switch (token->type)
            {
            case TokenOpenEntity:
              entity = AddEntity(entity, GetTokenValue(name));
              break;
            case TokenWord:
              if (!ParseTokenWord(entity, &i, line))
                return false;
              break;
            case TokenCloseEntity:
              *index = i;
              return true;
            case TokenComment:
              break;
            case TokenSpace:
              break;
            case TokenEOL:
              (*line)++;
              break;
            default:
              PARSE_ERR("syntax error 3", *line);
              return false;
            }
        }
      PARSE_ERR("missing ')'", *line);
    }
  return false;
}


// Parse an property from the token list.
bool Worldfile::ParseTokenProperty(int entity, int *index, int *line)
{
  CProperty* property(NULL);
  int name( *index );
  CToken *token(NULL);
  
  for(int i = *index + 1; i < (int)this->tokens.size(); i++)
    {
      token = &this->tokens[i];

      switch (token->type)
        {
        case TokenNum:
          property = AddProperty(entity, GetTokenValue(name), *line);
          AddPropertyValue(property, 0, i);
          *index = i;
          return true;
        case TokenString:
          property = AddProperty(entity, GetTokenValue(name), *line);
          AddPropertyValue(property, 0, i);
          *index = i;
          return true;
        case TokenOpenTuple:
          property = AddProperty(entity, GetTokenValue(name), *line);
          if (!ParseTokenTuple( property, &i, line))
            return false;
          *index = i;
          return true;
        case TokenSpace:
          break;
        default:
          PARSE_ERR("syntax error 4", *line);
          return false;
        }
    }
  return true;
}


// Parse a tuple.
bool Worldfile::ParseTokenTuple( CProperty* property, int *index, int *line)
{
  unsigned int i, count;
  CToken *token;

  count = 0;

  for (i = *index + 1; i < this->tokens.size(); i++)
    {
      token = &this->tokens[i];

      switch (token->type)
        {
        case TokenNum:
          AddPropertyValue(property, count++, i);
          *index = i;
          break;
        case TokenString:
          AddPropertyValue(property, count++, i);
          *index = i;
          break;
        case TokenCloseTuple:
          *index = i;
          return true;
        case TokenSpace:
          break;
        default:
          PARSE_ERR("syntax error 5", *line);
          return false;
        }
    }
  return true;
}


// Clear the macro list
void Worldfile::ClearMacros()
{
        macros.clear();
}


// Add a macro
void Worldfile::AddMacro(const char *macroname, const char *entityname,
                                                                                                 int line, int starttoken, int endtoken)
{
        macros.insert( std::pair<std::string,CMacro>( macroname, CMacro( macroname, entityname, line, starttoken, endtoken )));
}


// Lookup a macro by name
// Returns -1 if there is no macro with this name.
Worldfile::CMacro* Worldfile::LookupMacro(const char *macroname)
{
        std::map<std::string,CMacro>::iterator it = macros.find( macroname );
        
        if( it == macros.end() )
                return NULL;
        else
                return &it->second;
}


// Dump the macro list for debugging
void Worldfile::DumpMacros()
{
  printf("\n## begin macros\n");

        FOR_EACH( it, macros )
                //for (int i = 0; i < this->macro_count; i++)
    {
      CMacro *macro = &(it->second);//this->macros + i;

                                printf("## [%s][%s]", macro->macroname.c_str(), macro->entityname.c_str());
      for (int j = macro->starttoken; j <= macro->endtoken; j++)
        {
          if (this->tokens[j].type == TokenEOL)
            printf("[\\n]");
          else
            printf("[%s]", GetTokenValue(j));
        }
      printf("\n");
    }
  printf("## end macros\n");
}


// Clear the entity list
void Worldfile::ClearEntities()
{
  // free(this->entities);
  // this->entities = NULL;
  // this->entity_size = 0;
  // this->entity_count = 0;

        this->entities.clear();
}


// Add a entity
int Worldfile::AddEntity(int parent, const char *type)
{
  // if (this->entity_count >= this->entity_size)
  //   {
  //     this->entity_size += 100;
  //     this->entities = (CEntity*)
        // realloc(this->entities, this->entity_size * sizeof(this->entities[0]));
  //   }

  // int entity = this->entity_count;
  // this->entities[entity].parent = parent;
  // this->entities[entity].type = type;
  // this->entity_count++;

        this->entities.push_back( CEntity( parent, type ));
  return( entities.size()-1); // index of the new entity
}


// Get the number of entities
int Worldfile::GetEntityCount()
{
  return this->entities.size();
}


// Get a entity's parent entity
int Worldfile::GetEntityParent(int entity)
{
  if (entity < 0 || entity >= (int)this->entities.size())
    return -1;
  return this->entities[entity].parent;
}


// Get a entity (returns the entity type value)
const char *Worldfile::GetEntityType(int entity)
{
  if (entity < 0 || entity >= (int)this->entities.size())
    return NULL;
  return this->entities[entity].type.c_str();
}


// Lookup a entity number by type name
// Returns -1 if there is no entity with this type
int Worldfile::LookupEntity(const char *type)
{
  for (int entity = 0; entity < GetEntityCount(); entity++)
    {
      if (strcmp(GetEntityType(entity), type) == 0)
        return entity;
    }
  return -1;
}


void PrintProp( const char* key, CProperty* prop )
{
  if( prop )
    printf( "Print key %s prop ent %d name %s\n", key, prop->entity, prop->name.c_str() );
}

// Dump the entity list for debugging
void Worldfile::DumpEntities()
{
  printf("\n## begin entities\n");

        FOR_EACH( it, properties )
                PrintProp( it->first.c_str(), it->second );

  printf("## end entities\n");
}


// Clear the property list
void Worldfile::ClearProperties()
{
        FOR_EACH( it, properties )
                delete it->second;      
        properties.clear();
}


// Add an property
CProperty* Worldfile::AddProperty(int entity, const char *name, int line)
{
  char key[128];
  snprintf( key, 127, "%d%s", entity, name );

  CProperty *property = new CProperty( entity, name, line );

        properties[ key ] = property;

        return property;
}


// Add an property value
void Worldfile::AddPropertyValue( CProperty* property, int index, int value_token)
{
  assert(property);

  // Set the relevant value
        
        if( index >= (int)property->values.size() )
                property->values.resize( index+1 );

  property->values[index] = value_token;
}



// Get an property
CProperty* Worldfile::GetProperty(int entity, const char *name)
{
  char key[128];
  snprintf( key, 127, "%d%s", entity, name );
  
  //printf( "looking up key %s for entity %d name %s\n", key, entity, name );
  
  static char cache_key[128] = { 0 }; 
  static CProperty* cache_property = NULL;
  
  if( strncmp( key, cache_key, 128 ) != 0 ) // different to last time
         {              
                strncpy( cache_key, key, 128 ); // remember for next time               
                
                std::map<std::string,CProperty*>::iterator it = properties.find( key ); 
                if( it == properties.end() ) // not found
                  cache_property = NULL;
                else
                  cache_property = it->second;          
         }
  //else
  // printf( "cache hit with %s\n", cache_key );
        
  return cache_property;
}

bool Worldfile::PropertyExists( int section, const char* token )
{
  return (bool)GetProperty( section, token );
}


// Set the value of an property
void Worldfile::SetPropertyValue( CProperty* property, int index, const char *value)
{
  assert( property );
  //printf( "property %s index %d values %d \n",
  //      property->name.c_str(), index, (int)property->values.size() );

  assert(index >= 0 && index < (int)property->values.size() );
  // Set the relevant value
  SetTokenValue( property->values[index], value);
}

// Get the value of an property
const char *Worldfile::GetPropertyValue(CProperty* property, int index)
{
  assert(property);
  property->used = true;
  return GetTokenValue(property->values[index]);
}


// Dump the property list for debugging
void Worldfile::DumpProperties()
{
  printf("\n## begin properties\n");
  //   for (int i = 0; i < this->property_count; i++)
  //   {
  //     CProperty *property = this->properties + i;
  //     CEntity *entity = this->entities + property->entity;

  //     printf("## [%d]", property->entity);
  //     printf("[%s]", entity->type);
  //     printf("[%s]", property->name);
  //     for (int j = 0; j < property->value_count; j++)
  //       printf("[%s]", GetTokenValue(property->values[j]));
  //     printf("\n");
  //   }
  printf("## end properties\n");
}


// Read a string
const std::string Worldfile::ReadString(int entity, const char *name, const std::string& value)
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return value;
  return GetPropertyValue(property, 0);
}


// Write a string
void Worldfile::WriteString(int entity, const char *name, const std::string &value)
{
  CProperty* property = GetProperty(entity, name);
  if( property == NULL )
    return;
  SetPropertyValue(property, 0, value.c_str());
}


// Read an int
int Worldfile::ReadInt(int entity, const char *name, int value)
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return value;
  return atoi(GetPropertyValue(property, 0));
}


// Write an int
void Worldfile::WriteInt(int entity, const char *name, int value)
{
  char default_str[64];
  snprintf(default_str, sizeof(default_str), "%d", value);
  WriteString(entity, name, default_str);
}

// Write a float
void Worldfile::WriteFloat(int entity, const char *name, double value)
{
  // compact zeros make the file more readable
  if( fabs(value) < 0.001 ) // nearly 0
    WriteString(entity, name, "0" ); 
  else
    {
      char default_str[64];
      snprintf(default_str, sizeof(default_str), "%.3f", value);
      WriteString(entity, name, default_str);
    }
}


// Read a float
double Worldfile::ReadFloat(int entity, const char *name, double value)
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return value;
  return atof(GetPropertyValue(property, 0));
}


// Read a file name
// Always returns an absolute path.
// If the filename is entered as a relative path, we prepend
// the world files path to it.
// Known bug: will leak memory everytime it is called (but its not called often,
// so I cant be bothered fixing it).
const char *Worldfile::ReadFilename(int entity, const char *name, const char *value)
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return value;
  const char *filename = GetPropertyValue(property, 0);

  if( filename[0] == '/' || filename[0] == '~' )
    return filename;

  else if (this->filename[0] == '/' || this->filename[0] == '~')
    {
      // Note that dirname() modifies the contents, so
      // we need to make a copy of the filename.
      // There's no bounds-checking, but what the heck.
      char *tmp = strdup(this->filename.c_str());
                char* fullpath = new char[PATH_MAX];
      memset(fullpath, 0, PATH_MAX);
      strcat( fullpath, dirname(tmp));
      strcat( fullpath, "/" );
      strcat( fullpath, filename );
      assert(strlen(fullpath) + 1 < PATH_MAX);
      if(tmp) free(tmp);
      return fullpath;
    }
  else
    {
      // Prepend the path
      // Note that dirname() modifies the contents, so
      // we need to make a copy of the filename.
      // There's no bounds-checking, but what the heck.
      char *tmp = strdup(this->filename.c_str());
                char* fullpath = new char[PATH_MAX];
      char* dummy = getcwd(fullpath, PATH_MAX);
      if (!dummy)
      {
        PRINT_ERR2("unable to get cwd %d: %s", errno, strerror(errno));
                  if( fullpath ) delete[] fullpath;
                  if( tmp ) free(tmp);
        return value;
      }

      strcat( fullpath, "/" );
      strcat( fullpath, dirname(tmp));
      strcat( fullpath, "/" );
      strcat( fullpath, filename );
      assert(strlen(fullpath) + 1 < PATH_MAX);
      free(tmp);

      return fullpath;
    }
}


// Read a series of floats from a tuple (experimental)
int Worldfile::ReadTuple( const int entity, const char *name,
                         const unsigned int first, const unsigned int count, const char* format, ... )
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return 0;
  
  if( property->values.size() < first+count )
    {
      PRINT_ERR4( "Worldfile: reading tuple \"%s\" index %u to %u - tuple has length %u\n",
                  name, first, first+count-1, (unsigned int)property->values.size() );
      exit(-1);
    }
    
  if( strlen(format) != count )
    {
      PRINT_ERR2( "format string length %u does not match argument count %u", 
                  (unsigned int)strlen(format), count );
      exit(-1);
    }
  
  va_list args;
  va_start( args, format );

  for( unsigned int i=0; i<count; i++ )
    {
      const char* val = GetPropertyValue(property, first+i);
      
      switch( format[i] )
        {
        case 'i': // signed integer
          *va_arg( args, int* ) = atoi(val);
          break;

        case 'u': // unsigned integer
          *va_arg( args, unsigned int* ) = (unsigned int)atoi(val);
          break;

        case 'f': // float
          *va_arg( args, double* ) = atof(val);
          break;
          
        case 'l': //length
          *va_arg( args, double* ) = atof(val) * unit_length;
          break;
          
        case 'a': // angle
          *va_arg( args, double* ) = atof(val) * unit_angle;
          break;
          
        case 's': // string
          *va_arg( args, char** ) = strdup(val);
          break;
          
        default:
          PRINT_ERR3( "Unknown format character %c in string %s loading %s",
                      format[i], format, name );
        }
    }

  va_end( args );
  
  return count;
}

// Write a series of floats to a tuple (experimental)
void Worldfile::WriteTuple( const int entity, const char *name,
                         const unsigned int first, const unsigned int count, const char* format, ... )
{
  CProperty* property = GetProperty(entity, name);
  if (property == NULL )
    return;
  
  if( property->values.size() < first+count )
    {
      PRINT_ERR4( "Worldfile: reading tuple \"%s\" index %d to %d - tuple has length %d\n",
                  name, first, first+count-1, (int)property->values.size() );
      exit(-1);
    }
  
  if( strlen(format) != count )
    {
      PRINT_ERR2( "format string length %u does not match argument count %u", 
                  (unsigned int)strlen(format), count );
      exit(-1);
    }
  
  char buf[2048];
  buf[0] = 0;
  
  va_list args;
  va_start( args, format );
  
  for( unsigned int i=0; i<count; i++ )
    {      
      switch( format[i] )
        {
        case 'i': // integer
          snprintf( buf, sizeof(buf), "%d", va_arg( args, int ) );
          break;

        case 'u': // unsigned integer
          snprintf( buf, sizeof(buf), "%u", va_arg( args, unsigned int ) );
          break;

        case 'f': // float
          snprintf( buf, sizeof(buf), "%.3f", va_arg( args, double ) );
          break;
          
        case 'l': //length
          snprintf( buf, sizeof(buf), "%.3f",  va_arg( args, double ) / unit_length );
          break;
          
        case 'a': // angle
          snprintf( buf, sizeof(buf), "%.3f", va_arg( args, double ) / unit_angle );
          break;
          
        case 's': // string
          strncpy( buf, va_arg( args, char* ), sizeof(buf) );
          buf[sizeof(buf)-1] = 0; // force zero terminator
          break;
          
        default:
          PRINT_ERR3( "Unknown format character %c in string %s loading %s",
                      format[i], format, name );
          exit(-1);
        }
      
      //printf( "writing %s %d %s\n", name, first+i, buf );
      SetPropertyValue(property, first+i, buf );
    }
  
  va_end( args );
}