stage.hh

Go to the documentation of this file.

#ifndef STG_H
#define STG_H
/*
 *  Stage : a multi-robot simulator. Part of the Player Project.
 *
 *  Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
 *  Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
 *  Asher, Pooya Karimian
 *
 *  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
 *
 */

// C libs
#include <assert.h>
#include <libgen.h>
#include <pthread.h>
#include <stdint.h> // for portable int types eg. uint32_t
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

// C++ libs
#include <algorithm>
#include <cmath>
#include <iostream>
#include <list>
#include <map>
#include <queue>
#include <set>
#include <vector>

// FLTK Gui includes
#include <FL/Fl.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Gl_Window.H>
#include <FL/Fl_Menu_Bar.H>
#include <FL/Fl_Window.H>
#include <FL/fl_draw.H>
#include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
// except GLU
#ifdef __APPLE__
#include <OpenGL/glu.h>
#else
#include <GL/glu.h>
#endif

namespace Stg {
// forward declare
class Block;
class Canvas;
class Cell;
class Worldfile;
class World;
class WorldGui;
class Model;
class OptionsDlg;
class Camera;
class FileManager;
class Option;

typedef Model *(*creator_t)(World *, Model *, const std::string &type);

void Init(int *argc, char **argv[]);

bool InitDone();

const char *Version();

unsigned int FullVersion();

const char COPYRIGHT[] = "Copyright Richard Vaughan and contributors 2000-2017";

const char AUTHORS[] = "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, "
                       "Toby Collett, Jeremy Asher, Alex Couture-Beil, Adrian Böckenkamp and "
                       "contributors.";

const char WEBSITE[] = "http://playerstage.org";

const char DESCRIPTION[] = "Robot simulation library\nPart of the Player Project";

const char LICENSE[] =
    "Stage robot simulation library\n"
    "Copyright (C) 2000-2017 Richard Vaughan and contributors\n"
    "Part of the Player Project [http://playerstage.org]\n"
    "\n"
    "This program is free software; you can redistribute it and/or\n"
    "modify it under the terms of the GNU General Public License\n"
    "as published by the Free Software Foundation; either version 2\n"
    "of the License, or (at your option) any later version.\n"
    "\n"
    "This program is distributed in the hope that it will be useful,\n"
    "but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
    "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n"
    "GNU General Public License for more details.\n"
    "\n"
    "You should have received a copy of the GNU General Public License\n"
    "along with this program; if not, write to the Free Software\n"
    "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\n"
    "\n"
    "The text of the license may also be available online at\n"
    "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";

const double thousand = 1e3;

const double million = 1e6;

const double billion = 1e9;

inline double rtod(double r)
{
  return (r * 180.0 / M_PI);
}

inline double dtor(double d)
{
  return (d * M_PI / 180.0);
}

inline double normalize(double a)
{
  while (a < -M_PI)
    a += 2.0 * M_PI;
  while (a > M_PI)
    a -= 2.0 * M_PI;
  return a;
}

inline int sgn(int a)
{
  return (a < 0 ? -1 : 1);
}

inline double sgn(double a)
{
  return (a < 0 ? -1.0 : 1.0);
}

enum { FiducialNone = 0 };

typedef uint32_t id_t;

typedef double meters_t;

typedef double radians_t;

typedef struct timeval time_t;

typedef unsigned long msec_t;

typedef uint64_t usec_t;

typedef double kg_t; // Kilograms (mass)

typedef double joules_t;

typedef double watts_t;

class Color {
public:
  double r, g, b, a;

  explicit Color(double r, double g, double b, double a = 1.0);

  explicit Color(const std::string &name);

  Color();

  bool operator!=(const Color &other) const;
  bool operator==(const Color &other) const;
  static Color RandomColor();
  void Print(const char *prefix) const;

  static const Color blue, red, green, yellow, magenta, cyan;

  const Color &Load(Worldfile *wf, int entity);

  void GLSet(void) { glColor4f(r, g, b, a); }
};

class Size {
public:
  meters_t x, y, z;

  Size(meters_t x, meters_t y, meters_t z) : x(x), y(y), z(z) { /*empty*/}

  Size() : x(0.4), y(0.4), z(1.0) { /*empty*/}

  Size &Load(Worldfile *wf, int section, const char *keyword);
  void Save(Worldfile *wf, int section, const char *keyword) const;

  void Zero() { x = y = z = 0.0; }
};

class Pose {
public:
  meters_t x, y, z; 
  radians_t a; 

  Pose(meters_t x, meters_t y, meters_t z, radians_t a) : x(x), y(y), z(z), a(a) { /*empty*/}

  Pose() : x(0.0), y(0.0), z(0.0), a(0.0) { /*empty*/}

  virtual ~Pose() {}
  static Pose Random(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax)
  {
    return Pose(xmin + drand48() * (xmax - xmin), ymin + drand48() * (ymax - ymin), 0,
                normalize(drand48() * (2.0 * M_PI)));
  }

  virtual void Print(const char *prefix) const
  {
    printf("%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x, y, z, a);
  }

  std::string String() const
  {
    char buf[256];
    snprintf(buf, 256, "[ %.3f %.3f %.3f %.3f ]", x, y, z, a);
    return std::string(buf);
  }

  bool IsZero() const { return (!(x || y || z || a)); }
  void Zero() { x = y = z = a = 0.0; }
  Pose &Load(Worldfile *wf, int section, const char *keyword);
  void Save(Worldfile *wf, int section, const char *keyword);

  inline Pose operator+(const Pose &p) const
  {
    const double cosa = cos(a);
    const double sina = sin(a);

    return Pose(x + p.x * cosa - p.y * sina, y + p.x * sina + p.y * cosa, z + p.z,
                normalize(a + p.a));
  }

  bool operator<(const Pose &p) const
  {
    // return( hypot( y, x ) < hypot( otHer.y, other.x ));
    // just compare the squared values to avoid the sqrt()
    return ((y * y + x * x) < (p.y * p.y + p.x * p.x));
  }

  bool operator==(const Pose &other) const
  {
    return (x == other.x && y == other.y && z == other.z && a == other.a);
  }

  bool operator!=(const Pose &other) const
  {
    return (x != other.x || y != other.y || z != other.z || a != other.a);
  }

  meters_t Distance(const Pose &other) const { return hypot(x - other.x, y - other.y); }
};

class RaytraceResult {
public:
  Pose pose;
  Model *mod;
  Color color;
  meters_t range;

  RaytraceResult() : pose(), mod(NULL), color(), range(0.0) {}
  RaytraceResult(const Pose &pose, Model *mod, const Color &color, const meters_t range)
      : pose(pose), mod(mod), color(color), range(range)
  {
  }
};

class Velocity : public Pose {
public:
  Velocity(double x, double y, double z, double a) : Pose(x, y, z, a) { /*empty*/}

  Velocity() { /*empty*/}

  Velocity &Load(Worldfile *wf, int section, const char *keyword)
  {
    Pose::Load(wf, section, keyword);
    return *this;
  }

  virtual void Print(const char *prefix) const
  {
    if (prefix)
      printf("%s", prefix);

    printf("velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", x, y, z, a);
  }
};

class Geom {
public:
  Pose pose; 
  Size size; 

  void Print(const char *prefix) const
  {
    if (prefix)
      printf("%s", prefix);

    printf("geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n", pose.x, pose.y, pose.a, size.x,
           size.y);
  }

  Geom() : pose(), size() {}
  Geom(const Pose &p, const Size &s) : pose(p), size(s) {}
  void Zero()
  {
    pose.Zero();
    size.Zero();
  }
};

class Bounds {
public:
  double min;
  double max;

  Bounds() : min(0), max(0) { /* empty*/}
  Bounds(double min, double max) : min(min), max(max) { /* empty*/}

  Bounds &Load(Worldfile *wf, int section, const char *keyword);

  double Constrain(double value);
};

class bounds3d_t {
public:
  Bounds x;
  Bounds y;
  Bounds z;

  bounds3d_t() : x(), y(), z() {}
  bounds3d_t(const Bounds &x, const Bounds &y, const Bounds &z) : x(x), y(y), z(z) {}
};

typedef struct {
  Bounds range; 
  radians_t angle; 
} fov_t;

class point_t {
public:
  meters_t x, y;
  point_t(meters_t x, meters_t y) : x(x), y(y) {}
  point_t() : x(0.0), y(0.0) {}
  bool operator+=(const point_t &other) { return ((x += other.x) && (y += other.y)); }
  bool operator<(const point_t &other) const
  {
    if (x < other.x)
      return true;
    if (other.x < x)
      return false;
    return y < other.y;
  }

  bool operator==(const point_t &other) const { return ((x == other.x) && (y == other.y)); }
};

class point3_t {
public:
  meters_t x, y, z;
  point3_t(meters_t x, meters_t y, meters_t z) : x(x), y(y), z(z) {}
  point3_t() : x(0.0), y(0.0), z(0.0) {}
};

class point_int_t {
public:
  int x, y;
  point_int_t(int x, int y) : x(x), y(y) {}
  point_int_t() : x(0), y(0) {}
  bool operator<(const point_int_t &other) const
  {
    if (x < other.x)
      return true;
    if (other.x < x)
      return false;
    return y < other.y;
  }

  bool operator==(const point_int_t &other) const { return ((x == other.x) && (y == other.y)); }
};

point_t *unit_square_points_create();

namespace Gl {
void pose_shift(const Pose &pose);
void pose_inverse_shift(const Pose &pose);
void coord_shift(double x, double y, double z, double a);
void draw_grid(bounds3d_t vol);
void draw_string(float x, float y, float z, const char *string);
void draw_string_multiline(float x, float y, float w, float h, const char *string, Fl_Align align);
void draw_speech_bubble(float x, float y, float z, const char *str);
void draw_octagon(float w, float h, float m);
void draw_octagon(float x, float y, float w, float h, float m);
void draw_vector(double x, double y, double z);
void draw_origin(double len);
void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset,
                float min, float max);
void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset);
void draw_centered_rect(float x, float y, float dx, float dy);
} // namespace Gl

void RegisterModels();

class Visualizer {
private:
  const std::string menu_name;
  const std::string worldfile_name;

public:
  Visualizer(const std::string &menu_name, const std::string &worldfile_name)
      : menu_name(menu_name), worldfile_name(worldfile_name)
  {
  }

  virtual ~Visualizer(void) {}
  virtual void Visualize(Model *mod, Camera *cam) = 0;

  const std::string &GetMenuName() { return menu_name; }
  const std::string &GetWorldfileName() { return worldfile_name; }
};

typedef int (*model_callback_t)(Model *mod, void *user);

typedef int (*world_callback_t)(World *world, void *user);

double constrain(double val, double minval, double maxval);

typedef struct {
  int enabled;
  Pose pose;
  meters_t size; 
  Color color;
  msec_t period; 
  double duty_cycle; 
} blinkenlight_t;

typedef struct {
  Pose pose;
  Size size;
} rotrect_t; 

int polys_from_image_file(const std::string &filename, std::vector<std::vector<point_t> > &polys);

typedef bool (*ray_test_func_t)(Model *candidate, const Model *finder, const void *arg);

#define VAR(V, init) __typeof(init) V = (init)

//#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I)

// NOTE:
// this version assumes the container is not modified in the loop,
// which I think is true everywhere it is used in Stage
#define FOR_EACH(I, C) for (VAR(I, (C).begin()), ite = (C).end(); (I) != ite; ++(I))

template <class T, class C> void EraseAll(T thing, C &cont)
{
  cont.erase(std::remove(cont.begin(), cont.end(), thing), cont.end());
}

// Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m, a)                                                                           \
  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_ERR2(m, a, b)                                                                        \
  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_ERR3(m, a, b, c)                                                                     \
  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_ERR4(m, a, b, c, d)                                                                  \
  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_ERR5(m, a, b, c, d, e)                                                               \
  fprintf(stderr, "\033[41merr\033[0m: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

// Warning macros
#define PRINT_WARN(m) printf("\033[44mwarn\033[0m: " m " (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_WARN1(m, a) printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_WARN2(m, a, b)                                                                       \
  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_WARN3(m, a, b, c)                                                                    \
  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_WARN4(m, a, b, c, d)                                                                 \
  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_WARN5(m, a, b, c, d, e)                                                              \
  printf("\033[44mwarn\033[0m: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

// Message macros
#ifdef DEBUG
#define PRINT_MSG(m) printf("Stage: " m " (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_MSG1(m, a) printf("Stage: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_MSG2(m, a, b) printf("Stage: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_MSG3(m, a, b, c) printf("Stage: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_MSG4(m, a, b, c, d)                                                                  \
  printf("Stage: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_MSG5(m, a, b, c, d, e)                                                               \
  printf("Stage: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
#else
#define PRINT_MSG(m) printf("Stage: " m "\n")
#define PRINT_MSG1(m, a) printf("Stage: " m "\n", a)
#define PRINT_MSG2(m, a, b) printf("Stage: " m "\n,", a, b)
#define PRINT_MSG3(m, a, b, c) printf("Stage: " m "\n", a, b, c)
#define PRINT_MSG4(m, a, b, c, d) printf("Stage: " m "\n", a, b, c, d)
#define PRINT_MSG5(m, a, b, c, d, e) printf("Stage: " m "\n", a, b, c, d, e)
#endif

// DEBUG macros
#ifdef DEBUG
#define PRINT_DEBUG(m) printf("debug: " m " (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_DEBUG1(m, a) printf("debug: " m " (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_DEBUG2(m, a, b) printf("debug: " m " (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_DEBUG3(m, a, b, c) printf("debug: " m " (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_DEBUG4(m, a, b, c, d)                                                                \
  printf("debug: " m " (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_DEBUG5(m, a, b, c, d, e)                                                             \
  printf("debug: " m " (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
#else
#define PRINT_DEBUG(m)
#define PRINT_DEBUG1(m, a)
#define PRINT_DEBUG2(m, a, b)
#define PRINT_DEBUG3(m, a, b, c)
#define PRINT_DEBUG4(m, a, b, c, d)
#define PRINT_DEBUG5(m, a, b, c, d, e)
#endif

class Block;
class Model;

// ANCESTOR CLASS
class Ancestor {
  friend class Canvas; // allow Canvas access to our private members

protected:
  std::map<std::string, unsigned int> child_type_counts;

  std::vector<Model *> children;

  bool debug;

  std::map<std::string, void *> props;

  std::string token;

  Ancestor &Load(Worldfile *wf, int section);
  void Save(Worldfile *wf, int section);

public:
  Ancestor();
  virtual ~Ancestor();

  std::vector<Model *> &GetChildren() { return children; }
  void ForEachDescendant(model_callback_t func, void *arg);

  virtual void AddChild(Model *mod);
  virtual void RemoveChild(Model *mod);
  virtual Pose GetGlobalPose() const;

  const char *Token() const { return token.c_str(); }
  const std::string &TokenStr() const { return token; }
  virtual void SetToken(const std::string &str)
  {
    // printf( "Ancestor::SetToken( %s )\n", str.c_str() );

    if (str.size() > 0)
      token = str;
    else
      PRINT_WARN("Ancestor::SetToken() called with zero length string. Ignored.");
  }

  void SetProperty(std::string &key, void *value) { props[key] = value; }
  void *GetProperty(std::string &key)
  {
    std::map<std::string, void *>::iterator it = props.find(key);
    return (it == props.end() ? NULL : it->second);
  }
};

class Ray {
public:
  Ray(const Model *mod, const Pose &origin, const meters_t range, const ray_test_func_t func,
      const void *arg, const bool ztest)
      : mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
  {
  }

  Ray() : mod(NULL), origin(0, 0, 0, 0), range(0), func(NULL), arg(NULL), ztest(true) {}
  const Model *mod;
  Pose origin;
  meters_t range;
  ray_test_func_t func;
  const void *arg;
  bool ztest;
};

// defined in stage_internal.hh
class Region;
class SuperRegion;
class BlockGroup;
class PowerPack;

class LogEntry {
  usec_t timestamp;
  Model *mod;
  Pose pose;

public:
  LogEntry(usec_t timestamp, Model *mod);

  static std::vector<LogEntry> log;

  static size_t Count() { return log.size(); }
  static void Clear() { log.clear(); }
  static void Print();
};

class CtrlArgs {
public:
  std::string worldfile;
  std::string cmdline;

  CtrlArgs(std::string w, std::string c) : worldfile(w), cmdline(c) {}
};

class ModelPosition;

class World : public Ancestor {
public:
  friend class Block;
  friend class Model; // allow access to private members
  friend class ModelFiducial;
  friend class Canvas;
  friend class WorkerThread;

public:
  static std::vector<std::string> args;
  static std::string ctrlargs;

private:
  static std::set<World *> world_set; 
  static bool quit_all; 
  static void UpdateCb(World *world);
  static unsigned int next_id; 

  bool destroy;
  bool dirty; 

  std::set<Model *> models;

  std::map<std::string, Model *> models_by_name;

  std::map<int, Model *> models_by_wfentity;

  std::vector<Model *> models_with_fiducials;

  struct ltx {
    bool operator()(const Model *a, const Model *b) const;
  };

  struct lty {
    bool operator()(const Model *a, const Model *b) const;
  };

  std::set<Model *, ltx> models_with_fiducials_byx;

  std::set<Model *, lty> models_with_fiducials_byy;

  void FiducialInsert(Model *mod)
  {
    FiducialErase(mod); // make sure it's not there already
    models_with_fiducials.push_back(mod);
  }

  void FiducialErase(Model *mod) { EraseAll(mod, models_with_fiducials); }
  void LoadWorldPostHook();

  double ppm; 
  bool quit; 
  bool show_clock; 
  unsigned int show_clock_interval; 

  //--- thread sync ----
  pthread_mutex_t sync_mutex; 
  unsigned int threads_working; 
  pthread_cond_t threads_start_cond; 
  pthread_cond_t threads_done_cond; 
  int total_subs; 
  unsigned int worker_threads; 

protected:
  std::list<std::pair<world_callback_t, void *> >
      cb_list; 
  bounds3d_t extent; 
  bool graphics; 

  std::set<Option *> option_table; 
  std::list<PowerPack *>
      powerpack_list; 

  usec_t quit_time;
  std::list<float *> ray_list; 
  usec_t sim_time; 
  std::map<point_int_t, SuperRegion *> superregions;

  uint64_t updates; 
  Worldfile *wf; 

  void CallUpdateCallbacks(); 

public:
  uint64_t UpdateCount() { return updates; }
  bool paused; 

  virtual void Start() { paused = false; }
  virtual void Stop() { paused = true; }
  virtual void TogglePause() { paused ? Start() : Stop(); }
  bool Paused() const { return (paused); }
  virtual void Redraw(void) {} // does nothing
  std::vector<point_int_t> rt_cells;
  std::vector<point_int_t> rt_candidate_cells;

  static const int DEFAULT_PPM = 50; //<! default resolution in pixels per meter

  void AddUpdateCallback(world_callback_t cb, void *user);

  int RemoveUpdateCallback(world_callback_t cb, void *user);

  void Log(Model *mod);

  void NeedRedraw() { dirty = true; }
  Model *ground;

  virtual std::string ClockString(void) const;

  Model *CreateModel(Model *parent, const std::string &typestr);

  void LoadModel(Worldfile *wf, int entity);
  void LoadBlock(Worldfile *wf, int entity);
  void LoadBlockGroup(Worldfile *wf, int entity);
  void LoadSensor(Worldfile *wf, int entity);

  virtual Model *RecentlySelectedModel() const { return NULL; }
  void MapPoly(const std::vector<point_int_t> &poly, Block *block, unsigned int layer);

  SuperRegion *AddSuperRegion(const point_int_t &coord);
  SuperRegion *GetSuperRegion(const point_int_t &org);
  SuperRegion *GetSuperRegionCreate(const point_int_t &org);

  int32_t MetersToPixels(meters_t x) const { return (int32_t)floor(x * ppm); }
  point_int_t MetersToPixels(const point_t &pt) const
  {
    return point_int_t(MetersToPixels(pt.x), MetersToPixels(pt.y));
  }

  virtual void PushColor(Color col) { /* do nothing */ (void)col; }
  virtual void PushColor(double r, double g, double b, double a)
  { /* do nothing */
    (void)r;
    (void)g;
    (void)b;
    (void)a;
  }

  virtual void PopColor() { /* do nothing */}

  SuperRegion *CreateSuperRegion(point_int_t origin);
  void DestroySuperRegion(SuperRegion *sr);

  RaytraceResult Raytrace(const Ray &ray);

  RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func,
                          const Model *finder, const void *arg, const bool ztest);

  void Raytrace(const Pose &gpose, // global pose
                const meters_t range, const radians_t fov, const ray_test_func_t func,
                const Model *model, const void *arg, const bool ztest,
                std::vector<RaytraceResult> &results);

  inline void Extend(point3_t pt);

  virtual void AddModel(Model *mod);
  virtual void RemoveModel(Model *mod);

  void AddModelName(Model *mod, const std::string &name);

  void AddPowerPack(PowerPack *pp);
  void RemovePowerPack(PowerPack *pp);

  void ClearRays();

  void RecordRay(double x1, double y1, double x2, double y2);

  bool PastQuitTime();

  static void *update_thread_entry(std::pair<World *, int> *info);

  class Event {
  public:
    Event(usec_t time, Model *mod, model_callback_t cb, void *arg)
        : time(time), mod(mod), cb(cb), arg(arg)
    {
    }

    usec_t time; 
    Model *mod; 
    model_callback_t cb;
    void *arg;

    bool operator<(const Event &other) const;
  };

  std::vector<std::priority_queue<Event> > event_queues;

  std::vector<std::queue<Model *> > pending_update_callbacks;

  void Enqueue(unsigned int queue_num, usec_t delay, Model *mod, model_callback_t cb, void *arg)
  {
    event_queues[queue_num].push(Event(sim_time + delay, mod, cb, arg));
  }

  std::set<Model *> active_energy;
  void EnableEnergy(Model *m) { active_energy.insert(m); }
  void DisableEnergy(Model *m) { active_energy.erase(m); }
  std::set<ModelPosition *> active_velocity;

  usec_t sim_interval;

  int update_cb_count;

  void ConsumeQueue(unsigned int queue_num);

  unsigned int GetEventQueue(Model *mod) const;

public:
  static bool UpdateAll();

  static void Run();

  World(const std::string &name = "MyWorld", double ppm = DEFAULT_PPM);

  virtual ~World();

  usec_t SimTimeNow(void) const { return sim_time; }
  Worldfile *GetWorldFile() { return wf; }
  virtual bool IsGUI() const { return false; }
  virtual bool Load(const std::string &worldfile_path);

  virtual bool Load(std::istream &world_content, const std::string &worldfile_path = std::string());

  virtual void UnLoad();

  virtual void Reload();

  virtual bool Save(const char *filename);

  virtual bool Update(void);

  bool TestQuit() const { return (quit || quit_all); }
  void Quit() { quit = true; }
  void QuitAll() { quit_all = true; }
  void CancelQuit() { quit = false; }
  void CancelQuitAll() { quit_all = false; }
  void TryCharge(PowerPack *pp, const Pose &pose);

  double Resolution() const { return ppm; }
  Model *GetModel(const std::string &name) const;

  const std::set<Model *> GetAllModels() const { return models; }
  const bounds3d_t &GetExtent() const { return extent; }
  uint64_t GetUpdateCount() const { return updates; }
  void RegisterOption(Option *opt);

  void ShowClock(bool enable) { show_clock = enable; }
  Model *GetGround() { return ground; }
};

class Block {
  friend class BlockGroup;
  friend class Model;
  friend class SuperRegion;
  friend class World;
  friend class Canvas;
  friend class Cell;

public:
  Block(BlockGroup *group, const std::vector<point_t> &pts, const Bounds &zrange);

  Block(BlockGroup *group, Worldfile *wf, int entity);

  ~Block();

  void Map(unsigned int layer);

  void UnMap(unsigned int layer);

  void DrawSolid(bool topview);

  void DrawFootPrint();

  void Translate(double x, double y);

  double CenterX();

  double CenterY();

  void SetCenterX(double y);

  void SetCenterY(double y);

  void SetCenter(double x, double y);

  void SetZ(double min, double max);

  void AppendTouchingModels(std::set<Model *> &touchers);

  Model *TestCollision();

  void Load(Worldfile *wf, int entity);

  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                 meters_t cellheight);

  BlockGroup *group; 
private:
  std::vector<point_t> pts; 
  Bounds local_z; 
  Bounds global_z; 

  std::vector<Cell *> rendered_cells[2];

  void DrawTop();
  void DrawSides();
};

class BlockGroup {
  friend class Model;
  friend class Block;
  friend class World;
  friend class SuperRegion;

private:
  std::vector<Block> blocks; 
  int displaylist; 

public:
  Model &mod;

private:
  void AppendBlock(const Block &block);

  void CalcSize();
  void Clear(); 
  void AppendTouchingModels(std::set<Model *> &touchers);

  Model *TestCollision();

  void Map(unsigned int layer);
  void UnMap(unsigned int layer);

  void LoadBitmap(const std::string &bitmapfile, Worldfile *wf);

  void LoadBlock(Worldfile *wf, int entity);

  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                 meters_t cellheight);

  void DrawSolid(const Geom &geom);

  void BuildDisplayList();

  void CallDisplayList();

public:
  explicit BlockGroup(Model &mod);
  ~BlockGroup();

  uint32_t GetCount() const { return blocks.size(); }
  const Block &GetBlock(unsigned int index) const { return blocks[index]; }
  Block &GetBlockMutable(unsigned int index) { return blocks[index]; }
  bounds3d_t BoundingBox() const;

  void DrawFootPrint(const Geom &geom);
};

class Camera {
protected:
  double _pitch; // left-right (about y)
  double _yaw; // up-down (about x)
  double _x, _y, _z;

public:
  Camera() : _pitch(0), _yaw(0), _x(0), _y(0), _z(0) {}
  virtual ~Camera() {}
  virtual void Draw(void) const = 0;
  virtual void SetProjection(void) const = 0;

  double yaw(void) const { return _yaw; }
  double pitch(void) const { return _pitch; }
  double x(void) const { return _x; }
  double y(void) const { return _y; }
  double z(void) const { return _z; }
  virtual void reset() = 0;
  virtual void Load(Worldfile *wf, int sec) = 0;

  // TODO data should be passed in somehow else. (at least min/max stuff)
  // virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max )
  // const = 0;
};

class PerspectiveCamera : public Camera {
private:
  double _z_near;
  double _z_far;
  double _vert_fov;
  double _horiz_fov;
  double _aspect;

public:
  PerspectiveCamera(void);

  virtual void Draw(void) const;
  virtual void SetProjection(void) const;
  // void SetProjection( double aspect ) const;
  void update(void);

  void strafe(double amount);
  void forward(double amount);

  void setPose(double x, double y, double z)
  {
    _x = x;
    _y = y;
    _z = z;
  }
  void addPose(double x, double y, double z)
  {
    _x += x;
    _y += y;
    _z += z;
    if (_z < 0.1)
      _z = 0.1;
  }
  void move(double x, double y, double z);
  void setFov(double horiz_fov, double vert_fov)
  {
    _horiz_fov = horiz_fov;
    _vert_fov = vert_fov;
  }
  void setAspect(double aspect) { _aspect = aspect; }
  void setYaw(double yaw) { _yaw = yaw; }
  double horizFov(void) const { return _horiz_fov; }
  double vertFov(void) const { return _vert_fov; }
  void addYaw(double yaw) { _yaw += yaw; }
  void setPitch(double pitch) { _pitch = pitch; }
  void addPitch(double pitch)
  {
    _pitch += pitch;
    if (_pitch < 0)
      _pitch = 0;
    else if (_pitch > 180)
      _pitch = 180;
  }

  double realDistance(double z_buf_val) const
  {
    return _z_near * _z_far / (_z_far - z_buf_val * (_z_far - _z_near));
  }
  void scroll(double dy) { _z += dy; }
  double nearClip(void) const { return _z_near; }
  double farClip(void) const { return _z_far; }
  void setClip(double near, double far)
  {
    _z_far = far;
    _z_near = near;
  }

  void reset()
  {
    setPitch(70);
    setYaw(0);
  }

  void Load(Worldfile *wf, int sec);
  void Save(Worldfile *wf, int sec);
};

class OrthoCamera : public Camera {
private:
  double _scale;
  double _pixels_width;
  double _pixels_height;
  double _y_min;
  double _y_max;

public:
  OrthoCamera(void) : _scale(15), _pixels_width(0), _pixels_height(0), _y_min(0), _y_max(0) {}
  virtual void Draw() const;

  virtual void SetProjection(double pixels_width, double pixels_height, double y_min, double y_max);

  virtual void SetProjection(void) const;

  void move(double x, double y);

  void setYaw(double yaw) { _yaw = yaw; }
  void setPitch(double pitch) { _pitch = pitch; }
  void addYaw(double yaw) { _yaw += yaw; }
  void addPitch(double pitch)
  {
    _pitch += pitch;
    if (_pitch > 90)
      _pitch = 90;
    else if (_pitch < 0)
      _pitch = 0;
  }

  void setScale(double scale) { _scale = scale; }
  void setPose(double x, double y)
  {
    _x = x;
    _y = y;
  }

  void scale(double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0);
  void reset(void) { _pitch = _yaw = 0; }
  double scale() const { return _scale; }
  void Load(Worldfile *wf, int sec);
  void Save(Worldfile *wf, int sec);
};

class WorldGui : public World, public Fl_Window {
  friend class Canvas;
  friend class ModelCamera;
  friend class Model;
  friend class Option;

private:
  Canvas *canvas;
  std::vector<Option *> drawOptions;
  FileManager *fileMan; 
  std::vector<usec_t> interval_log;

  double speedup;

  bool confirm_on_quit; 

  Fl_Menu_Bar *mbar;
  OptionsDlg *oDlg;
  bool pause_time;
  std::string caption_prefix; 

  usec_t real_time_interval;

  usec_t real_time_now;

  usec_t real_time_recorded;

  uint64_t timing_interval;

  // static callback functions
  static void windowCb(Fl_Widget *w, WorldGui *wg);
  static void fileLoadCb(Fl_Widget *w, WorldGui *wg);
  static void fileSaveCb(Fl_Widget *w, WorldGui *wg);
  static void fileSaveAsCb(Fl_Widget *w, WorldGui *wg);
  static void fileExitCb(Fl_Widget *w, WorldGui *wg);
  static void viewOptionsCb(OptionsDlg *oDlg, WorldGui *wg);
  static void optionsDlgCb(OptionsDlg *oDlg, WorldGui *wg);
  static void helpAboutCb(Fl_Widget *w, WorldGui *wg);
  static void pauseCb(Fl_Widget *w, WorldGui *wg);
  static void onceCb(Fl_Widget *w, WorldGui *wg);
  static void fasterCb(Fl_Widget *w, WorldGui *wg);
  static void slowerCb(Fl_Widget *w, WorldGui *wg);
  static void realtimeCb(Fl_Widget *w, WorldGui *wg);
  static void fasttimeCb(Fl_Widget *w, WorldGui *wg);
  static void resetViewCb(Fl_Widget *w, WorldGui *wg);
  static void moreHelptCb(Fl_Widget *w, WorldGui *wg);

  // GUI functions
  bool saveAsDialog();
  bool closeWindowQuery();

  virtual void AddModel(Model *mod);

  void SetTimeouts();

  void LoadWorldGuiPostHook(usec_t load_start_time);

protected:
  virtual void PushColor(Color col);
  virtual void PushColor(double r, double g, double b, double a);
  virtual void PopColor();

  void DrawOccupancy() const;
  void DrawVoxels() const;

public:
  WorldGui(int width, int height, const char *caption = NULL);
  ~WorldGui();

  virtual void Redraw(void);

  virtual std::string ClockString() const;
  virtual bool Update();
  virtual bool Load(const std::string &worldfile_path);
  virtual bool Load(std::istream &world_content, const std::string &worldfile_path = std::string());

  virtual void UnLoad();
  virtual bool Save(const char *filename);
  virtual bool IsGUI() const { return true; }
  virtual Model *RecentlySelectedModel() const;

  virtual void Start();
  virtual void Stop();

  usec_t RealTimeNow(void) const;

  void DrawBoundingBoxTree();

  Canvas *GetCanvas(void) const { return canvas; }
  void Show();

  std::string EnergyString(void) const;
  virtual void RemoveChild(Model *mod);

  bool IsTopView();
};

class StripPlotVis : public Visualizer {
private:
  // Model* mod;
  float *data;
  size_t len;
  size_t count;
  // unsigned int index;
  float x, y, w, h, min, max;
  Color fgcolor, bgcolor;

public:
  StripPlotVis(float x, float y, float w, float h, size_t len, Color fgcolor, Color bgcolor,
               const char *name, const char *wfname);
  virtual ~StripPlotVis();
  virtual void Visualize(Model *mod, Camera *cam);
  void AppendValue(float value);
};

class PowerPack {
  friend class WorldGui;
  friend class Canvas;

protected:
  class DissipationVis : public Visualizer {
  private:
    unsigned int columns, rows;
    meters_t width, height;

    std::vector<joules_t> cells;

    joules_t peak_value;
    double cellsize;

    static joules_t global_peak_value;

  public:
    DissipationVis(meters_t width, meters_t height, meters_t cellsize);

    virtual ~DissipationVis();
    virtual void Visualize(Model *mod, Camera *cam);

    void Accumulate(meters_t x, meters_t y, joules_t amount);
  } event_vis;

  StripPlotVis output_vis;
  StripPlotVis stored_vis;

  Model *mod;

  joules_t stored;

  joules_t capacity;

  bool charging;

  joules_t dissipated;

  // these are used to visualize the power draw
  usec_t last_time;
  joules_t last_joules;
  watts_t last_watts;

public:
  static joules_t global_stored;
  static joules_t global_capacity;
  static joules_t global_dissipated;
  static joules_t global_input;

public:
  explicit PowerPack(Model *mod);
  ~PowerPack();

  void Visualize(Camera *cam);

  joules_t RemainingCapacity() const;

  void Add(joules_t j);

  void Subtract(joules_t j);

  void TransferTo(PowerPack *dest, joules_t amount);

  double ProportionRemaining() const { return (stored / capacity); }
  void Print(const char *prefix) const
  {
    if (prefix)
      printf("%s", prefix);

    printf("PowerPack %.2f/%.2f J\n", stored, capacity);
  }

  joules_t GetStored() const;
  joules_t GetCapacity() const;
  joules_t GetDissipated() const;
  void SetCapacity(joules_t j);
  void SetStored(joules_t j);

  bool GetCharging() const { return charging; }
  void ChargeStart() { charging = true; }
  void ChargeStop() { charging = false; }
  void Dissipate(joules_t j);

  void Dissipate(joules_t j, const Pose &p);
};

class Model : public Ancestor {
  friend class Ancestor;
  friend class World;
  friend class World::Event;
  friend class WorldGui;
  friend class Canvas;
  friend class Block;
  friend class Region;
  friend class BlockGroup;
  friend class PowerPack;
  friend class Ray;
  friend class ModelFiducial;

private:
  static uint32_t count;
  static std::map<id_t, Model *> modelsbyid;

  bool mapped;

  std::vector<Option *> drawOptions;
  const std::vector<Option *> &getOptions() const { return drawOptions; }
protected:
  bool alwayson;

  BlockGroup blockgroup;

  int boundary;

public:
  class cb_t {
  public:
    model_callback_t callback;
    void *arg;

    cb_t(model_callback_t cb, void *arg) : callback(cb), arg(arg) {}
    cb_t(world_callback_t cb, void *arg) : callback(NULL), arg(arg) { (void)cb; }
    cb_t() : callback(NULL), arg(NULL) {}
    bool operator<(const cb_t &other) const
    {
      if (callback == other.callback)
        return (arg < other.arg);
      // else
      return callback < other.callback;
    }

    bool operator==(const cb_t &other) const { return (callback == other.callback); }
  };

  class Flag {
  private:
    Color color;
    double size;
    int displaylist;

  public:
    void SetColor(const Color &col);
    void SetSize(double sz);

    Color GetColor() { return color; }
    double GetSize() { return size; }
    Flag(const Color &color, double size);
    Flag *Nibble(double portion);

    void Draw(GLUquadric *quadric);
  };

  typedef enum {
    CB_FLAGDECR,
    CB_FLAGINCR,
    CB_GEOM,
    CB_INIT,
    CB_LOAD,
    CB_PARENT,
    CB_POSE,
    CB_SAVE,
    CB_SHUTDOWN,
    CB_STARTUP,
    CB_UPDATE,
    CB_VELOCITY,
    // CB_POSTUPDATE,
    __CB_TYPE_COUNT 
  } callback_type_t;

protected:
  std::vector<std::set<cb_t> > callbacks;

  Color color;

  bool data_fresh;

  bool disabled;

  std::list<Visualizer *> cv_list;

  std::list<Flag *> flag_list;

  double friction;

  Geom geom;

  class GuiState {
  public:
    bool grid;
    bool move;
    bool nose;
    bool outline;

    GuiState();
    GuiState &Load(Worldfile *wf, int wf_entity);
  } gui;

  bool has_default_block;

  uint32_t id;
  usec_t interval; 
  usec_t interval_energy; 
  usec_t last_update; 
  bool log_state; 
  meters_t map_resolution;
  kg_t mass;

  Model *parent;

  Pose pose;

  PowerPack *power_pack;

  std::list<PowerPack *> pps_charging;

  class RasterVis : public Visualizer {
  private:
    uint8_t *data;
    unsigned int width, height;
    meters_t cellwidth, cellheight;
    std::vector<point_t> pts;

  public:
    RasterVis();
    virtual ~RasterVis(void) {}
    virtual void Visualize(Model *mod, Camera *cam);

    void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                 meters_t cellheight);

    int subs; //< the number of subscriptions to this model
    int used; //< the number of connections to this model

    void AddPoint(meters_t x, meters_t y);
    void ClearPts();

  } rastervis;

  bool rebuild_displaylist; 
  std::string say_string; 

  bool stack_children; 

  bool stall; 
  int subs; 

  bool thread_safe;

  class TrailItem {
  public:
    usec_t time;
    Pose pose;
    Color color;

    TrailItem() : time(0), pose(), color() {}
    // TrailItem( usec_t time, Pose pose, Color color )
    //: time(time), pose(pose), color(color){}
  };

  std::vector<TrailItem> trail;

  unsigned int trail_index;

//   /** The maxiumum length of the trail drawn. Default is 20, but can
// be set in the world file using the trail_length model
// property. */
//   unsigned int trail_length;

  uint64_t trail_interval;

  void UpdateTrail();

  // model_type_t type;
  const std::string type;
  unsigned int event_queue_num;
  bool used; 

  watts_t watts; 

  watts_t watts_give;

  watts_t watts_take;

  Worldfile *wf;
  int wf_entity;
  World *world; 
  WorldGui *world_gui; 

public:
  virtual void SetToken(const std::string &str)
  {
    // printf( "Model::SetToken( %s )\n", str.c_str() );

    if (str.size() > 0) {
      world->AddModelName(this, str);
      Ancestor::SetToken(str);
    } else
      PRINT_ERR("Model::SetToken() called with zero length string. Ignored.");
  }

  const std::string &GetModelType() const { return type; }
  std::string GetSayString() { return std::string(say_string); }
  Model *GetChild(const std::string &name) const;

  usec_t GetInterval() { return interval; }
  class Visibility {
  public:
    bool blob_return;
    int fiducial_key;
    int fiducial_return;
    bool gripper_return;
    bool obstacle_return;
    double ranger_return; 

    Visibility();

    Visibility &Load(Worldfile *wf, int wf_entity);
    void Save(Worldfile *wf, int wf_entity);
  } vis;

  usec_t GetUpdateInterval() const { return interval; }
  usec_t GetEnergyInterval() const { return interval_energy; }
  //    usec_t GetPoseInterval() const { return interval_pose; }

  void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                 meters_t cellheight);

  bool HasCollision() { return TestCollision() != NULL; }
private:
  explicit Model(const Model &original);

  Model &operator=(const Model &original);

protected:
  void RegisterOption(Option *opt);

  void AppendTouchingModels(std::set<Model *> &touchers);

  Model *TestCollision();

  void Map(unsigned int layer);

  inline void Map()
  {
    Map(0);
    Map(1);
  }

  void UnMap(unsigned int layer);

  inline void UnMap()
  {
    UnMap(0);
    UnMap(1);
  }

  void MapWithChildren(unsigned int layer);
  void UnMapWithChildren(unsigned int layer);

  void MapFromRoot(unsigned int layer);
  void UnMapFromRoot(unsigned int layer);

  RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func,
                          const void *arg, const bool ztest)
  {
    return world->Raytrace(LocalToGlobal(pose), range, func, this, arg, ztest);
  }

  void Raytrace(const Pose &pose, const meters_t range, const radians_t fov,
                const ray_test_func_t func, const void *arg, const bool ztest,
                std::vector<RaytraceResult> &results)
  {
    return world->Raytrace(LocalToGlobal(pose), range, fov, func, this, arg, ztest, results);
  }

  virtual void UpdateCharge();

  static int UpdateWrapper(Model *mod, void *)
  {
    mod->Update();
    return 0;
  }

  void CallUpdateCallbacks(void);

  meters_t ModelHeight() const;

  void DrawBlocksTree();
  virtual void DrawBlocks();
  void DrawBoundingBox();
  void DrawBoundingBoxTree();
  virtual void DrawStatus(Camera *cam);
  void DrawStatusTree(Camera *cam);

  void DrawOriginTree();
  void DrawOrigin();

  void PushLocalCoords();
  void PopCoords();

  void DrawImage(uint32_t texture_id, Camera *cam, float alpha, double width = 1.0,
                 double height = 1.0);

  virtual void DrawPicker();
  virtual void DataVisualize(Camera *cam);
  virtual void DrawSelected(void);

  void DrawTrailFootprint();
  void DrawTrailBlocks();
  void DrawTrailArrows();
  void DrawGrid();
  //    void DrawBlinkenlights();
  void DataVisualizeTree(Camera *cam);
  void DrawFlagList();
  void DrawPose(Pose pose);

public:
  virtual void PushColor(Color col) { world->PushColor(col); }
  virtual void PushColor(double r, double g, double b, double a) { world->PushColor(r, g, b, a); }
  virtual void PopColor() { world->PopColor(); }
  PowerPack *FindPowerPack() const;

  // void RecordRenderPoint( GSList** head, GSList* link,
  //                                    unsigned int* c1, unsigned int* c2 );

  bool PlaceInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax,
                        size_t max_iter = 0);

  bool RandomPoseInFreeSpace(meters_t xmin, meters_t xmax, meters_t ymin, meters_t ymax,
                             size_t max_iter = 0);

  std::string PoseString() { return pose.String(); }
  static Model *LookupId(uint32_t id) { return modelsbyid[id]; }
  Model(World *world, Model *parent = NULL, const std::string &type = "model",
        const std::string &name = "");

  virtual ~Model();

  Model()
      : mapped(false), alwayson(false), blockgroup(*this), boundary(false), data_fresh(false),
        disabled(true), friction(0), has_default_block(false), id(0), interval(0),
        interval_energy(0), last_update(0), log_state(false), map_resolution(0), mass(0),
        parent(NULL), power_pack(NULL), rebuild_displaylist(false), stack_children(true),
        stall(false), subs(0), thread_safe(false), trail_index(0), event_queue_num(0), used(false),
        watts(0), watts_give(0), watts_take(0), wf(NULL), wf_entity(0), world(NULL), world_gui(NULL)
  {
  }

  void Say(const std::string &str);

  void AddVisualizer(Visualizer *custom_visual, bool on_by_default);

  void RemoveVisualizer(Visualizer *custom_visual);

  void BecomeParentOf(Model *child);

  void Load(Worldfile *wf, int wf_entity)
  {
    SetWorldfile(wf, wf_entity);
    Load(); // call virtual load
  }

  void SetWorldfile(Worldfile *wf, int wf_entity)
  {
    this->wf = wf;
    this->wf_entity = wf_entity;
  }

  virtual void Load();

  virtual void Save();

  void InitControllers();

  void AddFlag(Flag *flag);
  void RemoveFlag(Flag *flag);

  void PushFlag(Flag *flag);
  Flag *PopFlag();

  unsigned int GetFlagCount() const { return flag_list.size(); }
  void Disable() { disabled = true; }
  void Enable() { disabled = false; }
  bool IsEnabled() const { return !disabled; }
  void LoadControllerModule(const char *lib);

  void NeedRedraw();

  void Redraw();

  void LoadBlock(Worldfile *wf, int entity);

  void AddBlockRect(meters_t x, meters_t y, meters_t dx, meters_t dy, meters_t dz);

  void ClearBlocks();

  Model *Parent() const { return this->parent; }
  World *GetWorld() const { return this->world; }
  Model *Root() { return (parent ? parent->Root() : this); }
  bool IsAntecedent(const Model *testmod) const;

  bool IsDescendent(const Model *testmod) const;

  bool IsRelated(const Model *testmod) const;

  Pose GetGlobalPose() const;

  void Subscribe();

  void Unsubscribe();

  void SetGlobalPose(const Pose &gpose);

  void SetPose(const Pose &pose);

  void AddToPose(const Pose &pose);

  void AddToPose(double dx, double dy, double dz, double da);

  void SetGeom(const Geom &src);

  void SetFiducialReturn(int fid);

  int GetFiducialReturn() const { return vis.fiducial_return; }
  void SetFiducialKey(int key);

  Color GetColor() const { return color; }
  uint32_t GetId() const { return id; }
  kg_t GetTotalMass() const;

  kg_t GetMassOfChildren() const;

  int SetParent(Model *newparent);

  Geom GetGeom() const { return geom; }
  Pose GetPose() const { return pose; }
  // guess what these do?
  void SetColor(Color col);
  void SetMass(kg_t mass);
  void SetStall(bool stall);
  void SetGravityReturn(bool val);
  void SetGripperReturn(bool val);
  void SetStickyReturn(bool val);
  void SetRangerReturn(double val);
  void SetObstacleReturn(bool val);
  void SetBlobReturn(bool val);
  void SetRangerReturn(bool val);
  void SetBoundary(bool val);
  void SetGuiNose(bool val);
  void SetGuiMove(bool val);
  void SetGuiGrid(bool val);
  void SetGuiOutline(bool val);
  void SetWatts(watts_t watts);
  void SetMapResolution(meters_t res);
  void SetFriction(double friction);

  bool DataIsFresh() const { return this->data_fresh; }
  void AddCallback(callback_type_t type, model_callback_t cb, void *user);

  int RemoveCallback(callback_type_t type, model_callback_t callback);

  int CallCallbacks(callback_type_t type);

  virtual void Print(char *prefix) const;
  virtual const char *PrintWithPose() const;

  Pose GlobalToLocal(const Pose &pose) const;

  Pose LocalToGlobal(const Pose &pose) const { return ((GetGlobalPose() + geom.pose) + pose); }
  std::vector<point_int_t> LocalToPixels(const std::vector<point_t> &local) const;

  point_t LocalToGlobal(const point_t &pt) const;

  Model *GetUnsubscribedModelOfType(const std::string &type) const;

  Model *GetUnusedModelOfType(const std::string &type);

  bool Stalled() const { return this->stall; }
  unsigned int GetSubscriptionCount() const { return subs; }
  bool HasSubscribers() const { return (subs > 0); }
  static std::map<std::string, creator_t> name_map;

protected:
  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
};

// BLOBFINDER MODEL --------------------------------------------------------
class ModelBlobfinder : public Model {
public:
  class Blob {
  public:
    Color color;
    uint32_t left, top, right, bottom;
    meters_t range;
  };

  class Vis : public Visualizer {
  public:
    explicit Vis(World *world);
    virtual ~Vis(void) {}
    virtual void Visualize(Model *mod, Camera *cam);
  } vis;

private:
  std::vector<Blob> blobs;

  std::vector<Color> colors;

  static bool BlockMatcher(Block *testblock, Model *finder);

public:
  radians_t fov; 
  radians_t pan; 
  meters_t range; 
  unsigned int scan_height; 
  unsigned int scan_width; 

  explicit ModelBlobfinder(World *world, Model *parent, const std::string &type);
  ~ModelBlobfinder();

  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
  virtual void Load();

  const std::vector<Blob> &GetBlobs() const { return blobs; }
  std::vector<Blob> &GetBlobsMutable() { return blobs; }
  void AddColor(Color col);

  void RemoveColor(Color col);

  void RemoveAllColors();
};

// Light indicator model
class ModelLightIndicator : public Model {
public:
  explicit ModelLightIndicator(World *world, Model *parent, const std::string &type);
  ~ModelLightIndicator();

  void SetState(bool isOn);

protected:
  virtual void DrawBlocks();

private:
  bool m_IsOn;
};

// \todo  GRIPPER MODEL --------------------------------------------------------

class ModelGripper : public Model {
public:
  enum paddle_state_t {
    PADDLE_OPEN = 0, 
    PADDLE_CLOSED,
    PADDLE_OPENING,
    PADDLE_CLOSING
  };

  enum lift_state_t {
    LIFT_DOWN = 0, 
    LIFT_UP,
    LIFT_UPPING, 
    LIFT_DOWNING
  };

  enum cmd_t {
    CMD_NOOP = 0, 
    CMD_OPEN,
    CMD_CLOSE,
    CMD_UP,
    CMD_DOWN
  };

  struct config_t {
    Size paddle_size; 
    paddle_state_t paddles;
    lift_state_t lift;
    double paddle_position; 
    double lift_position; 
    Model *gripped;
    bool paddles_stalled; 
    double
        close_limit; 
    bool autosnatch; 
    double break_beam_inset[2]; 
    Model *beam[2]; 
    Model *contact[2]; 
  };

private:
  virtual void Update();
  virtual void DataVisualize(Camera *cam);

  void FixBlocks();
  void PositionPaddles();
  void UpdateBreakBeams();
  void UpdateContacts();

  config_t cfg;
  cmd_t cmd;

  Block *paddle_left;
  Block *paddle_right;

  static Option showData;

public:
  static const Size size;

  ModelGripper(World *world, Model *parent, const std::string &type);
  virtual ~ModelGripper();

  virtual void Load();
  virtual void Save();

  void SetConfig(config_t &newcfg)
  {
    this->cfg = newcfg;
    FixBlocks();
  }

  config_t GetConfig() { return cfg; }
  void SetCommand(cmd_t cmd) { this->cmd = cmd; }
  void CommandClose() { SetCommand(CMD_CLOSE); }
  void CommandOpen() { SetCommand(CMD_OPEN); }
  void CommandUp() { SetCommand(CMD_UP); }
  void CommandDown() { SetCommand(CMD_DOWN); }
};

// BUMPER MODEL --------------------------------------------------------
class ModelBumper : public Model {
public:
  class BumperConfig {
  public:
    Pose pose;
    meters_t length;
  };

  class BumperSample {
  public:
    Model *hit;
    point_t hit_point;
  };

public:
  ModelBumper(World *world, Model *parent, const std::string &type);
  virtual ~ModelBumper();

  virtual void Load();

  uint32_t bumper_count;
  BumperConfig *bumpers;
  BumperSample *samples;

protected:
  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
  virtual void Print(char *prefix) const;

  class BumperVis : public Visualizer {
  public:
    BumperVis();
    virtual ~BumperVis();
    virtual void Visualize(Model *mod, Camera *cam);
  } bumpervis;

private:
  static Option showBumperData;
};

// FIDUCIAL MODEL --------------------------------------------------------

class ModelFiducial : public Model {
public:
  class Fiducial {
  public:
    meters_t range; 
    radians_t bearing; 
    Pose geom; 
    // Pose pose_rel; /// relative pose of the target in local coordinates
    Pose pose; 
    Model *mod; 
    int id; 
  };

private:
  void AddModelIfVisible(Model *him);

  virtual void Update();
  virtual void DataVisualize(Camera *cam);

  static Option showData;
  static Option showFov;

  std::vector<Fiducial> fiducials;

public:
  ModelFiducial(World *world, Model *parent, const std::string &type);
  virtual ~ModelFiducial();

  virtual void Load();
  void Shutdown(void);

  meters_t max_range_anon; 
  meters_t max_range_id; 
  meters_t min_range; 
  radians_t fov; 
  radians_t heading; 
  int key; 
  bool ignore_zloc; 

  std::vector<Fiducial> &GetFiducials() { return fiducials; }
  Fiducial *GetFiducials(unsigned int *count)
  {
    if (count)
      *count = fiducials.size();
    return &fiducials[0];
  }
};

// RANGER MODEL --------------------------------------------------------

class ModelRanger : public Model {
public:
public:
  ModelRanger(World *world, Model *parent, const std::string &type);
  virtual ~ModelRanger();

  virtual void Print(char *prefix) const;

  class Vis : public Visualizer {
  public:
    static Option showArea;
    static Option showStrikes;
    static Option showFov;
    static Option showBeams;
    static Option showTransducers;

    explicit Vis(World *world);
    virtual ~Vis(void) {}
    virtual void Visualize(Model *mod, Camera *cam);
  } vis;

  class Sensor {
  public:
    Pose pose;
    Size size;
    Bounds range;
    radians_t fov;
    double angle_noise; //< variance for ranger angle
    double range_noise; //< variance for range readings
    double range_noise_const; //< variance for constant noise (not depending on range)
    unsigned int sample_count;
    Color color;

    std::vector<meters_t> ranges;
    std::vector<double> intensities;
    std::vector<double> bearings;

    Sensor()
        : pose(0, 0, 0, 0), size(0.02, 0.02, 0.02), // teeny transducer
          range(0.0, 5.0), fov(0.1), angle_noise(0.0), range_noise(0.0), range_noise_const(0.0),
          sample_count(1), color(Color(0, 0, 1, 0.15)), ranges(), intensities(), bearings()
    {
    }

    void Update(ModelRanger *rgr);
    void Visualize(Vis *vis, ModelRanger *rgr) const;
    std::string String() const;
    void Load(Worldfile *wf, int entity);
  };

  const std::vector<Sensor> &GetSensors() const { return sensors; }
  std::vector<Sensor> &GetSensorsMutable() { return sensors; }
  void LoadSensor(Worldfile *wf, int entity);

private:
  std::vector<Sensor> sensors;

protected:
  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
};

// BLINKENLIGHT MODEL ----------------------------------------------------
class ModelBlinkenlight : public Model {
private:
  double dutycycle;
  bool enabled;
  msec_t period;
  bool on;

  static Option showBlinkenData;

public:
  ModelBlinkenlight(World *world, Model *parent, const std::string &type);

  ~ModelBlinkenlight();

  virtual void Load();
  virtual void Update();
  virtual void DataVisualize(Camera *cam);
};

// CAMERA MODEL ----------------------------------------------------

class ModelCamera : public Model {
public:
  typedef struct {
    // GL_V3F
    GLfloat x, y, z;
  } ColoredVertex;

private:
  Canvas *_canvas;

  GLfloat *_frame_data; // opengl read buffer
  GLubyte *_frame_color_data; // opengl read buffer

  bool _valid_vertexbuf_cache;
  ColoredVertex *_vertexbuf_cache; // cached unit vectors with appropriate rotations (these must be
  // scalled by z-buffer length)

  int _width; // width of buffer
  int _height; // height of buffer
  static const int _depth = 4;

  int _camera_quads_size;
  GLfloat *_camera_quads;
  GLubyte *_camera_colors;

  static Option showCameraData;

  PerspectiveCamera _camera;
  double _yaw_offset; // position camera is mounted at
  double _pitch_offset;

  bool GetFrame();

public:
  ModelCamera(World *world, Model *parent, const std::string &type);

  ~ModelCamera();

  virtual void Load();

  virtual void Update();

  // virtual void Draw( uint32_t flags, Canvas* canvas );

  virtual void DataVisualize(Camera *cam);

  int getWidth(void) const { return _width; }
  int getHeight(void) const { return _height; }
  const PerspectiveCamera &getCamera(void) const { return _camera; }
  const GLfloat *FrameDepth() const { return _frame_data; }
  const GLubyte *FrameColor() const { return _frame_color_data; }
  void setPitch(double pitch)
  {
    _pitch_offset = pitch;
    _valid_vertexbuf_cache = false;
  }

  void setYaw(double yaw)
  {
    _yaw_offset = yaw;
    _valid_vertexbuf_cache = false;
  }
};

// POSITION MODEL --------------------------------------------------------

class ModelPosition : public Model {
  friend class Canvas;
  friend class World;

public:
  typedef enum { CONTROL_ACCELERATION, CONTROL_VELOCITY, CONTROL_POSITION } ControlMode;

  typedef enum { LOCALIZATION_GPS, LOCALIZATION_ODOM } LocalizationMode;

  typedef enum { DRIVE_DIFFERENTIAL, DRIVE_OMNI, DRIVE_CAR } DriveMode;

private:
  Velocity velocity;
  Pose goal; 
  ControlMode control_mode;
  DriveMode drive_mode;
  LocalizationMode localization_mode; 
  Velocity integration_error; 
  double wheelbase;

public:
  Bounds acceleration_bounds[4];

  Bounds velocity_bounds[4];

  ModelPosition(World *world, Model *parent, const std::string &type);
  ~ModelPosition();

  Velocity GetVelocity() const { return velocity; }
  void SetVelocity(const Velocity &val);
  Velocity GetGlobalVelocity() const;
  void SetGlobalVelocity(const Velocity &gvel);

  Velocity GetOdomError() const { return integration_error; }
  class Waypoint {
  public:
    Waypoint(meters_t x, meters_t y, meters_t z, radians_t a, Color color);
    Waypoint(const Pose &pose, Color color);
    Waypoint();
    void Draw() const;

    Pose pose;
    Color color;
  };

  std::vector<Waypoint> waypoints;

  class WaypointVis : public Visualizer {
  public:
    WaypointVis();
    virtual ~WaypointVis(void) {}
    virtual void Visualize(Model *mod, Camera *cam);
  } wpvis;

  class PoseVis : public Visualizer {
  public:
    PoseVis();
    virtual ~PoseVis(void) {}
    virtual void Visualize(Model *mod, Camera *cam);
  } posevis;

  void SetOdom(Pose odom);

  void SetSpeed(double x, double y, double a);
  void SetXSpeed(double x);
  void SetYSpeed(double y);
  void SetZSpeed(double z);
  void SetTurnSpeed(double a);
  void SetSpeed(Velocity vel);
  void Stop();

  void GoTo(double x, double y, double a);
  void GoTo(Pose pose);

  void SetAcceleration(double x, double y, double a);

  // localization state
  Pose est_pose; //<! position estimate in local coordinates
  Pose est_pose_error; //<! estimated error in position estimate
  Pose est_origin; //<! global origin of the local coordinate system

protected:
  virtual void Move();
  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
  virtual void Load();
};

// ACTUATOR MODEL --------------------------------------------------------

class ModelActuator : public Model {
public:
  typedef enum { CONTROL_VELOCITY, CONTROL_POSITION } ControlMode;

  typedef enum { TYPE_LINEAR, TYPE_ROTATIONAL } ActuatorType;

private:
  double goal; //< the current velocity or pose to reach depending on the value of control_mode
  double pos;
  double max_speed;
  double min_position;
  double max_position;
  double start_position;
  double cosa;
  double sina;
  ControlMode control_mode;
  ActuatorType actuator_type;
  point3_t axis;

  Pose InitialPose;

public:
  ModelActuator(World *world, Model *parent, const std::string &type);
  ~ModelActuator();

  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
  virtual void Load();

  void SetSpeed(double speed);

  double GetSpeed() const { return goal; }
  void GoTo(double pose);

  double GetPosition() const { return pos; }
  double GetMaxPosition() const { return max_position; }
  double GetMinPosition() const { return min_position; }
  ActuatorType GetType() const { return actuator_type; }
  point3_t GetAxis() const { return axis; }
};

} // end namespace stg

#endif