From 34f3cb4c0049025752b62ea24b71bd6f767c9500 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Sat, 11 Nov 2017 23:33:24 +0300
Subject: [PATCH 01/22] Removing FLTK usage from libstage

I've separated gui-specific code to libstage-gui library, so base simulation does not need fltk anymore.

Now stage compiles two executables: stage and stage-headless. Headless variant does not needs FLTK

Fixed most of compiler errors. Linker to be fixed
---
 libstage/CMakeLists.txt     |  39 ++--
 libstage/canvas.cc          | 362 +++---------------------------------
 libstage/canvas.hh          |  24 ++-
 libstage/canvas_fltk.cc     | 336 +++++++++++++++++++++++++++++++++
 libstage/canvas_fltk.hh     |  52 ++++++
 libstage/gl.cc              |  11 +-
 libstage/image.cc           |  47 +++++
 libstage/image.hh           |  39 ++++
 libstage/main.cc            |   2 +
 libstage/main_headless.cc   | 111 +++++++++++
 libstage/model.cc           |   8 +-
 libstage/model_camera.cc    |  26 +--
 libstage/model_draw.cc      |  19 +-
 libstage/option.cc          |  28 +--
 libstage/option.hh          |  10 +-
 libstage/stage.cc           |  29 ++-
 libstage/stage.hh           | 147 +++------------
 libstage/texture_manager.cc |   2 +
 libstage/vis_strip.cc       |   5 +-
 libstage/world.cc           |  37 +++-
 libstage/world_gui.hh       | 130 +++++++++++++
 libstage/worldgui.cc        |  39 +---
 22 files changed, 916 insertions(+), 587 deletions(-)
 create mode 100644 libstage/canvas_fltk.cc
 create mode 100644 libstage/canvas_fltk.hh
 create mode 100644 libstage/image.cc
 create mode 100644 libstage/image.hh
 create mode 100644 libstage/main_headless.cc
 create mode 100644 libstage/world_gui.hh

diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index c25f0b77d..2c23329a0 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -31,33 +31,33 @@ set( stageSrcs
 	stage.cc
 	stage.hh
 	texture_manager.cc
-	typetable.cc		
-	world.cc			
-	worldfile.cc		
-	canvas.cc 
-	options_dlg.cc
-	options_dlg.hh
-	vis_strip.cc			
-	worldgui.cc 
+	typetable.cc
+	world.cc
+	worldfile.cc
+	canvas.cc
+	vis_strip.cc
 	ancestor.cc
+	image.cc
 )
-
-#	model_getset.cc
-#	model_load.cc
+#	worldgui.cc 
 
 #set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS" )
  
 add_library(stage SHARED ${stageSrcs})
 
+# GUI part of the stage
+add_library(stage_gui SHARED canvas_fltk.cc options_dlg.cc worldgui.cc)
+
 # if fltk-config didn't bring along the OpenGL dependencies (eg. on
 # Debian/Ubuntu), add them explicity 
 IF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
-  target_link_libraries( stage ${OPENGL_LIBRARIES}) 
+  target_link_libraries( stage ${OPENGL_LIBRARIES})
+  #target_link_libraries( stage_gui ${OPENGL_LIBRARIES}) 
 ENDIF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
 
 
 # causes the shared library to have a version number
-set_target_properties( stage PROPERTIES
+set_target_properties( stage_gui PROPERTIES
 		       VERSION ${VERSION}
 #           LINK_FLAGS "${FLTK_LDFLAGS}"  
 )
@@ -66,9 +66,10 @@ target_link_libraries( stage
                        ${LTDL_LIB} 
                        ${JPEG_LIBRARIES} 
                        ${PNG_LIBRARIES}
-                       ${FLTK_LIBRARIES}
 )
 
+target_link_libraries( stage_gui stage ${FLTK_LIBRARIES})
+
 set( stagebinarySrcs main.cc )
 set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
 
@@ -83,21 +84,23 @@ set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK
 # add_executable( stagebinary MACOSX_BUNDLE ${stagebinarySrcs} )
 
 add_executable( stagebinary ${stagebinarySrcs} )
-
-
 set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage )
 
 # Apple seems to have trouble when libstage and stagebinary are linked against FLTK
 # Newer Linux distributions won't allow stagebinary to inherit libstage's links to fltk, so we need
 # to explicitly link on Linux
+target_link_libraries( stagebinary stage stage_gui )
 
-target_link_libraries( stagebinary stage )
+# Headless version
+add_executable( stagebinary_headless main_headless.cc )
+set_target_properties( stagebinary_headless PROPERTIES OUTPUT_NAME stage_headless )
+target_link_libraries( stagebinary_headless stage )
 
 IF(PROJECT_OS_LINUX)
   target_link_libraries( stagebinary stage pthread )
 ENDIF(PROJECT_OS_LINUX)
 
-INSTALL(TARGETS stagebinary stage
+INSTALL(TARGETS stagebinary stagebinary_headless stage stage_gui 
 	RUNTIME DESTINATION bin
 	LIBRARY DESTINATION ${PROJECT_LIB_DIR}
 )
diff --git a/libstage/canvas.cc b/libstage/canvas.cc
index e0bf52e63..8a4b9f12d 100644
--- a/libstage/canvas.cc
+++ b/libstage/canvas.cc
@@ -39,19 +39,8 @@ static bool texture_load_done = false;
 // GLuint glowTex;
 GLuint checkTex;
 
-void Canvas::TimerCallback(Canvas *c)
-{
-  if (c->world->dirty) {
-    // puts( "timer redraw" );
-    c->redraw();
-    c->world->dirty = false;
-  }
-
-  Fl::repeat_timeout(c->interval / 1000.0, (Fl_Timeout_Handler)Canvas::TimerCallback, c);
-}
-
-Canvas::Canvas(WorldGui *world, int x, int y, int width, int height)
-    : Fl_Gl_Window(x, y, width, height), colorstack(), models_sorted(), current_camera(NULL),
+Canvas::Canvas(World *world, int x, int y, int width, int height)
+    : colorstack(), models_sorted(), current_camera(NULL),
       camera(), perspective_camera(), dirty_buffer(false), wf(NULL), startx(-1), starty(-1),
       selected_models(), last_selection(NULL), interval(40), // msec between redraws
       // initialize Option objects
@@ -77,13 +66,6 @@ Canvas::Canvas(WorldGui *world, int x, int y, int width, int height)
       // and the rest
       graphics(true), world(world), frames_rendered_count(0), screenshot_frame_skip(1)
 {
-  end();
-  // show(); // must do this so that the GL context is created before
-  // configuring GL
-  // but that line causes a segfault in Linux/X11! TODO: test in OS X
-
-  mode( FL_RGB |FL_DOUBLE|FL_DEPTH| FL_MULTISAMPLE | FL_ALPHA );
-  
   perspective_camera.setPose(0.0, -4.0, 3.0);
   current_camera = &camera;
   setDirtyBuffer();
@@ -95,8 +77,8 @@ Canvas::Canvas(WorldGui *world, int x, int y, int width, int height)
 
 void Canvas::InitGl()
 {
-  valid(1);
-  FixViewport(w(), h());
+  //valid(1);
+  FixViewport(getWidth(), getHeight());
 
   // set gl state that won't change every redraw
   glClearColor(0.7, 0.7, 0.8, 1.0);
@@ -117,7 +99,9 @@ void Canvas::InitGl()
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
   // install a font
+#ifdef FIX_LATER
   gl_font(FL_HELVETICA, 12);
+#endif
 
   blur = false;
 
@@ -203,7 +187,8 @@ Canvas::~Canvas()
 Model *Canvas::getModel(int x, int y)
 {
   // render all models in a unique color
-  make_current(); // make sure the GL context is current
+	// Why we are calling make_current here? This should be done on upper level
+  //make_current(); // make sure the GL context is current
   glClearColor(1, 1, 1, 1); // white
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   glLoadIdentity();
@@ -285,7 +270,7 @@ void Canvas::select(Model *mod)
     selected_models.push_front(mod);
 
     //		mod->Disable();
-    redraw();
+    doRedraw();
   }
 }
 
@@ -293,7 +278,7 @@ void Canvas::unSelect(Model *mod)
 {
   if (mod) {
     EraseAll(mod, selected_models);
-    redraw();
+    doRedraw();
   }
 }
 
@@ -307,12 +292,12 @@ void Canvas::CanvasToWorld(int px, int py, double *wx, double *wy, double *wz)
 {
   if (px <= 0)
     px = 1;
-  else if (px >= w())
-    px = w() - 1;
+  else if (px >= getWidth())
+    px = getWidth() - 1;
   if (py <= 0)
     py = 1;
-  else if (py >= h())
-    py = h() - 1;
+  else if (py >= getHeight())
+    py = getHeight() - 1;
 
   // redraw the screen only if the camera model isn't active.
   // TODO new selection technique will simply use drawfloor to result in z = 0
@@ -338,240 +323,8 @@ void Canvas::CanvasToWorld(int px, int py, double *wx, double *wy, double *wz)
   glGetDoublev(GL_PROJECTION_MATRIX, projection);
 
   GLfloat pz;
-  glReadPixels(px, h() - py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz);
-  gluUnProject(px, w() - py, pz, modelview, projection, viewport, wx, wy, wz);
-}
-
-int Canvas::handle(int event)
-{
-  // printf( "cam %.2f %.2f\n", camera.yaw(), camera.pitch() );
-
-  switch (event) {
-  case FL_MOUSEWHEEL:
-    if (pCamOn == true) {
-      perspective_camera.scroll(Fl::event_dy() / 10.0);
-    } else {
-      camera.scale(Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h());
-    }
-    invalidate();
-    redraw();
-    return 1;
-
-  case FL_MOVE: // moused moved while no button was pressed
-    if (Fl::event_state(FL_META)) {
-      puts("TODO: HANDLE HISTORY");
-      // world->paused = ! world->paused;
-      return 1;
-    }
-
-    if (startx >= 0) {
-      // mouse pointing to valid value
-
-      if (Fl::event_state(FL_CTRL)) // rotate the camera view
-      {
-        int dx = Fl::event_x() - startx;
-        int dy = Fl::event_y() - starty;
-
-        if (pCamOn == true) {
-          perspective_camera.addYaw(-dx);
-          perspective_camera.addPitch(-dy);
-        } else {
-          camera.addPitch(-0.5 * static_cast<double>(dy));
-          camera.addYaw(-0.5 * static_cast<double>(dx));
-        }
-        invalidate();
-        redraw();
-      } else if (Fl::event_state(FL_ALT)) {
-        int dx = Fl::event_x() - startx;
-        int dy = Fl::event_y() - starty;
-
-        if (pCamOn == true) {
-          perspective_camera.move(-dx, dy, 0.0);
-        } else {
-          camera.move(-dx, dy);
-        }
-        invalidate();
-      }
-    }
-    startx = Fl::event_x();
-    starty = Fl::event_y();
-    return 1;
-  case FL_PUSH: // button pressed
-  {
-    // else
-    {
-      Model *mod = getModel(startx, starty);
-      startx = Fl::event_x();
-      starty = Fl::event_y();
-      selectedModel = false;
-      switch (Fl::event_button()) {
-      case 1:
-        clicked_empty_space = (mod == NULL);
-        empty_space_startx = startx;
-        empty_space_starty = starty;
-        if (mod) {
-          // clicked a model
-          if (Fl::event_state(FL_SHIFT)) {
-            // holding shift, toggle selection
-            if (selected(mod))
-              unSelect(mod);
-            else {
-              select(mod);
-              selectedModel = true; // selected a model
-            }
-          } else {
-            if (!selected(mod)) {
-              // clicked on an unselected model while
-              //  not holding shift, this is the new
-              //  selection
-              unSelectAll();
-              select(mod);
-            }
-            selectedModel = true; // selected a model
-          }
-        }
-
-        redraw(); // probably required
-        return 1;
-      case 3: {
-        // leave selections alone
-        // rotating handled within FL_DRAG
-        return 1;
-      }
-      default: return 0;
-      }
-    }
-  }
-
-  case FL_DRAG: // mouse moved while button was pressed
-  {
-    int dx = Fl::event_x() - startx;
-    int dy = Fl::event_y() - starty;
-
-    if (Fl::event_state(FL_BUTTON1) && Fl::event_state(FL_CTRL) == false) {
-      // Left mouse button drag
-      if (selectedModel) {
-        // started dragging on a selected model
-
-        double sx, sy, sz;
-        CanvasToWorld(startx, starty, &sx, &sy, &sz);
-        double x, y, z;
-        CanvasToWorld(Fl::event_x(), Fl::event_y(), &x, &y, &z);
-        // move all selected models to the mouse pointer
-        FOR_EACH (it, selected_models) {
-          Model *mod = *it;
-          mod->AddToPose(x - sx, y - sy, 0, 0);
-        }
-      } else {
-        // started dragging on empty space or an
-        //  unselected model, move the canvas
-        if (pCamOn == true) {
-          perspective_camera.move(-dx, dy, 0.0);
-        } else {
-          camera.move(-dx, dy);
-        }
-        invalidate(); // so the projection gets updated
-      }
-    } else if (Fl::event_state(FL_BUTTON3)
-               || (Fl::event_state(FL_BUTTON1) && Fl::event_state(FL_CTRL))) {
-      // rotate all selected models
-
-      if (selected_models.size()) {
-        FOR_EACH (it, selected_models) {
-          Model *mod = *it;
-          mod->AddToPose(0, 0, 0, 0.05 * (dx + dy));
-        }
-      } else {
-        // printf( "button 2\n" );
-
-        int dx = Fl::event_x() - startx;
-        int dy = Fl::event_y() - starty;
-
-        if (pCamOn == true) {
-          perspective_camera.addYaw(-dx);
-          perspective_camera.addPitch(-dy);
-        } else {
-          camera.addPitch(-0.5 * static_cast<double>(dy));
-          camera.addYaw(-0.5 * static_cast<double>(dx));
-        }
-      }
-      invalidate();
-      redraw();
-    }
-
-    startx = Fl::event_x();
-    starty = Fl::event_y();
-
-    redraw();
-    return 1;
-  } // end case FL_DRAG
-
-  case FL_RELEASE: // mouse button released
-    if (empty_space_startx == Fl::event_x() && empty_space_starty == Fl::event_y()
-        && clicked_empty_space == true) {
-      // clicked on empty space, unselect all
-      unSelectAll();
-      redraw();
-    }
-    return 1;
-
-  case FL_FOCUS:
-  case FL_UNFOCUS:
-    //.... Return 1 if you want keyboard events, 0 otherwise
-    return 1;
-
-  case FL_KEYBOARD:
-    switch (Fl::event_key()) {
-    case FL_Left:
-      if (pCamOn == false) {
-        camera.move(-10, 0);
-      } else {
-        perspective_camera.strafe(-0.5);
-      }
-      break;
-    case FL_Right:
-      if (pCamOn == false) {
-        camera.move(10, 0);
-      } else {
-        perspective_camera.strafe(0.5);
-      }
-      break;
-    case FL_Down:
-      if (pCamOn == false) {
-        camera.move(0, -10);
-      } else {
-        perspective_camera.forward(-0.5);
-      }
-      break;
-    case FL_Up:
-      if (pCamOn == false) {
-        camera.move(0, 10);
-      } else {
-        perspective_camera.forward(0.5);
-      }
-      break;
-    default:
-      redraw(); // we probably set a display config - so need this
-      return 0; // keypress unhandled
-    }
-
-    invalidate(); // update projection
-    return 1;
-
-  //	case FL_SHORTCUT:
-  //		//... shortcut, key is in Fl::event_key(), ascii in
-  // Fl::event_text()
-  //		//... Return 1 if you understand/use the shortcut event, 0
-  // otherwise...
-  //		return 1;
-  default:
-    // pass other events to the base class...
-    // printf( "EVENT %d\n", event );
-    return Fl_Gl_Window::handle(event);
-
-  } // end switch( event )
-
-  return 0;
+  glReadPixels(px, getHeight() - py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz);
+  gluUnProject(px, getWidth() - py, pz, modelview, projection, viewport, wx, wy, wz);
 }
 
 void Canvas::FixViewport(int W, int H)
@@ -583,7 +336,7 @@ void Canvas::FixViewport(int W, int H)
 void Canvas::AddModel(Model *mod)
 {
   models_sorted.push_back(mod);
-  redraw();
+  doRedraw();
 }
 
 void Canvas::RemoveModel(Model *mod)
@@ -694,8 +447,9 @@ void Canvas::DrawBoundingBoxes()
   glLineWidth(2.0);
   glPointSize(5.0);
   //glDisable(GL_CULL_FACE);
-
-  world->DrawBoundingBoxTree();
+  const std::vector<Model *> & children = world->GetChildren();
+  FOR_EACH (it, children)
+      (*it)->DrawBoundingBoxTree();
 
   //glEnable(GL_CULL_FACE);
   glLineWidth(1.0);
@@ -731,8 +485,8 @@ void Canvas::resetCamera()
   float x = (min_x + max_x) / 2.0;
   float y = (min_y + max_y) / 2.0;
   camera.setPose(x, y);
-  float scale_x = w() / (max_x - min_x) * 0.9;
-  float scale_y = h() / (max_y - min_y) * 0.9;
+  float scale_x = getWidth() / (max_x - min_x) * 0.9;
+  float scale_y = getHeight() / (max_y - min_y) * 0.9;
   camera.setScale(scale_x < scale_y ? scale_x : scale_y);
 
   // TODO reset perspective cam
@@ -974,7 +728,7 @@ void Canvas::renderFrame()
     glMatrixMode(GL_PROJECTION);
     glPushMatrix(); // save old projection
     glLoadIdentity();
-    glOrtho(0, w(), 0, h(), -100, 100);
+    glOrtho(0, getWidth(), 0, getHeight(), -100, 100);
     glMatrixMode(GL_MODELVIEW);
 
     glPushMatrix();
@@ -985,10 +739,10 @@ void Canvas::renderFrame()
     if (showFollow == true && last_selection)
       clockstr.append(" [FOLLOW MODE]");
 
-    float txtWidth = gl_width(clockstr.c_str());
+    float txtWidth = Gl::gl_width(clockstr.c_str());
     if (txtWidth < 200)
       txtWidth = 200;
-    int txtHeight = gl_height();
+    int txtHeight = Gl::gl_height();
 
     const int margin = 5;
     int width, height;
@@ -1039,7 +793,7 @@ void Canvas::EnterScreenCS()
   glMatrixMode(GL_PROJECTION);
   glPushMatrix(); // save old projection
   glLoadIdentity();
-  glOrtho(0, w(), 0, h(), -100, 100);
+  glOrtho(0, getWidth(), 0, getHeight(), -100, 100);
   glMatrixMode(GL_MODELVIEW);
 
   glPushMatrix();
@@ -1058,8 +812,8 @@ void Canvas::LeaveScreenCS()
 
 void Canvas::Screenshot()
 {
-  int width = w();
-  int height = h();
+  int width = getWidth();
+  int height = getHeight();
   int depth = 4; // RGBA
 
   // we use RGBA throughout, though we only need RGB, as the 4-byte
@@ -1113,43 +867,6 @@ void Canvas::Screenshot()
   delete[] rowpointers;
 }
 
-void Canvas::perspectiveCb(Fl_Widget *w, void *p)
-{
-  Canvas *canvas = static_cast<Canvas *>(w);
-  Option *opt = static_cast<Option *>(p); // pCamOn
-  if (opt) {
-    // Perspective mode is on, change camera
-    canvas->current_camera = &canvas->perspective_camera;
-  } else {
-    canvas->current_camera = &canvas->camera;
-  }
-
-  canvas->invalidate();
-}
-
-void Canvas::createMenuItems(Fl_Menu_Bar *menu, std::string path)
-{
-  showData.createMenuItem(menu, path);
-  //  visualizeAll.createMenuItem( menu, path );
-  showBlocks.createMenuItem(menu, path);
-  showFlags.createMenuItem(menu, path);
-  showClock.createMenuItem(menu, path);
-  showFlags.createMenuItem(menu, path);
-  showFollow.createMenuItem(menu, path);
-  showFootprints.createMenuItem(menu, path);
-  showGrid.createMenuItem(menu, path);
-  showStatus.createMenuItem(menu, path);
-  pCamOn.createMenuItem(menu, path);
-  pCamOn.menuCallback(perspectiveCb, this);
-  showOccupancy.createMenuItem(menu, path);
-  showTrailArrows.createMenuItem(menu, path);
-  showTrails.createMenuItem(menu, path);
-  showTrailRise.createMenuItem(menu, path); // broken
-  showBBoxes.createMenuItem(menu, path);
-  // showVoxels.createMenuItem( menu, path );
-  showScreenshots.createMenuItem(menu, path);
-}
-
 void Canvas::Load(Worldfile *wf, int sec)
 {
   this->wf = wf;
@@ -1178,12 +895,6 @@ void Canvas::Load(Worldfile *wf, int sec)
   // showVoxels.Load( wf, sec );
   showScreenshots.Load(wf, sec);
   pCamOn.Load(wf, sec);
-
-  if (!world->paused)
-    // // start the timer that causes regular redraws
-    Fl::add_timeout(((double)interval / 1000), (Fl_Timeout_Handler)Canvas::TimerCallback, this);
-
-  invalidate(); // we probably changed something
 }
 
 void Canvas::Save(Worldfile *wf, int sec)
@@ -1217,19 +928,19 @@ void Canvas::draw()
   //	if( loaded_texture == true && pCamOn == true )
   //		return;
 
-  if (!valid()) {
+  if (!isValid()) {
     if (!init_done)
       InitGl();
     if (!texture_load_done)
       InitTextures();
 
     if (pCamOn == true) {
-      perspective_camera.setAspect(static_cast<float>(w()) / static_cast<float>(h()));
+      perspective_camera.setAspect(static_cast<float>(getWidth()) / static_cast<float>(getHeight()));
       perspective_camera.SetProjection();
       current_camera = &perspective_camera;
     } else {
       bounds3d_t extent = world->GetExtent();
-      camera.SetProjection(w(), h(), extent.y.min, extent.y.max);
+      camera.SetProjection(getWidth(), getHeight(), extent.y.min, extent.y.max);
       current_camera = &camera;
     }
 
@@ -1251,14 +962,3 @@ void Canvas::draw()
   renderFrame();
 }
 
-void Canvas::resize(int X, int Y, int W, int H)
-{
-  Fl_Gl_Window::resize(X, Y, W, H);
-
-  if (!init_done) // hack - should capture a create event to do this rather
-    // thann have it in multiple places
-    InitGl();
-
-  FixViewport(W, H);
-  invalidate();
-}
diff --git a/libstage/canvas.hh b/libstage/canvas.hh
index f92614277..100d5ddac 100644
--- a/libstage/canvas.hh
+++ b/libstage/canvas.hh
@@ -1,3 +1,5 @@
+#ifndef _CANVAS_HH_
+#define _CANVAS_HH_
 
 #include "option.hh"
 #include "stage.hh"
@@ -6,7 +8,8 @@
 #include <stack>
 
 namespace Stg {
-class Canvas : public Fl_Gl_Window {
+
+class Canvas {
   friend class WorldGui; // allow access to private members
   friend class Model;
 
@@ -72,11 +75,11 @@ private:
       showVoxels, pCamOn, visualizeAll;
 
 public:
-  Canvas(WorldGui *world, int x, int y, int width, int height);
-  ~Canvas();
+  Canvas(World *world, int x, int y, int width, int height);
+  virtual ~Canvas();
 
   bool graphics;
-  WorldGui *world;
+  World *world;
   unsigned long frames_rendered_count;
   int screenshot_frame_skip;
 
@@ -85,7 +88,6 @@ public:
   void Screenshot();
   void InitGl();
   void InitTextures();
-  void createMenuItems(Fl_Menu_Bar *menu, std::string path);
 
   void FixViewport(int W, int H);
   void DrawFloor(); // simpler floor compared to grid
@@ -94,8 +96,12 @@ public:
   void resetCamera();
   virtual void renderFrame();
   virtual void draw();
-  virtual int handle(int event);
-  void resize(int X, int Y, int W, int H);
+
+  virtual bool isValid() {return false;}
+  virtual void doRedraw() {}
+  virtual int getWidth() const {return 1;}
+  virtual int getHeight() const {return 1;}
+  virtual void setInvalidate() {}
 
   void CanvasToWorld(int px, int py, double *wx, double *wy, double *wz);
 
@@ -114,7 +120,7 @@ public:
 
   bool VisualizeAll() { return !visualizeAll; }
   static void TimerCallback(Canvas *canvas);
-  static void perspectiveCb(Fl_Widget *w, void *p);
+  //static void perspectiveCb(Fl_Widget *w, void *p);
 
   void EnterScreenCS();
   void LeaveScreenCS();
@@ -126,3 +132,5 @@ public:
 };
 
 } // namespace Stg
+
+#endif
diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
new file mode 100644
index 000000000..b3193b92f
--- /dev/null
+++ b/libstage/canvas_fltk.cc
@@ -0,0 +1,336 @@
+#include "canvas_fltk.hh"
+
+namespace Stg
+{
+
+//Fl_Gl_Window(x, y, width, height),
+
+CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
+:Canvas(world, x, y, width, height), screenshot_frame_skip(1)
+{
+  end();
+  // show(); // must do this so that the GL context is created before
+  // configuring GL
+  // but that line causes a segfault in Linux/X11! TODO: test in OS X
+  mode( FL_RGB |FL_DOUBLE|FL_DEPTH| FL_MULTISAMPLE | FL_ALPHA );
+}
+
+void CanvasFLTK::perspectiveCb(Option *opt, void *p)
+{
+  Canvas *canvas = static_cast<Canvas *>(p);
+  if (opt) {
+    // Perspective mode is on, change camera
+    canvas->current_camera = &canvas->perspective_camera;
+  } else {
+    canvas->current_camera = &canvas->camera;
+  }
+
+  canvas->invalidate();
+}
+
+void CanvasFLTK::createMenuItems(Fl_Menu_Bar *menu, std::string path)
+{
+	createMenuItem(showData.menu, path);
+  // createMenuItem(visualizeAll, menu, path );
+	createMenuItem(showBlocks,menu, path);
+	createMenuItem(showFlags, menu, path);
+	createMenuItem(showClock, menu, path);
+	createMenuItem(showFlags, menu, path);
+	createMenuItem(showFollow, menu, path);
+	createMenuItem(showFootprints, menu, path);
+	createMenuItem(showGrid, menu, path);
+	createMenuItem(showStatus, menu, path);
+	createMenuItem(pCamOn, menu, path);
+	pCamOn.setOptionCallback(perspectiveCb, this);
+	createMenuItem(showOccupancy, menu, path);
+	createMenuItem(showTrailArrows, menu, path);
+	createMenuItem(showTrails, menu, path);
+	createMenuItem(showTrailRise, menu, path); // broken
+	createMenuItem(showBBoxes, menu, path);
+	// createMenuItem(showVoxels, menu, path );
+	createMenuItem(showScreenshots, menu, path);
+}
+
+void CanvasFLTK::toggleCb(Fl_Widget *, void *p)
+{
+  // Fl_Menu_* menu = static_cast<Fl_Menu_*>( w );
+  Option *opt = static_cast<Option *>(p);
+  opt->invert();
+  if (opt->menuCb)
+    opt->menuCb(opt, opt->callbackData);
+}
+
+void CanvasFLTK::createMenuItem(Option * opt, Fl_Menu_Bar *m, std::string path)
+{
+  opt->menu = m;
+  path = path + "/" + optName;
+  // create a menu item and save its index
+  opt->menuIndex = menu->add(path.c_str(), shortcut.c_str(), toggleCb, opt, FL_MENU_TOGGLE | (value ? FL_MENU_VALUE : 0));
+}
+
+
+int CanvasFLTK::handle(int event)
+{
+  // printf( "cam %.2f %.2f\n", camera.yaw(), camera.pitch() );
+
+  switch (event) {
+  case FL_MOUSEWHEEL:
+    if (pCamOn == true) {
+      perspective_camera.scroll(Fl::event_dy() / 10.0);
+    } else {
+      camera.scale(Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h());
+    }
+    invalidate();
+    redraw();
+    return 1;
+
+  case FL_MOVE: // moused moved while no button was pressed
+    if (Fl::event_state(FL_META)) {
+      puts("TODO: HANDLE HISTORY");
+      // world->paused = ! world->paused;
+      return 1;
+    }
+
+    if (startx >= 0) {
+      // mouse pointing to valid value
+
+      if (Fl::event_state(FL_CTRL)) // rotate the camera view
+      {
+        int dx = Fl::event_x() - startx;
+        int dy = Fl::event_y() - starty;
+
+        if (pCamOn == true) {
+          perspective_camera.addYaw(-dx);
+          perspective_camera.addPitch(-dy);
+        } else {
+          camera.addPitch(-0.5 * static_cast<double>(dy));
+          camera.addYaw(-0.5 * static_cast<double>(dx));
+        }
+        invalidate();
+        redraw();
+      } else if (Fl::event_state(FL_ALT)) {
+        int dx = Fl::event_x() - startx;
+        int dy = Fl::event_y() - starty;
+
+        if (pCamOn == true) {
+          perspective_camera.move(-dx, dy, 0.0);
+        } else {
+          camera.move(-dx, dy);
+        }
+        invalidate();
+      }
+    }
+    startx = Fl::event_x();
+    starty = Fl::event_y();
+    return 1;
+  case FL_PUSH: // button pressed
+  {
+    // else
+    {
+      Model *mod = getModel(startx, starty);
+      startx = Fl::event_x();
+      starty = Fl::event_y();
+      selectedModel = false;
+      switch (Fl::event_button()) {
+      case 1:
+        clicked_empty_space = (mod == NULL);
+        empty_space_startx = startx;
+        empty_space_starty = starty;
+        if (mod) {
+          // clicked a model
+          if (Fl::event_state(FL_SHIFT)) {
+            // holding shift, toggle selection
+            if (selected(mod))
+              unSelect(mod);
+            else {
+              select(mod);
+              selectedModel = true; // selected a model
+            }
+          } else {
+            if (!selected(mod)) {
+              // clicked on an unselected model while
+              //  not holding shift, this is the new
+              //  selection
+              unSelectAll();
+              select(mod);
+            }
+            selectedModel = true; // selected a model
+          }
+        }
+
+        redraw(); // probably required
+        return 1;
+      case 3: {
+        // leave selections alone
+        // rotating handled within FL_DRAG
+        return 1;
+      }
+      default: return 0;
+      }
+    }
+  }
+
+  case FL_DRAG: // mouse moved while button was pressed
+  {
+    int dx = Fl::event_x() - startx;
+    int dy = Fl::event_y() - starty;
+
+    if (Fl::event_state(FL_BUTTON1) && Fl::event_state(FL_CTRL) == false) {
+      // Left mouse button drag
+      if (selectedModel) {
+        // started dragging on a selected model
+
+        double sx, sy, sz;
+        CanvasToWorld(startx, starty, &sx, &sy, &sz);
+        double x, y, z;
+        CanvasToWorld(Fl::event_x(), Fl::event_y(), &x, &y, &z);
+        // move all selected models to the mouse pointer
+        FOR_EACH (it, selected_models) {
+          Model *mod = *it;
+          mod->AddToPose(x - sx, y - sy, 0, 0);
+        }
+      } else {
+        // started dragging on empty space or an
+        //  unselected model, move the canvas
+        if (pCamOn == true) {
+          perspective_camera.move(-dx, dy, 0.0);
+        } else {
+          camera.move(-dx, dy);
+        }
+        invalidate(); // so the projection gets updated
+      }
+    } else if (Fl::event_state(FL_BUTTON3)
+               || (Fl::event_state(FL_BUTTON1) && Fl::event_state(FL_CTRL))) {
+      // rotate all selected models
+
+      if (selected_models.size()) {
+        FOR_EACH (it, selected_models) {
+          Model *mod = *it;
+          mod->AddToPose(0, 0, 0, 0.05 * (dx + dy));
+        }
+      } else {
+        // printf( "button 2\n" );
+
+        int dx = Fl::event_x() - startx;
+        int dy = Fl::event_y() - starty;
+
+        if (pCamOn == true) {
+          perspective_camera.addYaw(-dx);
+          perspective_camera.addPitch(-dy);
+        } else {
+          camera.addPitch(-0.5 * static_cast<double>(dy));
+          camera.addYaw(-0.5 * static_cast<double>(dx));
+        }
+      }
+      invalidate();
+      redraw();
+    }
+
+    startx = Fl::event_x();
+    starty = Fl::event_y();
+
+    redraw();
+    return 1;
+  } // end case FL_DRAG
+
+  case FL_RELEASE: // mouse button released
+    if (empty_space_startx == Fl::event_x() && empty_space_starty == Fl::event_y()
+        && clicked_empty_space == true) {
+      // clicked on empty space, unselect all
+      unSelectAll();
+      redraw();
+    }
+    return 1;
+
+  case FL_FOCUS:
+  case FL_UNFOCUS:
+    //.... Return 1 if you want keyboard events, 0 otherwise
+    return 1;
+
+  case FL_KEYBOARD:
+    switch (Fl::event_key()) {
+    case FL_Left:
+      if (pCamOn == false) {
+        camera.move(-10, 0);
+      } else {
+        perspective_camera.strafe(-0.5);
+      }
+      break;
+    case FL_Right:
+      if (pCamOn == false) {
+        camera.move(10, 0);
+      } else {
+        perspective_camera.strafe(0.5);
+      }
+      break;
+    case FL_Down:
+      if (pCamOn == false) {
+        camera.move(0, -10);
+      } else {
+        perspective_camera.forward(-0.5);
+      }
+      break;
+    case FL_Up:
+      if (pCamOn == false) {
+        camera.move(0, 10);
+      } else {
+        perspective_camera.forward(0.5);
+      }
+      break;
+    default:
+      redraw(); // we probably set a display config - so need this
+      return 0; // keypress unhandled
+    }
+
+    invalidate(); // update projection
+    return 1;
+
+  //	case FL_SHORTCUT:
+  //		//... shortcut, key is in Fl::event_key(), ascii in
+  // Fl::event_text()
+  //		//... Return 1 if you understand/use the shortcut event, 0
+  // otherwise...
+  //		return 1;
+  default:
+    // pass other events to the base class...
+    // printf( "EVENT %d\n", event );
+    return Fl_Gl_Window::handle(event);
+
+  } // end switch( event )
+
+  return 0;
+}
+
+void CanvasFLTK::resize(int X, int Y, int W, int H)
+{
+  Fl_Gl_Window::resize(X, Y, W, H);
+
+  if (!init_done) // hack - should capture a create event to do this rather
+    // thann have it in multiple places
+    InitGl();
+
+  FixViewport(W, H);
+  invalidate();
+}
+
+void CanvasFLTK::Load(Worldfile *wf, int sec)
+{
+	Canvas::Load(wf, sec);
+  if (!world->paused)
+    // // start the timer that causes regular redraws
+    Fl::add_timeout(((double)interval / 1000), (Fl_Timeout_Handler)Canvas::TimerCallback, this);
+
+  invalidate(); // we probably changed something
+}
+
+void CanvasFLTK::TimerCallback(Canvas *c)
+{
+  if (c->world->dirty) {
+    // puts( "timer redraw" );
+    c->doRedraw();
+    c->world->dirty = false;
+  }
+  Fl::repeat_timeout(c->interval / 1000.0, (Fl_Timeout_Handler)Canvas::TimerCallback, c);
+}
+
+}
diff --git a/libstage/canvas_fltk.hh b/libstage/canvas_fltk.hh
new file mode 100644
index 000000000..c23b9e515
--- /dev/null
+++ b/libstage/canvas_fltk.hh
@@ -0,0 +1,52 @@
+#ifndef _CANVAS_FLTK_HH_
+#define _CANVAS_FLTK_HH_
+
+#include <FL/Fl_Gl_Window.H>
+#include <FL/Fl_Menu_Bar.H>
+
+#include "canvas.hh"
+
+namespace Stg {
+class CanvasFLTK : public Canvas, public Fl_Gl_Window {
+  friend class WorldGui; // allow access to private members
+  friend class Model;
+
+protected:
+  void createMenuItem(Option * opt, Fl_Menu_Bar *menu, std::string path);
+  static void toggleCb(Fl_Widget *w, void *p);
+
+public:
+  CanvasFLTK(WorldGui *world, int x, int y, int width, int height);
+  ~CanvasFLTK();
+
+  bool graphics;
+  int screenshot_frame_skip;
+
+  void Screenshot();
+  void InitGl();
+  void InitTextures();
+  void createMenuItems(Fl_Menu_Bar *menu, std::string path);
+
+  void FixViewport(int W, int H);
+  void resetCamera();
+  virtual void renderFrame();
+  virtual void draw();
+  virtual int handle(int event);
+  void resize(int X, int Y, int W, int H);
+
+  virtual bool isValid() {return valid();}
+	virtual void doRedraw() {redraw();}
+	virtual int getWidth() const {return w();}
+	virtual int getHeight() const {return h();}
+	virtual void setInvalidate() { invalidate(); }
+
+  void CanvasToWorld(int px, int py, double *wx, double *wy, double *wz);
+
+  void InvertView(uint32_t invertflags);
+
+  static void TimerCallback(Canvas *canvas);
+  static void perspectiveCb(Option * opt, void *p);
+};
+} //namespace Stg
+
+#endif
diff --git a/libstage/gl.cc b/libstage/gl.cc
index 14528ee2e..2a8337958 100644
--- a/libstage/gl.cc
+++ b/libstage/gl.cc
@@ -1,7 +1,11 @@
-
 #include "stage.hh"
+
 using namespace Stg;
 
+float view_height();
+void gl_draw(const char * text, float x, float y, float w, float h, int align);
+void gl_draw(const char * text);
+
 // transform the current coordinate frame by the given pose
 void Stg::Gl::coord_shift(double x, double y, double z, double a)
 {
@@ -41,7 +45,7 @@ void Stg::Gl::draw_array(float x, float y, float w, float h, float *data, size_t
   snprintf(buf, 63, "%.2f", min);
   Gl::draw_string(x, y, 0, buf);
   snprintf(buf, 63, "%.2f", max);
-  Gl::draw_string(x, y + h - fl_height(), 0, buf);
+  Gl::draw_string(x, y + h - view_height(), 0, buf);
 }
 
 void Stg::Gl::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset)
@@ -72,8 +76,7 @@ void Stg::Gl::draw_string(float x, float y, float z, const char *str)
     gl_draw(str); // fltk function
 }
 
-void Stg::Gl::draw_string_multiline(float x, float y, float w, float h, const char *str,
-                                    Fl_Align align)
+void Stg::Gl::draw_string_multiline(float x, float y, float w, float h, const char *str, int align)
 {
   // printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str),
   // str );
diff --git a/libstage/image.cc b/libstage/image.cc
new file mode 100644
index 000000000..a263a0b3f
--- /dev/null
+++ b/libstage/image.cc
@@ -0,0 +1,47 @@
+#include "image.hh"
+#include <cassert>
+#include <stdlib.h>
+
+namespace Stg{
+
+void Image::Initialize()
+{
+
+}
+
+Image::Image()
+{
+	width = 0;
+	height = 0;
+	bpp = 0;
+}
+
+int Image::getWidth() const
+{
+	return width;
+}
+
+int Image::getHeight() const
+{
+	return height;
+}
+
+int Image::getDepth() const
+{
+	return bpp;
+}
+
+uint8_t * Image::getData()
+{
+	if(data.empty())
+		return NULL;
+	return &data[0];
+}
+
+bool Image::load(const char * path)
+{
+	assert(false);
+	return false;
+}
+
+}
diff --git a/libstage/image.hh b/libstage/image.hh
new file mode 100644
index 000000000..334d1b1db
--- /dev/null
+++ b/libstage/image.hh
@@ -0,0 +1,39 @@
+/*
+ * image.hh
+ *
+ *  Created on: Nov 10, 2017
+ *      Author: vrobot
+ */
+
+#ifndef LIBSTAGE_IMAGE_HH_
+#define LIBSTAGE_IMAGE_HH_
+
+#include <stdint.h>
+#include <vector>
+
+namespace Stg
+{
+
+/** Wraps process of loading image from the file */
+class Image
+{
+public:
+	Image();
+	bool load(const char * path);
+	int getWidth() const;
+	int getHeight() const;
+	int getDepth() const;
+	uint8_t * getData();
+	/** Initializes image loading library */
+	static void Initialize();
+protected:
+	int width;
+	int height;
+	/** Bytes per pixel */
+	int bpp;
+	std::vector<uint8_t> data;
+};
+
+}
+
+#endif /* LIBSTAGE_IMAGE_HH_ */
diff --git a/libstage/main.cc b/libstage/main.cc
index 94630f9c6..db473963d 100644
--- a/libstage/main.cc
+++ b/libstage/main.cc
@@ -28,6 +28,8 @@
 
 #include "config.h"
 #include "stage.hh"
+#include "world_gui.hh"
+
 using namespace Stg;
 
 const char *USAGE = "USAGE:  stage [options] <worldfile1> [worldfile2 ... worldfileN]\n"
diff --git a/libstage/main_headless.cc b/libstage/main_headless.cc
new file mode 100644
index 000000000..f49e96770
--- /dev/null
+++ b/libstage/main_headless.cc
@@ -0,0 +1,111 @@
+/**
+  \defgroup stage The Stage standalone robot simulator.
+
+  USAGE:  stage [options] <worldfile1> [worldfile2 ... ]
+
+  Available [options] are:
+
+    --clock        : print simulation time peridically on standard output
+
+    -c             : equivalent to --clock
+
+    --gui          : run without a GUI
+
+    -g             : equivalent to --gui
+
+    --help         : print this message
+
+    --args \"str\"   : define an argument string to be passed to all controllers
+
+    -a \"str\"       : equivalent to --args "str"
+
+    -h             : equivalent to --help"
+
+    -?             : equivalent to --help
+ */
+
+#include <getopt.h>
+
+#include "config.h"
+#include "stage.hh"
+using namespace Stg;
+
+const char *USAGE = "USAGE:  stage [options] <worldfile1> [worldfile2 ... worldfileN]\n"
+                    "Available [options] are:\n"
+                    "  --clock        : print simulation time peridically on standard output\n"
+                    "  -c             : equivalent to --clock\n"
+                    "  --gui          : run without a GUI\n"
+                    "  -g             : equivalent to --gui\n"
+                    "  --help         : print this message\n"
+                    "  --args \"str\"   : define an argument string to be passed to all "
+                    "controllers\n"
+                    "  -a \"str\"       : equivalent to --args \"str\"\n"
+                    "  -h             : equivalent to --help\n"
+                    "  -?             : equivalent to --help";
+
+/* options descriptor */
+
+static struct option longopts[] = {
+  { "clock",  optional_argument,   NULL,  'c' },
+  { "help",  optional_argument,   NULL,  'h' },
+  { "args",  required_argument,   NULL,  'a' },
+  { NULL, 0, NULL, 0 }
+};
+
+int main( int argc, char* argv[] )
+{
+  // initialize libstage - call this first
+  Stg::Init(&argc, &argv);
+
+  printf("%s %s ", PROJECT, VERSION);
+
+  int ch = 0, optindex = 0;
+  bool showclock = false;
+
+  while ((ch = getopt_long(argc, argv, "cgh?", longopts, &optindex)) != -1) {
+    switch (ch) {
+    case 0: // long option given
+      printf("option %s given\n", longopts[optindex].name);
+      break;
+    case 'a': World::ctrlargs = std::string(optarg); break;
+    case 'c':
+      showclock = true;
+      printf("[Clock enabled]");
+      break;
+    case 'h':
+    case '?':
+      puts(USAGE);
+      //			 exit(0);
+      break;
+    default:
+      printf("unhandled option %c\n", ch);
+      puts(USAGE);
+      // exit(0);
+    }
+  }
+
+  puts(""); // end the first start-up line
+
+  // arguments at index [optindex] and later are not options, so they
+  // must be world file names
+
+  optindex = optind; // points to first non-option
+  while (optindex < argc) {
+    if (optindex > 0) {
+      const char *worldfilename = argv[optindex];
+      World *world = new World(worldfilename);
+      world->Load(worldfilename);
+      world->ShowClock(showclock);
+
+      if (!world->paused)
+        world->Start();
+    }
+    optindex++;
+  }
+
+  World::Run();
+
+  puts("\n[Stage: done]");
+
+  return EXIT_SUCCESS;
+}
diff --git a/libstage/model.cc b/libstage/model.cc
index f8c1976c8..a7d32b482 100644
--- a/libstage/model.cc
+++ b/libstage/model.cc
@@ -238,8 +238,7 @@ Model::Model(World *world, Model *parent, const std::string &type, const std::st
       power_pack(NULL), pps_charging(), rastervis(), rebuild_displaylist(true), say_string(),
       stack_children(true), stall(false), subs(0), thread_safe(false), trail(20),
       trail_index(0),  trail_interval(10), type(type), event_queue_num(0), used(false), watts(0.0), watts_give(0.0),
-      watts_take(0.0), wf(NULL), wf_entity(0), world(world),
-      world_gui(dynamic_cast<WorldGui *>(world))
+      watts_take(0.0), wf(NULL), wf_entity(0), world(world)	//,world_gui(dynamic_cast<WorldGui *>(world))
 {
   assert(world);
 
@@ -1534,3 +1533,8 @@ void Model::LoadControllerModule(const char *lib)
 
   fflush(stdout);
 }
+
+Canvas * Model::GetCanvas()
+{
+	return world != NULL ? world->GetCanvas() : NULL;
+}
diff --git a/libstage/model_camera.cc b/libstage/model_camera.cc
index d306c8a36..c84641d59 100644
--- a/libstage/model_camera.cc
+++ b/libstage/model_camera.cc
@@ -87,13 +87,14 @@ void cross(float &x1, float &y1, float &z1, float x2, float y2, float z2)
 }
 
 ModelCamera::ModelCamera(World *world, Model *parent, const std::string &type)
-    : Model(world, parent, type), _canvas(NULL), _frame_data(NULL), _frame_color_data(NULL),
+    : Model(world, parent, type), _frame_data(NULL), _frame_color_data(NULL),
       _valid_vertexbuf_cache(false), _vertexbuf_cache(NULL), _width(32), _height(32),
       _camera_quads_size(0), _camera_quads(NULL), _camera_colors(NULL), _camera(), _yaw_offset(0.0),
       _pitch_offset(0.0)
 {
 PRINT_DEBUG2("Constructing ModelCamera %u (%s)\n", id, type.c_str());
 
+/*
   WorldGui *world_gui = dynamic_cast<WorldGui *>(world);
 
   if (world_gui == NULL) {
@@ -101,7 +102,7 @@ PRINT_DEBUG2("Constructing ModelCamera %u (%s)\n", id, type.c_str());
     assert(0);
   }
   _canvas = world_gui->GetCanvas();
-
+*/
   _camera.setPitch(90.0);
 
   this->SetGeom(Geom(Pose(), DEFAULT_SIZE));
@@ -146,11 +147,12 @@ void ModelCamera::Load(void)
 
 void ModelCamera::Update(void)
 {
-  GetFrame();
+	// TODO: Get some rendering canvas here
+  //GetFrame();
   Model::Update();
 }
 
-bool ModelCamera::GetFrame(void)
+bool ModelCamera::GetFrame(Canvas * canvas)
 {
   if (_width == 0 || _height == 0)
     return false;
@@ -169,10 +171,10 @@ bool ModelCamera::GetFrame(void)
   // TODO overcome issue when glviewport is set LARGER than the window side
   // currently it just clips and draws outside areas black - resulting in bad
   // glreadpixel data
-  if (_width > _canvas->w())
-    _width = _canvas->w();
-  if (_height > _canvas->h())
-    _height = _canvas->h();
+  if (_width > canvas->getWidth())
+    _width = canvas->getWidth();
+  if (_height > canvas->getHeight())
+    _height = canvas->getHeight();
 
   GLint viewport[4];
   glGetIntegerv(GL_VIEWPORT, viewport);
@@ -193,8 +195,8 @@ bool ModelCamera::GetFrame(void)
   _camera.Draw();
 
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-  _canvas->DrawFloor();
-  _canvas->DrawBlocks();
+  canvas->DrawFloor();
+  canvas->DrawBlocks();
 
   // read depth buffer
   glReadPixels(0, 0, _width, _height,
@@ -211,8 +213,8 @@ bool ModelCamera::GetFrame(void)
   glReadPixels(0, 0, _width, _height, GL_RGBA, GL_UNSIGNED_BYTE, _frame_color_data);
 
   glViewport(viewport[0], viewport[1], viewport[2], viewport[3]);
-  _canvas->invalidate();
-  _canvas->setDirtyBuffer();
+  canvas->setInvalidate();
+  canvas->setDirtyBuffer();
   return true;
 }
 
diff --git a/libstage/model_draw.cc b/libstage/model_draw.cc
index e97c9a881..dc2b4b233 100644
--- a/libstage/model_draw.cc
+++ b/libstage/model_draw.cc
@@ -244,18 +244,19 @@ void Model::AddVisualizer(Visualizer *cv, bool on_by_default)
   assert(cv);
 
   // If there's no GUI, ignore this request
-  if (!world_gui)
-    return;
+  //if (!world_gui)
+  //  return;
 
   // save visualizer instance
   cv_list.push_back(cv);
 
   // register option for all instances which share the same name
-  Canvas *canvas = world_gui->GetCanvas();
+  Canvas *canvas = this->GetCanvas();
+
   std::map<std::string, Option *>::iterator i = canvas->_custom_options.find(cv->GetMenuName());
   if (i == canvas->_custom_options.end()) {
     Option *op =
-        new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default, world_gui);
+        new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default, world);
     canvas->_custom_options[cv->GetMenuName()] = op;
     RegisterOption(op);
   }
@@ -313,8 +314,8 @@ void Model::DrawStatus(Camera *cam)
 
       if (valid) {
         // fl_font( FL_HELVETICA, 12 );
-        float w = gl_width(this->say_string.c_str()); // scaled text width
-        float h = gl_height(); // scaled text height
+        float w = Gl::gl_width(this->say_string.c_str()); // scaled text width
+        float h = Gl::gl_height(); // scaled text height
 
         GLdouble wx, wy, wz;
         GLint viewport[4];
@@ -530,12 +531,14 @@ void Model::DataVisualizeTree(Camera *cam)
 {
   PushLocalCoords();
 
-  if (subs > 0) {
+  Canvas * canvas = GetCanvas();
+
+  if (subs > 0 && canvas != NULL) {
     DataVisualize(cam); // virtual function overridden by some model types
 
     FOR_EACH (it, cv_list) {
       Visualizer *vis = *it;
-      if (world_gui->GetCanvas()->_custom_options[vis->GetMenuName()]->isEnabled())
+      if (canvas->_custom_options[vis->GetMenuName()]->isEnabled())
         vis->Visualize(this, cam);
     }
   }
diff --git a/libstage/option.cc b/libstage/option.cc
index 0964945d9..e581a46df 100644
--- a/libstage/option.cc
+++ b/libstage/option.cc
@@ -8,7 +8,7 @@ using namespace Stg;
 Option::Option(const std::string &n, const std::string &tok, const std::string &key, bool v,
                World *world)
     : optName(n), value(v), wf_token(tok), shortcut(key), menu(NULL), menuIndex(0), menuCb(NULL),
-      menuCbWidget(NULL), _world(world), htname(n)
+			callbackData(NULL), _world(world), htname(n)
 {
   /* do nothing */
 }
@@ -30,28 +30,10 @@ void Option::Save(Worldfile *wf, int section)
   wf->WriteInt(section, wf_token.c_str(), value);
 }
 
-void Option::toggleCb(Fl_Widget *, void *p)
-{
-  // Fl_Menu_* menu = static_cast<Fl_Menu_*>( w );
-  Option *opt = static_cast<Option *>(p);
-  opt->invert();
-  if (opt->menuCb)
-    opt->menuCb(opt->menuCbWidget, opt);
-}
-
-void Option::menuCallback(Fl_Callback *cb, Fl_Widget *w)
+void Option::setOptionCallback(Option::Callback cb, void *data)
 {
   menuCb = cb;
-  menuCbWidget = w;
-}
-
-void Option::createMenuItem(Fl_Menu_Bar *m, std::string path)
-{
-  menu = m;
-  path = path + "/" + optName;
-  // create a menu item and save its index
-  menuIndex = menu->add(path.c_str(), shortcut.c_str(), toggleCb, this,
-                        FL_MENU_TOGGLE | (value ? FL_MENU_VALUE : 0));
+  this->callbackData = data;
 }
 
 void Option::set(bool val)
@@ -64,11 +46,13 @@ void Option::set(bool val)
   }
 
   if (_world) {
+#ifdef TEST_LATER
     WorldGui *wg = dynamic_cast<WorldGui *>(_world);
     if (wg == NULL)
       return;
     Canvas *canvas = wg->GetCanvas();
-    canvas->invalidate();
+    canvas->setInvalidate();
     canvas->redraw();
+#endif
   }
 }
diff --git a/libstage/option.hh b/libstage/option.hh
index 667328280..4a7864a99 100644
--- a/libstage/option.hh
+++ b/libstage/option.hh
@@ -27,8 +27,10 @@ private:
   std::string shortcut;
   Fl_Menu_ *menu;
   int menuIndex;
-  Fl_Callback *menuCb;
-  Fl_Widget *menuCbWidget;
+  /** Callback type for menu events*/
+  typedef void (*Callback)(Option *opt, void * data);
+  Callback menuCb;
+  void *callbackData;
   World *_world;
 
 public:
@@ -53,9 +55,7 @@ public:
   // 				{ return a->optName < b->optName; }
   // 		};
 
-  void createMenuItem(Fl_Menu_Bar *menu, std::string path);
-  void menuCallback(Fl_Callback *cb, Fl_Widget *w);
-  static void toggleCb(Fl_Widget *w, void *p);
+  void setOptionCallback(Callback cb, void *w);
   void Load(Worldfile *wf, int section);
   void Save(Worldfile *wf, int section);
 
diff --git a/libstage/stage.cc b/libstage/stage.cc
index 2ba228d9f..c8a84a4aa 100644
--- a/libstage/stage.cc
+++ b/libstage/stage.cc
@@ -1,6 +1,10 @@
 // Author: Richard Vaughan
 
+#ifdef USE_FLTK
 #include <FL/Fl_Shared_Image.H>
+#endif
+
+#include "image.hh"
 
 #include "config.h" // results of cmake's system configuration tests
 #include "file_manager.hh"
@@ -32,7 +36,8 @@ void Stg::Init(int *argc, char **argv[])
   RegisterModels();
 
   // ask FLTK to load support for various image formats
-  fl_register_images();
+  //fl_register_images();
+  Image::Initialize();
 
   init_called = true;
 }
@@ -89,21 +94,25 @@ int Stg::polys_from_image_file(const std::string &filename,
   // TODO: make this a parameter
   const int threshold = 127;
 
-  Fl_Shared_Image *img = Fl_Shared_Image::get(filename.c_str());
-  if (img == NULL) {
+  Image img;
+
+  //SDL_Surface * img = SDL_LoadBMP("image.bmp");
+
+  //Fl_Shared_Image *img = Fl_Shared_Image::get(filename.c_str());
+  if (!img.load(filename.c_str())) {
     std::cerr << "failed to open file: " << filename << std::endl;
 
-    assert(img); // easy access to this point in debugger
+    //assert(img); // easy access to this point in debugger
     exit(-1);
   }
 
   // printf( "loaded image %s w %d h %d d %d count %d ld %d\n",
   //  filename, img->w(), img->h(), img->d(), img->count(), img->ld() );
 
-  const unsigned int width = img->w();
-  const unsigned height = img->h();
-  const unsigned int depth = img->d();
-  uint8_t *pixels = (uint8_t *)img->data()[0];
+  const unsigned int width = img.getWidth();
+  const unsigned height = img.getHeight();
+  const unsigned int depth = img.getDepth();
+  uint8_t *pixels = (uint8_t *)img.getData();
 
   // a set of previously seen directed edges, The key is a 4-element vector
   // [x1,y1,x2,y2].
@@ -209,8 +218,8 @@ int Stg::polys_from_image_file(const std::string &filename,
     polys.push_back(poly);
   }
 
-  if (img)
-    img->release(); // frees all resources for this image
+  //if (img)
+  //  img->release(); // frees all resources for this image
   return 0; // ok
 }
 
diff --git a/libstage/stage.hh b/libstage/stage.hh
index 92a1d5f9b..2de90605f 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -55,14 +55,6 @@
 #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>
@@ -494,13 +486,19 @@ point_t *unit_square_points_create();
 /** Convenient OpenGL drawing routines, used by visualization
       code. */
 namespace Gl {
+
+/** Calculate scaled width of the string for current font*/
+float gl_width(const char * string);
+/** Calculate font height */
+float gl_height();
+
 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);
 /** Render a string at [x,y,z] in the current color */
 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_string_multiline(float x, float y, float w, float h, const char *string, int 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);
@@ -870,6 +868,10 @@ public:
 imlementation does nothing, but can be overridden by
 subclasses. */
   virtual void Redraw(void) {} // does nothing
+
+  void DrawOccupancy() const;
+  void DrawVoxels() const;
+
   std::vector<point_int_t> rt_cells;
   std::vector<point_int_t> rt_candidate_cells;
 
@@ -1123,6 +1125,8 @@ nonexistent */
   void ShowClock(bool enable) { show_clock = enable; }
   /** Return the floor model */
   Model *GetGround() { return ground; }
+
+  Canvas * GetCanvas() {return NULL; }
 };
 
 class Block {
@@ -1288,6 +1292,8 @@ public:
   virtual void reset() = 0;
   virtual void Load(Worldfile *wf, int sec) = 0;
 
+  Canvas * GetCanvas();
+
   // 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;
@@ -1415,118 +1421,6 @@ public:
   void Save(Worldfile *wf, int sec);
 };
 
-/** Extends World to implement an FLTK / OpenGL graphical user
-      interface.
-  */
-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; ///< Used to load and save worldfiles
-  std::vector<usec_t> interval_log;
-
-  /** Stage attempts to run this many times faster than real
-time. If -1, Stage runs as fast as possible. */
-  double speedup;
-
-  bool confirm_on_quit; ///< if true, show save dialog on (GUI) exit (default)
-
-  Fl_Menu_Bar *mbar;
-  OptionsDlg *oDlg;
-  bool pause_time;
-  std::string caption_prefix; //!< prefix of window caption (default PROJECT name constant)
-
-  /** The amount of real time elapsed between $timing_interval
-timesteps. */
-  usec_t real_time_interval;
-
-  /** The current real time in microseconds. */
-  usec_t real_time_now;
-
-  /** The last recorded real time, sampled every $timing_interval
-updates. */
-  usec_t real_time_recorded;
-
-  /** Number of updates between measuring elapsed real time. */
-  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();
-
-  /// Defines what all WorldGUI::Load(*) in methods have in common. Called after initial setup.
-  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();
-
-  /** Forces the window to be redrawn, even if paused.*/
-  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; }
-  /** show the window - need to call this if you don't Load(). */
-  void Show();
-
-  /** Get human readable string that describes the current global energy state. */
-  std::string EnergyString(void) const;
-  virtual void RemoveChild(Model *mod);
-
-  bool IsTopView();
-};
-
 class StripPlotVis : public Visualizer {
 private:
   // Model* mod;
@@ -1899,7 +1793,7 @@ watts_give >0 */
   Worldfile *wf;
   int wf_entity;
   World *world; //!< Pointer to the world in which this model exists
-  WorldGui *world_gui; //!< Pointer to the GUI world - NULL if running in non-gui mode
+  //WorldGui *world_gui; //!< Pointer to the GUI world - NULL if running in non-gui mode
 
 public:
   virtual void SetToken(const std::string &str)
@@ -2103,7 +1997,8 @@ public:
         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)
+        watts(0), watts_give(0), watts_take(0), wf(NULL), wf_entity(0), world(NULL)
+  //, world_gui(NULL)
   {
   }
 
@@ -2118,6 +2013,8 @@ public:
 
   void BecomeParentOf(Model *child);
 
+  Canvas * GetCanvas();
+
   void Load(Worldfile *wf, int wf_entity)
   {
     /** Set the worldfile and worldfile entity ID - must be called
@@ -2703,7 +2600,7 @@ public:
   } ColoredVertex;
 
 private:
-  Canvas *_canvas;
+  //Canvas *_canvas;
 
   GLfloat *_frame_data; // opengl read buffer
   GLubyte *_frame_color_data; // opengl read buffer
@@ -2728,7 +2625,7 @@ private:
 
   /// Take a screenshot from the camera's perspective. return: true for sucess, and data is
   /// available via FrameDepth() / FrameColor()
-  bool GetFrame();
+  bool GetFrame(Canvas * canvas);
 
 public:
   ModelCamera(World *world, Model *parent, const std::string &type);
diff --git a/libstage/texture_manager.cc b/libstage/texture_manager.cc
index 3f29aae92..708fd725c 100644
--- a/libstage/texture_manager.cc
+++ b/libstage/texture_manager.cc
@@ -8,6 +8,8 @@
 
 #include "texture_manager.hh"
 #include "file_manager.hh"
+#include "image.hh"
+
 #include <sstream>
 using namespace Stg;
 
diff --git a/libstage/vis_strip.cc b/libstage/vis_strip.cc
index e34bf0e24..77821cf04 100644
--- a/libstage/vis_strip.cc
+++ b/libstage/vis_strip.cc
@@ -21,9 +21,10 @@ StripPlotVis::~StripPlotVis()
     free(data);
 }
 
-void StripPlotVis::Visualize(Model *mod, Camera *)
+void StripPlotVis::Visualize(Model *mod, Camera *camera)
 {
-  Canvas *canvas = dynamic_cast<WorldGui *>(mod->GetWorld())->GetCanvas();
+  //Canvas *canvas = dynamic_cast<WorldGui *>(mod->GetWorld())->GetCanvas();
+	Canvas *canvas = camera->GetCanvas();
 
   if (!canvas->selected(mod)) // == canvas->SelectedVisualizeAll() )
     return;
diff --git a/libstage/world.cc b/libstage/world.cc
index cc44ad52a..67d24ef5e 100644
--- a/libstage/world.cc
+++ b/libstage/world.cc
@@ -204,9 +204,11 @@ void World::Run()
     // https://wiki.orfeo-toolbox.org/index.php/How_to_exit_every_fltk_window_in_the_world,
     // FLTK
     // is a piece of crap):
-    while (Fl::first_window() && !World::quit_all) {
-      Fl::wait();
-    }
+  	assert(false);
+  	// Dirty replacement here
+    //while (Fl::first_window() && !World::quit_all) {
+    //  Fl::wait();
+    //}
   } else {
     while (!UpdateAll())
       ;
@@ -1111,6 +1113,35 @@ void World::RemovePowerPack(PowerPack *pp)
   EraseAll(pp, powerpack_list);
 }
 
+void World::DrawOccupancy() const
+{
+  // 	int count=0;
+  //   FOR_EACH( it, superregions )
+  // 		printf( "sr %d [%d,%d]  %p\n", count++, it->first.x,
+  // it->first.y,
+  // it->second );
+  // 	printf( "done\n" );
+
+  //  unsigned int layer( updates % 2 );
+
+  FOR_EACH (it, superregions)
+    it->second->DrawOccupancy();
+
+  // 	 {
+
+  // it->second->DrawOccupancy(0);
+  //    it->second->DrawOccupancy(1);
+
+  //	 }
+}
+
+void World::DrawVoxels() const
+{
+  unsigned int layer(updates % 2);
+
+  FOR_EACH (it, superregions)
+    it->second->DrawVoxels(layer);
+}
 /// Register an Option for pickup by the GUI
 void World::RegisterOption(Option *opt)
 {  
diff --git a/libstage/world_gui.hh b/libstage/world_gui.hh
new file mode 100644
index 000000000..af5b7c630
--- /dev/null
+++ b/libstage/world_gui.hh
@@ -0,0 +1,130 @@
+/*
+ * stage_gui.hh
+ *
+ *  Created on: Nov 10, 2017
+ *      Author: vrobot
+ */
+
+#ifndef STAGE_LIBSTAGE_STAGE_GUI_HH_
+#define STAGE_LIBSTAGE_STAGE_GUI_HH_
+
+#include "stage.hh"
+#include "canvas_fltk.hh"
+
+// FLTK Gui includes
+#include <FL/Fl.H>
+#include <FL/Fl_Box.H>
+#include <FL/Fl_Window.H>
+#include <FL/fl_draw.H>
+#include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
+
+namespace Stg {
+/** Extends World to implement an FLTK / OpenGL graphical user
+      interface.
+  */
+class WorldGui : public World, public Fl_Window {
+  friend class Canvas;
+  friend class ModelCamera;
+  friend class Model;
+  friend class Option;
+
+private:
+  CanvasFLTK *canvas;
+  std::vector<Option *> drawOptions;
+  FileManager *fileMan; ///< Used to load and save worldfiles
+  std::vector<usec_t> interval_log;
+
+  /** Stage attempts to run this many times faster than real
+time. If -1, Stage runs as fast as possible. */
+  double speedup;
+
+  bool confirm_on_quit; ///< if true, show save dialog on (GUI) exit (default)
+
+  Fl_Menu_Bar *mbar;
+  OptionsDlg *oDlg;
+  bool pause_time;
+  std::string caption_prefix; //!< prefix of window caption (default PROJECT name constant)
+
+  /** The amount of real time elapsed between $timing_interval
+timesteps. */
+  usec_t real_time_interval;
+
+  /** The current real time in microseconds. */
+  usec_t real_time_now;
+
+  /** The last recorded real time, sampled every $timing_interval
+updates. */
+  usec_t real_time_recorded;
+
+  /** Number of updates between measuring elapsed real time. */
+  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();
+
+  /// Defines what all WorldGUI::Load(*) in methods have in common. Called after initial setup.
+  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();
+public:
+  WorldGui(int width, int height, const char *caption = NULL);
+  ~WorldGui();
+
+  /** Forces the window to be redrawn, even if paused.*/
+  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;
+
+  Canvas *GetCanvas(void) const { return canvas; }
+  /** show the window - need to call this if you don't Load(). */
+  void Show();
+
+  /** Get human readable string that describes the current global energy state. */
+  std::string EnergyString(void) const;
+  virtual void RemoveChild(Model *mod);
+
+  bool IsTopView();
+};
+
+} //namespace Stg
+
+#endif /* STAGE_LIBSTAGE_STAGE_GUI_HH_ */
diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc
index 2ff21eb02..b66521dc4 100644
--- a/libstage/worldgui.cc
+++ b/libstage/worldgui.cc
@@ -158,6 +158,7 @@ then select the option from the menu again to stop.
 #include "options_dlg.hh"
 #include "region.hh"
 #include "worldfile.hh"
+#include "world_gui.hh"
 
 using namespace Stg;
 
@@ -189,7 +190,7 @@ static const char *MoreHelpText =
     "(requires Doxygen and supporting programs to be installed first).\n";
 
 WorldGui::WorldGui(int width, int height, const char *caption)
-    : Fl_Window(width, height, NULL), canvas(new Canvas(this, 0, 30, width, height - 30)),
+    : Fl_Window(width, height, NULL), canvas(new CanvasFLTK(this, 0, 30, width, height - 30)),
       drawOptions(), fileMan(new FileManager()), interval_log(), speedup(1.0), // real time
       confirm_on_quit(true), mbar(new Fl_Menu_Bar(0, 0, width, 30)), oDlg(NULL), pause_time(false),
       real_time_interval(sim_interval), real_time_now(RealTimeNow()),
@@ -455,36 +456,6 @@ std::string WorldGui::EnergyString() const
   return std::string(str);
 }
 
-void WorldGui::DrawOccupancy() const
-{
-  // 	int count=0;
-  //   FOR_EACH( it, superregions )
-  // 		printf( "sr %d [%d,%d]  %p\n", count++, it->first.x,
-  // it->first.y,
-  // it->second );
-  // 	printf( "done\n" );
-
-  //  unsigned int layer( updates % 2 );
-
-  FOR_EACH (it, superregions)
-    it->second->DrawOccupancy();
-
-  // 	 {
-
-  // it->second->DrawOccupancy(0);
-  //    it->second->DrawOccupancy(1);
-
-  //	 }
-}
-
-void WorldGui::DrawVoxels() const
-{
-  unsigned int layer(updates % 2);
-
-  FOR_EACH (it, superregions)
-    it->second->DrawVoxels(layer);
-}
-
 void WorldGui::windowCb(Fl_Widget *, WorldGui *wg)
 {
   switch (Fl::event()) {
@@ -856,12 +827,6 @@ bool WorldGui::closeWindowQuery()
   }
 }
 
-void WorldGui::DrawBoundingBoxTree()
-{
-  FOR_EACH (it, World::children)
-    (*it)->DrawBoundingBoxTree();
-}
-
 void WorldGui::PushColor(Color col)
 {
   canvas->PushColor(col);

From 92215e83a88bad46fa4589e6016b1b8a29124b25 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 00:03:55 +0300
Subject: [PATCH 02/22] More code refactoring

Introduced MenuManager class. It descendants should deal with adding options to actuial GUI implementation

All model drawing functions now get a pointer to Canvas instance. No more need to kep a pointer to world data just to get Canvas from it (after another dynamic_cast)

And many more cleaning as well

Code compiles now
---
 examples/ctrl/fasr2.cc         |   2 +-
 libstage/canvas.cc             | 222 +++++++++++++++++--------------
 libstage/canvas.hh             |  40 ++++--
 libstage/canvas_fltk.cc        | 235 +++++++++++++++++++++++++++------
 libstage/canvas_fltk.hh        |  43 ++++--
 libstage/gl.cc                 |  96 --------------
 libstage/image.cc              |  85 +++++++++++-
 libstage/model.cc              |   7 +-
 libstage/model_blinkenlight.cc |   2 +-
 libstage/model_blobfinder.cc   |   2 +-
 libstage/model_bumper.cc       |   4 +-
 libstage/model_camera.cc       |   2 +-
 libstage/model_draw.cc         |  32 ++---
 libstage/model_fiducial.cc     |   9 +-
 libstage/model_gripper.cc      |   2 +-
 libstage/model_position.cc     |   9 +-
 libstage/model_ranger.cc       |  14 +-
 libstage/option.cc             |  51 ++++---
 libstage/option.hh             |  33 ++++-
 libstage/powerpack.cc          |   7 +-
 libstage/stage.hh              |  54 ++++----
 libstage/texture_manager.cc    |  24 ++--
 libstage/vis_strip.cc          |   7 +-
 libstage/world.cc              |  10 ++
 libstage/world_gui.hh          |   4 +-
 libstage/worldgui.cc           |  12 +-
 26 files changed, 636 insertions(+), 372 deletions(-)

diff --git a/examples/ctrl/fasr2.cc b/examples/ctrl/fasr2.cc
index dcbf389a8..acb66fd15 100644
--- a/examples/ctrl/fasr2.cc
+++ b/examples/ctrl/fasr2.cc
@@ -152,7 +152,7 @@ class Robot {
 
     explicit GraphVis(Graph **graphpp) : Visualizer("graph", "vis_graph"), graphpp(graphpp) {}
     virtual ~GraphVis() {}
-    virtual void Visualize(Model *mod, Camera *)
+    virtual void Visualize(Model *mod, Camera *, Canvas * canvas)
     {
       if (*graphpp == NULL)
         return;
diff --git a/libstage/canvas.cc b/libstage/canvas.cc
index 8a4b9f12d..9b583ed73 100644
--- a/libstage/canvas.cc
+++ b/libstage/canvas.cc
@@ -18,7 +18,6 @@
 
 #include <algorithm>
 #include <functional> // For greater<int>( )
-#include <png.h>
 #include <sstream>
 #include <string>
 
@@ -33,7 +32,6 @@ static const int checkImageHeight = 2;
 static GLubyte checkImage[checkImageHeight][checkImageWidth][4];
 static bool blur = true;
 
-static bool init_done = false;
 static bool texture_load_done = false;
 
 // GLuint glowTex;
@@ -41,30 +39,30 @@ GLuint checkTex;
 
 Canvas::Canvas(World *world, int x, int y, int width, int height)
     : colorstack(), models_sorted(), current_camera(NULL),
-      camera(), perspective_camera(), dirty_buffer(false), wf(NULL), startx(-1), starty(-1),
+      camera(), perspective_camera(), dirty_buffer(false), wf(NULL),
       selected_models(), last_selection(NULL), interval(40), // msec between redraws
+      init_done(false),
       // initialize Option objects
       //  showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ),
-      showBBoxes("Debug/Bounding boxes", "show_boundingboxes", "^b", false, world),
-      showBlocks("Blocks", "show_blocks", "b", true, world),
-      showBlur("Trails/Blur", "show_trailblur", "^d", false, world),
-      showClock("Clock", "show_clock", "c", true, world),
-      showData("Data", "show_data", "d", false, world),
-      showFlags("Flags", "show_flags", "l", true, world),
-      showFollow("Follow", "show_follow", "f", false, world),
-      showFootprints("Footprints", "show_footprints", "o", false, world),
-      showGrid("Grid", "show_grid", "g", true, world),
-      showOccupancy("Debug/Occupancy", "show_occupancy", "^o", false, world),
-      showScreenshots("Save screenshots", "screenshots", "", false, world),
-      showStatus("Status", "show_status", "s", true, world),
-      showTrailArrows("Trails/Rising Arrows", "show_trailarrows", "^a", false, world),
-      showTrailRise("Trails/Rising blocks", "show_trailrise", "^r", false, world),
-      showTrails("Trails/Fast", "show_trailfast", "^f", false, world),
-      showVoxels("Debug/Voxels", "show_voxels", "^v", false, world),
-      pCamOn("Perspective camera", "pcam_on", "r", false, world),
-      visualizeAll("Selected only", "vis_all", "v", false, world),
+      showBBoxes("Debug/Bounding boxes", "show_boundingboxes", "^b", false),
+      showBlocks("Blocks", "show_blocks", "b", true),
+      showBlur("Trails/Blur", "show_trailblur", "^d", false),
+      showClock("Clock", "show_clock", "c", true),
+      showData("Data", "show_data", "d", false),
+      showFlags("Flags", "show_flags", "l", true),
+      showFollow("Follow", "show_follow", "f", false),
+      showFootprints("Footprints", "show_footprints", "o", false),
+      showGrid("Grid", "show_grid", "g", true),
+      showOccupancy("Debug/Occupancy", "show_occupancy", "^o", false),
+      showStatus("Status", "show_status", "s", true),
+      showTrailArrows("Trails/Rising Arrows", "show_trailarrows", "^a", false),
+      showTrailRise("Trails/Rising blocks", "show_trailrise", "^r", false),
+      showTrails("Trails/Fast", "show_trailfast", "^f", false),
+      showVoxels("Debug/Voxels", "show_voxels", "^v", false),
+      pCamOn("Perspective camera", "pcam_on", "r", false),
+      visualizeAll("Selected only", "vis_all", "v", false),
       // and the rest
-      graphics(true), world(world), frames_rendered_count(0), screenshot_frame_skip(1)
+      graphics(true), world(world), frames_rendered_count(0)
 {
   perspective_camera.setPose(0.0, -4.0, 3.0);
   current_camera = &camera;
@@ -396,22 +394,22 @@ void Canvas::DrawGlobalGrid()
 
   for (double i = 0; i < bounds.x.max; i += skip) {
     snprintf(str, 16, "%d", (int)i);
-    Gl::draw_string(i, 0, 0, str);
+    this->draw_string(i, 0, 0, str);
   }
 
   for (double i = 0; i >= bounds.x.min; i -= skip) {
     snprintf(str, 16, "%d", (int)i);
-    Gl::draw_string(i, 0, 0, str);
+    this->draw_string(i, 0, 0, str);
   }
 
   for (double i = 0; i < bounds.y.max; i += skip) {
     snprintf(str, 16, "%d", (int)i);
-    Gl::draw_string(0, i, 0, str);
+    this->draw_string(0, i, 0, str);
   }
 
   for (double i = 0; i >= bounds.y.min; i -= skip) {
     snprintf(str, 16, "%d", (int)i);
-    Gl::draw_string(0, i, 0, str);
+    this->draw_string(0, i, 0, str);
   }
 
   PopColor();
@@ -559,7 +557,7 @@ void Canvas::renderFrame()
       char str[128];
       snprintf(str, 128, "(%d,%d)", world->rt_cells[i].x, world->rt_cells[i].y);
 
-      Gl::draw_string(world->rt_cells[i].x + 1, world->rt_cells[i].y + 1, 0.1, str);
+      this->draw_string(world->rt_cells[i].x + 1, world->rt_cells[i].y + 1, 0.1, str);
 
       // printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y
       // );
@@ -668,7 +666,7 @@ void Canvas::renderFrame()
   // LISTMETHOD( models_sorted, Model*, DrawWaypoints );
 
   FOR_EACH (it, selected_models)
-    (*it)->DrawSelected();
+    (*it)->DrawSelected(this);
 
   // useful debug - puts a point at the origin of each model
   // for( GList* it = world->World::children; it; it=it->next )
@@ -679,19 +677,19 @@ void Canvas::renderFrame()
     if (showData) {
       if (!visualizeAll) {
         FOR_EACH (it, world->World::children)
-          (*it)->DataVisualizeTree(current_camera);
+          (*it)->DataVisualizeTree(current_camera, this);
       } else if (selected_models.size() > 0) {
         FOR_EACH (it, selected_models)
-          (*it)->DataVisualizeTree(current_camera);
+          (*it)->DataVisualizeTree(current_camera, this);
       } else if (last_selection) {
-        last_selection->DataVisualizeTree(current_camera);
+        last_selection->DataVisualizeTree(current_camera, this);
       }
     }
   }
 
   if (showGrid)
     FOR_EACH (it, models_sorted)
-      (*it)->DrawGrid();
+      (*it)->DrawGrid(this);
 
   if (showStatus) {
     glPushMatrix();
@@ -700,7 +698,7 @@ void Canvas::renderFrame()
       glTranslatef(0, 0, 0.1);
 
     FOR_EACH (it, models_sorted)
-      (*it)->DrawStatusTree(&camera);
+      (*it)->DrawStatusTree(&camera, this);
 
     glPopMatrix();
   }
@@ -739,10 +737,10 @@ void Canvas::renderFrame()
     if (showFollow == true && last_selection)
       clockstr.append(" [FOLLOW MODE]");
 
-    float txtWidth = Gl::gl_width(clockstr.c_str());
+    float txtWidth = this->fontWidth(clockstr.c_str());
     if (txtWidth < 200)
       txtWidth = 200;
-    int txtHeight = Gl::gl_height();
+    int txtHeight = this->fontHeight();
 
     const int margin = 5;
     int width, height;
@@ -757,7 +755,7 @@ void Canvas::renderFrame()
     //char buf[ clockstr.size() ];
     //strcpy( buf, clockstr.c_str() );
 
-    Gl::draw_string(margin, margin, 0, clockstr.c_str() );
+    this->draw_string(margin, margin, 0, clockstr.c_str() );
     colorstack.Pop();
     colorstack.Pop();
 
@@ -781,9 +779,6 @@ void Canvas::renderFrame()
     glMatrixMode(GL_MODELVIEW);
   }
 
-  if (showScreenshots && (frames_rendered_count % screenshot_frame_skip == 0))
-    Screenshot();
-
   frames_rendered_count++;
 }
 
@@ -810,61 +805,16 @@ void Canvas::LeaveScreenCS()
   glMatrixMode(GL_MODELVIEW);
 }
 
-void Canvas::Screenshot()
+void Canvas::switchCameraMode(bool mode)
 {
-  int width = getWidth();
-  int height = getHeight();
-  int depth = 4; // RGBA
-
-  // we use RGBA throughout, though we only need RGB, as the 4-byte
-  // pixels avoid a nasty word-alignment problem when indexing into
-  // the pixel array.
-
-  // might save a bit of time with a static var as the image size rarely changes
-  static std::vector<uint8_t> pixels;
-  pixels.resize(width * height * depth);
-
-  glFlush(); // make sure the drawing is done
-  // read the pixels from the screen
-  glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, &pixels[0]);
-
-  static uint32_t count = 0;
-  char filename[64];
-  snprintf(filename, 63, "stage-%06d.png", count++);
-
-  FILE *fp = fopen(filename, "wb");
-  if (fp == NULL) {
-    PRINT_ERR1("Unable to open %s", filename);
-  }
-
-  // create PNG data
-  png_structp pp = png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
-  assert(pp);
-  png_infop info = png_create_info_struct(pp);
-  assert(info);
-
-  // setup the output file
-  png_init_io(pp, fp);
-
-  // need to invert the image as GL and PNG disagree on the row order
-  png_bytep *rowpointers = new png_bytep[height];
-  for (int i = 0; i < height; i++)
-    rowpointers[i] = &pixels[(height - 1 - i) * width * depth];
-
-  png_set_rows(pp, info, rowpointers);
-
-  png_set_IHDR(pp, info, width, height, 8, PNG_COLOR_TYPE_RGBA, PNG_INTERLACE_NONE,
-               PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);
-
-  png_write_png(pp, info, PNG_TRANSFORM_IDENTITY, NULL);
-
-  // free the PNG data - we reuse the pixel array next call.
-  png_destroy_write_struct(&pp, &info);
-
-  fclose(fp);
-
-  printf("Saved %s\n", filename);
-  delete[] rowpointers;
+	if (mode)
+	{
+		// Perspective mode is on, change camera
+		this->current_camera = &this->perspective_camera;
+	} else {
+		this->current_camera = &this->camera;
+	}
+	setInvalidate();
 }
 
 void Canvas::Load(Worldfile *wf, int sec)
@@ -875,10 +825,6 @@ void Canvas::Load(Worldfile *wf, int sec)
 
   interval = wf->ReadInt(sec, "interval", interval);
 
-  screenshot_frame_skip = wf->ReadInt(sec, "screenshot_skip", screenshot_frame_skip);
-  if (screenshot_frame_skip < 1)
-    screenshot_frame_skip = 1; // avoids div-by-zero if poorly set
-
   showData.Load(wf, sec);
   showFlags.Load(wf, sec);
   showBlocks.Load(wf, sec);
@@ -893,7 +839,6 @@ void Canvas::Load(Worldfile *wf, int sec)
   showTrailRise.Load(wf, sec);
   showTrails.Load(wf, sec);
   // showVoxels.Load( wf, sec );
-  showScreenshots.Load(wf, sec);
   pCamOn.Load(wf, sec);
 }
 
@@ -918,7 +863,6 @@ void Canvas::Save(Worldfile *wf, int sec)
   showTrailRise.Save(wf, sec);
   showTrails.Save(wf, sec);
   showVoxels.Save(wf, sec);
-  showScreenshots.Save(wf, sec);
   pCamOn.Save(wf, sec);
 }
 
@@ -962,3 +906,83 @@ void Canvas::draw()
   renderFrame();
 }
 
+void Canvas::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset,
+                         float min, float max)
+{
+  float sample_spacing = w / (float)len;
+  float yscale = h / (max - min);
+
+  // printf( "min %.2f max %.2f\n", min, max );
+
+  glBegin(GL_LINE_STRIP);
+
+  for (unsigned int i = 0; i < len; i++)
+    glVertex3f(x + (float)i * sample_spacing, y + (data[(i + offset) % len] - min) * yscale, 0.01);
+
+  glEnd();
+
+  glColor3f(0, 0, 0);
+  char buf[64];
+  snprintf(buf, 63, "%.2f", min);
+  this->draw_string(x, y, 0, buf);
+  snprintf(buf, 63, "%.2f", max);
+  this->draw_string(x, y + h - this->getHeight(), 0, buf);
+}
+
+void Canvas::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset)
+{
+  // wild initial bounds
+  float smallest = 1e16;
+  float largest = -1e16;
+
+  for (size_t i = 0; i < len; i++) {
+    smallest = std::min(smallest, data[i]);
+    largest = std::max(largest, data[i]);
+  }
+
+  draw_array(x, y, w, h, data, len, offset, smallest, largest);
+}
+
+void Canvas::draw_speech_bubble(float x, float y, float z, const char *str)
+{
+  draw_string(x, y, z, str);
+}
+
+void Canvas::draw_string(float x, float y, float z, const char *string)
+{
+	// TODO: To be implemented in child class
+}
+
+void Canvas::draw_string_multiline(float x, float y, float w, float h, const char *string, int align)
+{
+	// TODO: To be implemented in child class
+}
+
+void Canvas::draw_grid(bounds3d_t vol)
+{
+  glBegin(GL_LINES);
+
+  for (double i = floor(vol.x.min); i < vol.x.max; i++) {
+    glVertex2f(i, vol.y.min);
+    glVertex2f(i, vol.y.max);
+  }
+
+  for (double i = floor(vol.y.min); i < vol.y.max; i++) {
+    glVertex2f(vol.x.min, i);
+    glVertex2f(vol.x.max, i);
+  }
+
+  glEnd();
+
+  char str[16];
+
+  for (double i = floor(vol.x.min); i < vol.x.max; i++) {
+    snprintf(str, 16, "%d", (int)i);
+    draw_string(i, 0, 0.00, str);
+  }
+
+  for (double i = floor(vol.y.min); i < vol.y.max; i++) {
+    snprintf(str, 16, "%d", (int)i);
+    draw_string(0, i, 0.00, str);
+  }
+}
diff --git a/libstage/canvas.hh b/libstage/canvas.hh
index 100d5ddac..3e21b0705 100644
--- a/libstage/canvas.hh
+++ b/libstage/canvas.hh
@@ -13,7 +13,7 @@ class Canvas {
   friend class WorldGui; // allow access to private members
   friend class Model;
 
-private:
+protected:
   class GlColorStack {
   public:
     GlColorStack() : colorstack() {}
@@ -49,16 +49,14 @@ private:
   PerspectiveCamera perspective_camera;
   bool dirty_buffer;
   Worldfile *wf;
-
-  int startx, starty;
-  bool selectedModel;
   bool clicked_empty_space;
-  int empty_space_startx, empty_space_starty;
   std::list<Model *> selected_models;
   Model *last_selection; ///< the most recently selected model
   ///(even if it is now unselected).
 
   msec_t interval; // window refresh interval in ms
+  bool init_done;
+
 
   void RecordRay(double x1, double y1, double x2, double y2);
   void DrawRays();
@@ -71,7 +69,7 @@ private:
   Option // showBlinken,
       showBBoxes,
       showBlocks, showBlur, showClock, showData, showFlags, showFollow, showFootprints, showGrid,
-      showOccupancy, showScreenshots, showStatus, showTrailArrows, showTrailRise, showTrails,
+      showOccupancy, showStatus, showTrailArrows, showTrailRise, showTrails,
       showVoxels, pCamOn, visualizeAll;
 
 public:
@@ -81,11 +79,9 @@ public:
   bool graphics;
   World *world;
   unsigned long frames_rendered_count;
-  int screenshot_frame_skip;
 
   std::map<std::string, Option *> _custom_options;
 
-  void Screenshot();
   void InitGl();
   void InitTextures();
 
@@ -97,12 +93,34 @@ public:
   virtual void renderFrame();
   virtual void draw();
 
+  /** Switch between camera modes
+   * @param mode - camera mode:
+   *  - true: perspective camera
+   *  - false: default camera
+   */
+  void switchCameraMode(bool mode);
+
   virtual bool isValid() {return false;}
   virtual void doRedraw() {}
   virtual int getWidth() const {return 1;}
   virtual int getHeight() const {return 1;}
   virtual void setInvalidate() {}
 
+  msec_t getRedrawInterval() const {return interval; }
+
+  /// Wraps interaction with current font geometry
+  virtual double fontWidth(const char * str) const {return 0;};
+  virtual double fontHeight() const {return 0;};
+
+  virtual void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset, float min, float max);
+  virtual void draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset);
+
+  virtual void draw_string(float x, float y, float z, const char *string);
+  virtual void draw_string_multiline(float x, float y, float w, float h, const char *string, int align);
+
+  void draw_speech_bubble(float x, float y, float z, const char *str);
+  void draw_grid(bounds3d_t vol);
+
   void CanvasToWorld(int px, int py, double *wx, double *wy, double *wz);
 
   Model *getModel(int x, int y);
@@ -119,14 +137,14 @@ public:
   void InvertView(uint32_t invertflags);
 
   bool VisualizeAll() { return !visualizeAll; }
-  static void TimerCallback(Canvas *canvas);
+  //static void TimerCallback(Canvas *canvas);
   //static void perspectiveCb(Fl_Widget *w, void *p);
 
   void EnterScreenCS();
   void LeaveScreenCS();
 
-  void Load(Worldfile *wf, int section);
-  void Save(Worldfile *wf, int section);
+  virtual void Load(Worldfile *wf, int section);
+  virtual void Save(Worldfile *wf, int section);
 
   bool IsTopView() { return ((fabs(camera.yaw()) < 0.1) && (fabs(camera.pitch()) < 0.1)); }
 };
diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index b3193b92f..252e00790 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -1,4 +1,7 @@
 #include "canvas_fltk.hh"
+#include <FL/Fl.H>
+#include <FL/gl.h>
+#include "png.h"
 
 namespace Stg
 {
@@ -6,7 +9,12 @@ namespace Stg
 //Fl_Gl_Window(x, y, width, height),
 
 CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
-:Canvas(world, x, y, width, height), screenshot_frame_skip(1)
+:Canvas(world, x, y, width, height)
+,Fl_Gl_Window(x, y, width, height), selectedModel(false), startx(-1), starty(-1),
+ empty_space_startx(0), empty_space_starty(0),
+ showScreenshots("Save screenshots", "screenshots", "", false),
+ menu_manager(NULL),
+ graphics(true), screenshot_frame_skip(1)
 {
   end();
   // show(); // must do this so that the GL context is created before
@@ -15,59 +23,171 @@ CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
   mode( FL_RGB |FL_DOUBLE|FL_DEPTH| FL_MULTISAMPLE | FL_ALPHA );
 }
 
+CanvasFLTK::~CanvasFLTK()
+{
+
+}
+
 void CanvasFLTK::perspectiveCb(Option *opt, void *p)
 {
   Canvas *canvas = static_cast<Canvas *>(p);
-  if (opt) {
-    // Perspective mode is on, change camera
-    canvas->current_camera = &canvas->perspective_camera;
-  } else {
-    canvas->current_camera = &canvas->camera;
-  }
-
-  canvas->invalidate();
+  canvas->switchCameraMode(opt->val());
 }
 
 void CanvasFLTK::createMenuItems(Fl_Menu_Bar *menu, std::string path)
 {
-	createMenuItem(showData.menu, path);
-  // createMenuItem(visualizeAll, menu, path );
-	createMenuItem(showBlocks,menu, path);
-	createMenuItem(showFlags, menu, path);
-	createMenuItem(showClock, menu, path);
-	createMenuItem(showFlags, menu, path);
-	createMenuItem(showFollow, menu, path);
-	createMenuItem(showFootprints, menu, path);
-	createMenuItem(showGrid, menu, path);
-	createMenuItem(showStatus, menu, path);
-	createMenuItem(pCamOn, menu, path);
+	assert(this->menu_manager == NULL);
+	MenuManagerFLTK * mm = new MenuManagerFLTK(this, menu);
+	menu_manager = mm;
+	mm->createMenuItem(showData, path);
+  // mm->createMenuItem(visualizeAll, path );
+	mm->createMenuItem(showBlocks, path);
+	mm->createMenuItem(showFlags, path);
+	mm->createMenuItem(showClock, path);
+	mm->createMenuItem(showFlags, path);
+	mm->createMenuItem(showFollow, path);
+	mm->createMenuItem(showFootprints, path);
+	mm->createMenuItem(showGrid, path);
+	mm->createMenuItem(showStatus, path);
+	mm->createMenuItem(pCamOn, path);
 	pCamOn.setOptionCallback(perspectiveCb, this);
-	createMenuItem(showOccupancy, menu, path);
-	createMenuItem(showTrailArrows, menu, path);
-	createMenuItem(showTrails, menu, path);
-	createMenuItem(showTrailRise, menu, path); // broken
-	createMenuItem(showBBoxes, menu, path);
-	// createMenuItem(showVoxels, menu, path );
-	createMenuItem(showScreenshots, menu, path);
+	mm->createMenuItem(showOccupancy, path);
+	mm->createMenuItem(showTrailArrows, path);
+	mm->createMenuItem(showTrails, path);
+	mm->createMenuItem(showTrailRise, path); // broken
+	mm->createMenuItem(showBBoxes, path);
+	// mm->createMenuItem(showVoxels, path );
+	mm->createMenuItem(showScreenshots, path);
+}
+
+Fl_Menu_Item *getMenuItem(Fl_Menu_ *menu, int i)
+{
+  const Fl_Menu_Item *mArr = menu->menu();
+  return const_cast<Fl_Menu_Item *>(&mArr[i]);
+}
+
+MenuManagerFLTK::MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu)
+	:canvas(canvas), menu(menu)
+{
+
+}
+
+CanvasFLTK * MenuManagerFLTK::GetCanvas()
+{
+	return canvas;
+}
+
+void MenuManagerFLTK::createMenuItem(Option & opt, std::string path)
+{
+  path = path + "/" + opt.name();
+  std::string key = opt.key();
+  // create a menu item and save its index
+  size_t index = menu->add(path.c_str(), key.c_str(), toggleCb, &opt, FL_MENU_TOGGLE | (opt.val() ? FL_MENU_VALUE : 0));
+  opt.onAttachGUI(this, reinterpret_cast<void*>(index));
+}
+
+void MenuManagerFLTK::syncMenuState(Option * opt)
+{
+	size_t index = reinterpret_cast<size_t>(opt->getWidgetData());
+
+  if (menu != NULL) {
+    Fl_Menu_Item *item = getMenuItem(menu, index);
+    if(item != NULL) {
+    	opt->val() ? item->set() : item->clear();
+    }
+  }
+
+  CanvasFLTK *canvas = GetCanvas();
+  if (canvas != NULL)
+  {
+    canvas->setInvalidate();
+    canvas->redraw();
+  }
 }
 
-void CanvasFLTK::toggleCb(Fl_Widget *, void *p)
+void MenuManagerFLTK::toggleCb(Fl_Widget *, void *p)
 {
   // Fl_Menu_* menu = static_cast<Fl_Menu_*>( w );
   Option *opt = static_cast<Option *>(p);
   opt->invert();
-  if (opt->menuCb)
-    opt->menuCb(opt, opt->callbackData);
+  opt->callValueChanged();
 }
 
-void CanvasFLTK::createMenuItem(Option * opt, Fl_Menu_Bar *m, std::string path)
+double CanvasFLTK::fontWidth(const char * str) const
 {
-  opt->menu = m;
-  path = path + "/" + optName;
-  // create a menu item and save its index
-  opt->menuIndex = menu->add(path.c_str(), shortcut.c_str(), toggleCb, opt, FL_MENU_TOGGLE | (value ? FL_MENU_VALUE : 0));
+	return gl_width(str);
 }
 
+double CanvasFLTK::fontHeight() const
+{
+	return gl_height();
+}
+
+void CanvasFLTK::Screenshot()
+{
+  int width = getWidth();
+  int height = getHeight();
+  int depth = 4; // RGBA
+
+  // we use RGBA throughout, though we only need RGB, as the 4-byte
+  // pixels avoid a nasty word-alignment problem when indexing into
+  // the pixel array.
+
+  // might save a bit of time with a static var as the image size rarely changes
+  static std::vector<uint8_t> pixels;
+  pixels.resize(width * height * depth);
+
+  glFlush(); // make sure the drawing is done
+  // read the pixels from the screen
+  glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, &pixels[0]);
+
+  static uint32_t count = 0;
+  char filename[64];
+  snprintf(filename, 63, "stage-%06d.png", count++);
+
+  FILE *fp = fopen(filename, "wb");
+  if (fp == NULL) {
+    PRINT_ERR1("Unable to open %s", filename);
+  }
+
+  // create PNG data
+  png_structp pp = png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
+  assert(pp);
+  png_infop info = png_create_info_struct(pp);
+  assert(info);
+
+  // setup the output file
+  png_init_io(pp, fp);
+
+  // need to invert the image as GL and PNG disagree on the row order
+  png_bytep *rowpointers = new png_bytep[height];
+  for (int i = 0; i < height; i++)
+    rowpointers[i] = &pixels[(height - 1 - i) * width * depth];
+
+  png_set_rows(pp, info, rowpointers);
+
+  png_set_IHDR(pp, info, width, height, 8, PNG_COLOR_TYPE_RGBA, PNG_INTERLACE_NONE,
+               PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);
+
+  png_write_png(pp, info, PNG_TRANSFORM_IDENTITY, NULL);
+
+  // free the PNG data - we reuse the pixel array next call.
+  png_destroy_write_struct(&pp, &info);
+
+  fclose(fp);
+
+  printf("Saved %s\n", filename);
+  delete[] rowpointers;
+}
+
+
+void CanvasFLTK::renderFrame()
+{
+	Canvas::renderFrame();
+
+	if (showScreenshots && (frames_rendered_count % screenshot_frame_skip == 0))
+	    Screenshot();
+}
 
 int CanvasFLTK::handle(int event)
 {
@@ -316,21 +436,58 @@ void CanvasFLTK::resize(int X, int Y, int W, int H)
 void CanvasFLTK::Load(Worldfile *wf, int sec)
 {
 	Canvas::Load(wf, sec);
+
+	showScreenshots.Load(wf, sec);
+
+	screenshot_frame_skip = wf->ReadInt(sec, "screenshot_skip", screenshot_frame_skip);
+	if (screenshot_frame_skip < 1)
+		screenshot_frame_skip = 1; // avoids div-by-zero if poorly set
+
   if (!world->paused)
     // // start the timer that causes regular redraws
-    Fl::add_timeout(((double)interval / 1000), (Fl_Timeout_Handler)Canvas::TimerCallback, this);
+    Fl::add_timeout(((double)interval / 1000), (Fl_Timeout_Handler)CanvasFLTK::TimerCallback, this);
 
   invalidate(); // we probably changed something
 }
 
+void CanvasFLTK::Save(Worldfile *wf, int sec)
+{
+	wf->WriteInt(sec, "screenshot_skip", screenshot_frame_skip);
+	showScreenshots.Save(wf, sec);
+	Canvas::Save(wf, sec);
+}
+
 void CanvasFLTK::TimerCallback(Canvas *c)
 {
-  if (c->world->dirty) {
+  if (c->world->IsDirty())
+  {
     // puts( "timer redraw" );
     c->doRedraw();
-    c->world->dirty = false;
+    c->world->SetDirty(false);
   }
-  Fl::repeat_timeout(c->interval / 1000.0, (Fl_Timeout_Handler)Canvas::TimerCallback, c);
+  msec_t interval = c->getRedrawInterval();
+  Fl::repeat_timeout(interval / 1000.0, (Fl_Timeout_Handler)CanvasFLTK::TimerCallback, c);
+}
+
+void CanvasFLTK::draw_string(float x, float y, float z, const char *str)
+{
+  glRasterPos3f(x, y, z);
+
+  GLboolean b;
+  glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &b);
+
+  // printf( "[%.2f %.2f %.2f] %d string %u %s\n", x,y,z, (int)b, (unsigned
+  // int)strlen(str), str );
+
+  if (b)
+    gl_draw(str); // fltk function
+}
+
+void CanvasFLTK::draw_string_multiline(float x, float y, float w, float h, const char *str, int align)
+{
+  // printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str),
+  // str );
+  gl_draw(str, x, y, w, h, align); // fltk function
 }
 
 }
diff --git a/libstage/canvas_fltk.hh b/libstage/canvas_fltk.hh
index c23b9e515..195cf06fc 100644
--- a/libstage/canvas_fltk.hh
+++ b/libstage/canvas_fltk.hh
@@ -5,16 +5,40 @@
 #include <FL/Fl_Menu_Bar.H>
 
 #include "canvas.hh"
+#include "world_gui.hh"
 
 namespace Stg {
+
+class CanvasFLTK;
+
+class MenuManagerFLTK : public MenuManager
+{
+public:
+	MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu);
+	virtual void syncMenuState(Option * opt);
+	/** Creates menu item for an option */
+	void createMenuItem(Option & opt, std::string path);
+protected:
+	/** Toggle callback from FLTK */
+	static void toggleCb(Fl_Widget *w, void *p);
+	CanvasFLTK * GetCanvas();
+
+	Fl_Menu_ *menu;
+	CanvasFLTK * canvas;
+};
+
 class CanvasFLTK : public Canvas, public Fl_Gl_Window {
   friend class WorldGui; // allow access to private members
   friend class Model;
-
 protected:
-  void createMenuItem(Option * opt, Fl_Menu_Bar *menu, std::string path);
-  static void toggleCb(Fl_Widget *w, void *p);
+  /** State flag for drag&drop*/
+  bool selectedModel;
+  int startx, starty;
+  int empty_space_startx, empty_space_starty;
+
+  Option showScreenshots;
 
+  MenuManagerFLTK * menu_manager;
 public:
   CanvasFLTK(WorldGui *world, int x, int y, int width, int height);
   ~CanvasFLTK();
@@ -23,14 +47,10 @@ public:
   int screenshot_frame_skip;
 
   void Screenshot();
-  void InitGl();
   void InitTextures();
   void createMenuItems(Fl_Menu_Bar *menu, std::string path);
 
-  void FixViewport(int W, int H);
-  void resetCamera();
   virtual void renderFrame();
-  virtual void draw();
   virtual int handle(int event);
   void resize(int X, int Y, int W, int H);
 
@@ -40,12 +60,19 @@ public:
 	virtual int getHeight() const {return h();}
 	virtual void setInvalidate() { invalidate(); }
 
-  void CanvasToWorld(int px, int py, double *wx, double *wy, double *wz);
+	double fontWidth(const char * str) const;
+	double fontHeight() const;
+
+	void draw_string(float x, float y, float z, const char *str);
+	void draw_string_multiline(float x, float y, float w, float h, const char *str, int align);
 
   void InvertView(uint32_t invertflags);
 
   static void TimerCallback(Canvas *canvas);
   static void perspectiveCb(Option * opt, void *p);
+
+  void Load(Worldfile *wf, int sec);
+  void Save(Worldfile *wf, int section);
 };
 } //namespace Stg
 
diff --git a/libstage/gl.cc b/libstage/gl.cc
index 2a8337958..c3e1e1e7e 100644
--- a/libstage/gl.cc
+++ b/libstage/gl.cc
@@ -2,10 +2,6 @@
 
 using namespace Stg;
 
-float view_height();
-void gl_draw(const char * text, float x, float y, float w, float h, int align);
-void gl_draw(const char * text);
-
 // transform the current coordinate frame by the given pose
 void Stg::Gl::coord_shift(double x, double y, double z, double a)
 {
@@ -25,69 +21,6 @@ void Stg::Gl::pose_inverse_shift(const Pose &pose)
   coord_shift(-pose.x, -pose.y, -pose.z, 0);
 }
 
-void Stg::Gl::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset,
-                         float min, float max)
-{
-  float sample_spacing = w / (float)len;
-  float yscale = h / (max - min);
-
-  // printf( "min %.2f max %.2f\n", min, max );
-
-  glBegin(GL_LINE_STRIP);
-
-  for (unsigned int i = 0; i < len; i++)
-    glVertex3f(x + (float)i * sample_spacing, y + (data[(i + offset) % len] - min) * yscale, 0.01);
-
-  glEnd();
-
-  glColor3f(0, 0, 0);
-  char buf[64];
-  snprintf(buf, 63, "%.2f", min);
-  Gl::draw_string(x, y, 0, buf);
-  snprintf(buf, 63, "%.2f", max);
-  Gl::draw_string(x, y + h - view_height(), 0, buf);
-}
-
-void Stg::Gl::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset)
-{
-  // wild initial bounds
-  float smallest = 1e16;
-  float largest = -1e16;
-
-  for (size_t i = 0; i < len; i++) {
-    smallest = std::min(smallest, data[i]);
-    largest = std::max(largest, data[i]);
-  }
-
-  draw_array(x, y, w, h, data, len, offset, smallest, largest);
-}
-
-void Stg::Gl::draw_string(float x, float y, float z, const char *str)
-{
-  glRasterPos3f(x, y, z);
-
-  GLboolean b;
-  glGetBooleanv(GL_CURRENT_RASTER_POSITION_VALID, &b);
-
-  // printf( "[%.2f %.2f %.2f] %d string %u %s\n", x,y,z, (int)b, (unsigned
-  // int)strlen(str), str );
-
-  if (b)
-    gl_draw(str); // fltk function
-}
-
-void Stg::Gl::draw_string_multiline(float x, float y, float w, float h, const char *str, int align)
-{
-  // printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str),
-  // str );
-  gl_draw(str, x, y, w, h, align); // fltk function
-}
-
-void Stg::Gl::draw_speech_bubble(float x, float y, float z, const char *str)
-{
-  draw_string(x, y, z, str);
-}
-
 // draw an octagon with center rectangle dimensions w/h
 //   and outside margin m
 void Stg::Gl::draw_octagon(float w, float h, float m)
@@ -139,32 +72,3 @@ void Stg::Gl::draw_origin(double len)
   draw_vector(0, len, 0);
   draw_vector(0, 0, len);
 }
-
-void Stg::Gl::draw_grid(bounds3d_t vol)
-{
-  glBegin(GL_LINES);
-
-  for (double i = floor(vol.x.min); i < vol.x.max; i++) {
-    glVertex2f(i, vol.y.min);
-    glVertex2f(i, vol.y.max);
-  }
-
-  for (double i = floor(vol.y.min); i < vol.y.max; i++) {
-    glVertex2f(vol.x.min, i);
-    glVertex2f(vol.x.max, i);
-  }
-
-  glEnd();
-
-  char str[16];
-
-  for (double i = floor(vol.x.min); i < vol.x.max; i++) {
-    snprintf(str, 16, "%d", (int)i);
-    draw_string(i, 0, 0.00, str);
-  }
-
-  for (double i = floor(vol.y.min); i < vol.y.max; i++) {
-    snprintf(str, 16, "%d", (int)i);
-    draw_string(0, i, 0.00, str);
-  }
-}
diff --git a/libstage/image.cc b/libstage/image.cc
index a263a0b3f..dfbcc2533 100644
--- a/libstage/image.cc
+++ b/libstage/image.cc
@@ -1,4 +1,5 @@
 #include "image.hh"
+#include <png.h>
 #include <cassert>
 #include <stdlib.h>
 
@@ -38,10 +39,90 @@ uint8_t * Image::getData()
 	return &data[0];
 }
 
-bool Image::load(const char * path)
+bool Image::load(const char * file_name)
 {
 	assert(false);
-	return false;
+	/* open file and test for it being a png */
+	FILE *fp = fopen(file_name, "rb");
+
+	bool done = false;
+	if (!fp)
+	{
+		printf("[read_png_file] File %s could not be opened for reading\n", file_name);
+		return false;
+	}
+
+	do
+	{
+		unsigned char header[8];    // 8 is the maximum size that can be checked
+		fread(header, 1, 8, fp);
+		if (png_sig_cmp(header, 0, 8))
+		{
+			printf("[read_png_file] File %s is not recognized as a PNG file\n", file_name);
+			break;
+		}
+
+		/* initialize stuff */
+		png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+
+		if (!png_ptr)
+		{
+			printf("[read_png_file] png_create_read_struct failed\n");
+			break;
+		}
+
+		png_infop info_ptr = png_create_info_struct(png_ptr);
+		if (!info_ptr)
+		{
+			printf("[read_png_file] png_create_info_struct failed\n");
+			break;
+		}
+
+		//if (setjmp(png_jmpbuf(png_ptr)))
+		//		abort_("[read_png_file] Error during init_io");
+
+		png_init_io(png_ptr, fp);
+		png_set_sig_bytes(png_ptr, 8);
+
+		png_read_info(png_ptr, info_ptr);
+
+		;
+
+		width = png_get_image_width(png_ptr, info_ptr);
+		height = png_get_image_height(png_ptr, info_ptr);
+		png_byte color_type = png_get_color_type(png_ptr, info_ptr);
+		png_byte bit_depth = png_get_bit_depth(png_ptr, info_ptr);
+		//number_of_passes = png_set_interlace_handling(png_ptr);
+		png_read_update_info(png_ptr, info_ptr);
+
+
+		/* read file */
+		if (setjmp(png_jmpbuf(png_ptr)))
+		{
+			printf("[read_png_file] Error during read_image");
+			break;
+		}
+
+		int row_length = png_get_rowbytes(png_ptr, info_ptr);
+		std::vector<uint8_t> row_data(row_length);
+
+		this->data.resize(width*height*bpp);
+		for (int y=0; y<height; y++)
+		{
+			//row_pointers[y] = (png_byte*) malloc(png_get_rowbytes(png_ptr,info_ptr));
+			png_bytep row_pointer = &row_data[0];
+			png_read_row(png_ptr, row_pointer, NULL);
+			uint8_t * dst = &this->data[y*width*bpp];
+			memcpy(row_pointer, dst, width*bpp);
+		}
+
+		//png_read_image(png_ptr, row_pointers);
+		done = true;
+	}while(false);
+
+	fclose(fp);
+
+	return done;
 }
 
 }
diff --git a/libstage/model.cc b/libstage/model.cc
index a7d32b482..e00a65c72 100644
--- a/libstage/model.cc
+++ b/libstage/model.cc
@@ -139,6 +139,7 @@
 #include "config.h" // for build-time config
 #include "file_manager.hh"
 #include "stage.hh"
+#include "canvas.hh"
 #include "worldfile.hh"
 using namespace Stg;
 
@@ -881,7 +882,7 @@ Model::RasterVis::RasterVis()
 {
 }
 
-void Model::RasterVis::Visualize(Model *mod, Camera *cam)
+void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -910,7 +911,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam)
 
       char buf[128];
       snprintf(buf, 127, "[%.2f x %.2f]", pt.x, pt.y);
-      Gl::draw_string(pt.x, pt.y, 0, buf);
+      canvas->draw_string(pt.x, pt.y, 0, buf);
     }
     glEnd();
 
@@ -957,7 +958,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam)
   char buf[128];
   snprintf(buf, 127, "[%u x %u]", width, height);
   glTranslatef(0, 0, 0.01);
-  Gl::draw_string(1, height - 1, 0, buf);
+  canvas->draw_string(1, height - 1, 0, buf);
 
   mod->PopColor();
 
diff --git a/libstage/model_blinkenlight.cc b/libstage/model_blinkenlight.cc
index aa840606c..f4ebeed8c 100644
--- a/libstage/model_blinkenlight.cc
+++ b/libstage/model_blinkenlight.cc
@@ -56,7 +56,7 @@ enabled 1
 using namespace Stg;
 
 // TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelBlinkenlight::showBlinkenData("Show Blink", "show_blinken", "", true, NULL);
+Option ModelBlinkenlight::showBlinkenData("Show Blink", "show_blinken", "", true);
 
 ModelBlinkenlight::ModelBlinkenlight(World *world, Model *parent, const std::string &type)
     : Model(world, parent, type), dutycycle(1.0), enabled(true), period(1000), on(true)
diff --git a/libstage/model_blobfinder.cc b/libstage/model_blobfinder.cc
index 8c9fdddfe..ca8fcd119 100644
--- a/libstage/model_blobfinder.cc
+++ b/libstage/model_blobfinder.cc
@@ -287,7 +287,7 @@ ModelBlobfinder::Vis::Vis(World *) : Visualizer("Blobfinder", "blobfinder_vis")
   // world->RegisterOption( &showBeams );
 }
 
-void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam)
+void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   ModelBlobfinder *bf(dynamic_cast<ModelBlobfinder *>(mod));
 
diff --git a/libstage/model_bumper.cc b/libstage/model_bumper.cc
index a4a48afc4..80e5cc794 100644
--- a/libstage/model_bumper.cc
+++ b/libstage/model_bumper.cc
@@ -65,7 +65,7 @@ static const watts_t BUMPER_WATTS = 0.1; // bumper power consumption
 static const Color BUMPER_HIT_RGB(1, 0, 0, 1); // red
 static const Color BUMPER_NOHIT_RGB(0, 1, 0, 1);// green
 
-Option ModelBumper::showBumperData("Show Bumper Data", "show_bumper", "", true, NULL);
+Option ModelBumper::showBumperData("Show Bumper Data", "show_bumper", "", true);
 
 ModelBumper::ModelBumper(World *world, Model *parent, const std::string &type)
     : Model(world, parent, type), bumpervis()
@@ -213,7 +213,7 @@ ModelBumper::BumperVis::~BumperVis()
   // Nothing to do here
 }
 
-void ModelBumper::BumperVis::Visualize(Model *mod, Camera *)
+void ModelBumper::BumperVis::Visualize(Model *mod, Camera *camera, Canvas * canvas)
 {
   ModelBumper *bump = dynamic_cast<ModelBumper *>(mod);
 
diff --git a/libstage/model_camera.cc b/libstage/model_camera.cc
index c84641d59..0d8556e64 100644
--- a/libstage/model_camera.cc
+++ b/libstage/model_camera.cc
@@ -22,7 +22,7 @@ using namespace Stg;
 #include <sstream>
 
 // TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelCamera::showCameraData("Show Camera Data", "show_camera", "", true, NULL);
+Option ModelCamera::showCameraData("Show Camera Data", "show_camera", "", true);
 
 static const Stg::Size DEFAULT_SIZE(0.1, 0.07, 0.05);
 static const char DEFAULT_GEOM_COLOR[] = "black";
diff --git a/libstage/model_draw.cc b/libstage/model_draw.cc
index dc2b4b233..8e6d8667d 100644
--- a/libstage/model_draw.cc
+++ b/libstage/model_draw.cc
@@ -9,7 +9,7 @@ static const Color BUBBLE_FILL(1.0, 0.8, 0.8); // light blue/grey
 static const Color BUBBLE_BORDER(0, 0, 0); // black
 static const Color BUBBLE_TEXT(0, 0, 0); // black
 
-void Model::DrawSelected()
+void Model::DrawSelected(Canvas * canvas)
 {
   glPushMatrix();
 
@@ -21,7 +21,7 @@ void Model::DrawSelected()
   snprintf(buf, 63, "%s [%.2f %.2f %.2f %.2f]", Token(), gp.x, gp.y, gp.z, rtod(gp.a));
 
   PushColor(0, 0, 0, 1); // text color black
-  Gl::draw_string(0.5, 0.5, 0.5, buf);
+  canvas->draw_string(0.5, 0.5, 0.5, buf);
 
   glRotatef(rtod(pose.a), 0, 0, 1);
 
@@ -256,7 +256,7 @@ void Model::AddVisualizer(Visualizer *cv, bool on_by_default)
   std::map<std::string, Option *>::iterator i = canvas->_custom_options.find(cv->GetMenuName());
   if (i == canvas->_custom_options.end()) {
     Option *op =
-        new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default, world);
+        new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default);
     canvas->_custom_options[cv->GetMenuName()] = op;
     RegisterOption(op);
   }
@@ -271,16 +271,16 @@ void Model::RemoveVisualizer(Visualizer *cv)
   // attached to different models which have the same name
 }
 
-void Model::DrawStatusTree(Camera *cam)
+void Model::DrawStatusTree(Camera *cam, Canvas * canvas)
 {
   PushLocalCoords();
-  DrawStatus(cam);
+  DrawStatus(cam, canvas);
   FOR_EACH (it, children)
-    (*it)->DrawStatusTree(cam);
+    (*it)->DrawStatusTree(cam, canvas);
   PopCoords();
 }
 
-void Model::DrawStatus(Camera *cam)
+void Model::DrawStatus(Camera *cam, Canvas * canvas)
 {
   if (power_pack || !say_string.empty()) {
     float pitch = -cam->pitch();
@@ -314,8 +314,8 @@ void Model::DrawStatus(Camera *cam)
 
       if (valid) {
         // fl_font( FL_HELVETICA, 12 );
-        float w = Gl::gl_width(this->say_string.c_str()); // scaled text width
-        float h = Gl::gl_height(); // scaled text height
+        float w = canvas->fontWidth(this->say_string.c_str()); // scaled text width
+        float h = canvas->fontHeight(); // scaled text height
 
         GLdouble wx, wy, wz;
         GLint viewport[4];
@@ -357,7 +357,7 @@ void Model::DrawStatus(Camera *cam)
 
         PushColor(BUBBLE_TEXT);
         // draw text inside the bubble
-        Gl::draw_string(m, 2.5 * m, 0, this->say_string.c_str());
+        canvas->draw_string(m, 2.5 * m, 0, this->say_string.c_str());
         PopColor();
       }
     }
@@ -527,11 +527,11 @@ void Model::DataVisualize(Camera *cam)
   (void)cam; // avoid warning about unused var
 }
 
-void Model::DataVisualizeTree(Camera *cam)
+void Model::DataVisualizeTree(Camera *cam, Canvas * canvas)
 {
   PushLocalCoords();
 
-  Canvas * canvas = GetCanvas();
+  //Canvas * canvas = GetCanvas();
 
   if (subs > 0 && canvas != NULL) {
     DataVisualize(cam); // virtual function overridden by some model types
@@ -539,18 +539,18 @@ void Model::DataVisualizeTree(Camera *cam)
     FOR_EACH (it, cv_list) {
       Visualizer *vis = *it;
       if (canvas->_custom_options[vis->GetMenuName()]->isEnabled())
-        vis->Visualize(this, cam);
+        vis->Visualize(this, cam, canvas);
     }
   }
 
   // and draw the children
   FOR_EACH (it, children)
-    (*it)->DataVisualizeTree(cam);
+    (*it)->DataVisualizeTree(cam, canvas);
 
   PopCoords();
 }
 
-void Model::DrawGrid(void)
+void Model::DrawGrid(Canvas * canvas)
 {
   if (gui.grid) {
     PushLocalCoords();
@@ -564,7 +564,7 @@ void Model::DrawGrid(void)
     vol.z.max = geom.size.z;
 
     PushColor(0, 0, 1, 0.4);
-    Gl::draw_grid(vol);
+    canvas->draw_grid(vol);
     PopColor();
     PopCoords();
   }
diff --git a/libstage/model_fiducial.cc b/libstage/model_fiducial.cc
index f5d83eaae..59bae8bd5 100644
--- a/libstage/model_fiducial.cc
+++ b/libstage/model_fiducial.cc
@@ -18,11 +18,12 @@
 #include "option.hh"
 #include "stage.hh"
 #include "worldfile.hh"
+#include "canvas.hh"
 using namespace Stg;
 
 // TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelFiducial::showData("Fiducials", "show_fiducial", "", true, NULL);
-Option ModelFiducial::showFov("Fiducial FOV", "show_fiducial_fov", "", false, NULL);
+Option ModelFiducial::showData("Fiducials", "show_fiducial", "", true);
+Option ModelFiducial::showFov("Fiducial FOV", "show_fiducial_fov", "", false);
 
 /**
   @ingroup model
@@ -306,7 +307,7 @@ void ModelFiducial::Load(void)
   ignore_zloc = wf->ReadInt(wf_entity, "ignore_zloc", ignore_zloc);
 }
 
-void ModelFiducial::DataVisualize(Camera *cam)
+void ModelFiducial::DataVisualize(Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -358,7 +359,7 @@ void ModelFiducial::DataVisualize(Camera *cam)
       // show the fiducial ID
       char idstr[32];
       snprintf(idstr, 31, "%d", fid.id);
-      Gl::draw_string(0, 0, 0, idstr);
+      canvas->draw_string(0, 0, 0, idstr);
 
       glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
       glPopMatrix();
diff --git a/libstage/model_gripper.cc b/libstage/model_gripper.cc
index 7c3389233..2ab660665 100644
--- a/libstage/model_gripper.cc
+++ b/libstage/model_gripper.cc
@@ -56,7 +56,7 @@
 using namespace Stg;
 
 #include "option.hh"
-Option ModelGripper::showData("Gripper data", "show_gripper_data", "", true, NULL);
+Option ModelGripper::showData("Gripper data", "show_gripper_data", "", true);
 
 // TODO - simulate energy use when moving grippers
 
diff --git a/libstage/model_position.cc b/libstage/model_position.cc
index fb05a2f18..b08f6f0ac 100644
--- a/libstage/model_position.cc
+++ b/libstage/model_position.cc
@@ -14,6 +14,7 @@
 
 #include "stage.hh"
 #include "worldfile.hh"
+#include "canvas.hh"
 using namespace Stg;
 
 /**
@@ -681,7 +682,7 @@ ModelPosition::PoseVis::PoseVis() : Visualizer("Position coordinates", "show_pos
 {
 }
 
-void ModelPosition::PoseVis::Visualize(Model *mod, Camera *cam)
+void ModelPosition::PoseVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -711,10 +712,10 @@ void ModelPosition::PoseVis::Visualize(Model *mod, Camera *cam)
 
   char label[64];
   snprintf(label, 64, "x:%.3f", pos->est_pose.x);
-  Gl::draw_string(pos->est_pose.x / 2.0, -0.5, 0, label);
+  canvas->draw_string(pos->est_pose.x / 2.0, -0.5, 0, label);
 
   snprintf(label, 64, "y:%.3f", pos->est_pose.y);
-  Gl::draw_string(pos->est_pose.x + 0.5, pos->est_pose.y / 2.0, 0, (const char *)label);
+  canvas->draw_string(pos->est_pose.x + 0.5, pos->est_pose.y / 2.0, 0, (const char *)label);
 
   pos->PopColor();
 
@@ -744,7 +745,7 @@ ModelPosition::WaypointVis::WaypointVis()
 {
 }
 
-void ModelPosition::WaypointVis::Visualize(Model *mod, Camera *cam)
+void ModelPosition::WaypointVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc
index 922c23af5..1302c035b 100644
--- a/libstage/model_ranger.cc
+++ b/libstage/model_ranger.cc
@@ -80,6 +80,7 @@
 #include "option.hh"
 #include "stage.hh"
 #include "worldfile.hh"
+#include "canvas.hh"
 
 using namespace Stg;
 
@@ -91,11 +92,10 @@ static const Color RANGER_CONFIG_COLOR(0, 0, 0.5);
 // static const Color RANGER_GEOM_COLOR( 1,0,1 );
 
 // static members
-Option ModelRanger::Vis::showTransducers("Ranger transducers", "show_ranger_transducers", "", false,
-                                         NULL);
-Option ModelRanger::Vis::showArea("Ranger area", "show_ranger_ranges", "", true, NULL);
-Option ModelRanger::Vis::showStrikes("Ranger strikes", "show_ranger_strikes", "", false, NULL);
-Option ModelRanger::Vis::showFov("Ranger FOV", "show_ranger_fov", "", false, NULL);
+Option ModelRanger::Vis::showTransducers("Ranger transducers", "show_ranger_transducers", "", false);
+Option ModelRanger::Vis::showArea("Ranger area", "show_ranger_ranges", "", true);
+Option ModelRanger::Vis::showStrikes("Ranger strikes", "show_ranger_strikes", "", false);
+Option ModelRanger::Vis::showFov("Ranger FOV", "show_ranger_fov", "", false);
 // Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "",
 // false, NULL );
 
@@ -416,7 +416,7 @@ ModelRanger::Vis::Vis(World *world) : Visualizer("Ranger", "ranger_vis")
   world->RegisterOption(&showTransducers);
 }
 
-void ModelRanger::Vis::Visualize(Model *mod, Camera *cam)
+void ModelRanger::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -443,7 +443,7 @@ void ModelRanger::Vis::Visualize(Model *mod, Camera *cam)
 
       char buf[8];
       snprintf(buf, 8, "%d", (int)s);
-      Gl::draw_string(rngr.pose.x, rngr.pose.y, rngr.pose.z, buf);
+      canvas->draw_string(rngr.pose.x, rngr.pose.y, rngr.pose.z, buf);
     }
     ranger->PopColor();
   }
diff --git a/libstage/option.cc b/libstage/option.cc
index e581a46df..05193f971 100644
--- a/libstage/option.cc
+++ b/libstage/option.cc
@@ -5,18 +5,22 @@
 #include "worldfile.hh"
 using namespace Stg;
 
-Option::Option(const std::string &n, const std::string &tok, const std::string &key, bool v,
-               World *world)
-    : optName(n), value(v), wf_token(tok), shortcut(key), menu(NULL), menuIndex(0), menuCb(NULL),
-			callbackData(NULL), _world(world), htname(n)
+void MenuManager::callOptionCallback(Option * opt)
+{
+	if (opt->menuCb)
+			opt->menuCb(opt, opt->callbackData);
+}
+
+Option::Option(const std::string &n, const std::string &tok, const std::string &key, bool v)
+    : optName(n), value(v), wf_token(tok), shortcut(key), menuCb(NULL),
+			callbackData(NULL), manager(NULL), menu_widget(NULL), htname(n)
 {
   /* do nothing */
 }
 
-Fl_Menu_Item *getMenuItem(Fl_Menu_ *menu, int i)
+Option::~Option()
 {
-  const Fl_Menu_Item *mArr = menu->menu();
-  return const_cast<Fl_Menu_Item *>(&mArr[i]);
+
 }
 
 void Option::Load(Worldfile *wf, int section)
@@ -40,19 +44,26 @@ void Option::set(bool val)
 {
   value = val;
 
-  if (menu) {
-    Fl_Menu_Item *item = getMenuItem(menu, menuIndex);
-    value ? item->set() : item->clear();
+  if(manager != NULL)
+  {
+  	manager->syncMenuState(this);
   }
+}
 
-  if (_world) {
-#ifdef TEST_LATER
-    WorldGui *wg = dynamic_cast<WorldGui *>(_world);
-    if (wg == NULL)
-      return;
-    Canvas *canvas = wg->GetCanvas();
-    canvas->setInvalidate();
-    canvas->redraw();
-#endif
-  }
+void Option::onAttachGUI(MenuManager * manager, void * widget)
+{
+	assert(this->manager == NULL);
+	this->manager = manager;
+	this->menu_widget = widget;
+}
+
+void * Option::getWidgetData()
+{
+	return menu_widget;
+}
+
+void Option::callValueChanged()
+{
+	if (menuCb != NULL)
+		menuCb(this, callbackData);
 }
diff --git a/libstage/option.hh b/libstage/option.hh
index 4a7864a99..038ba9c05 100644
--- a/libstage/option.hh
+++ b/libstage/option.hh
@@ -8,6 +8,19 @@
 #include <FL/Fl_Menu_Item.H>
 
 namespace Stg {
+
+class Option;
+
+class MenuManager
+{
+public:
+	virtual ~MenuManager() {};
+	/** Updates menu state according to current option state */
+	virtual void syncMenuState(Option * opt) {};
+protected:
+	void callOptionCallback(Option * opt);
+};
+
 class World;
 /** option.hh
    Class that encapsulates a boolean and pairs it with a string description
@@ -17,6 +30,7 @@ class World;
    SVN: $Id$
    */
 class Option {
+	friend class MenuManager;
 private:
   friend bool compare(const Option *lhs, const Option *rhs);
 
@@ -25,19 +39,23 @@ private:
   /** worldfile entry string for loading and saving this value */
   std::string wf_token;
   std::string shortcut;
-  Fl_Menu_ *menu;
-  int menuIndex;
+
   /** Callback type for menu events*/
   typedef void (*Callback)(Option *opt, void * data);
   Callback menuCb;
   void *callbackData;
-  World *_world;
+
+  MenuManager * manager;
+  // Pointer to menu widget implementation
+  //Fl_Menu_ *menu;
+  void *menu_widget;
 
 public:
-  Option(const std::string &n, const std::string &tok, const std::string &key, bool v,
-         World *world);
+  Option(const std::string &n, const std::string &tok, const std::string &key, bool v);
+  ~Option();
 
   const std::string name() const { return optName; }
+  const std::string key() const { return shortcut; }
   inline bool isEnabled() const { return value; }
   inline bool val() const { return value; }
   inline operator bool() { return val(); }
@@ -56,6 +74,11 @@ public:
   // 		};
 
   void setOptionCallback(Callback cb, void *w);
+  void callValueChanged();
+  // Called when option is attached to some menu, to save this data
+  void onAttachGUI(MenuManager * manager, void * widget_data);
+  void * getWidgetData();
+
   void Load(Worldfile *wf, int section);
   void Save(Worldfile *wf, int section);
 
diff --git a/libstage/powerpack.cc b/libstage/powerpack.cc
index 68ae15b06..1d8421130 100644
--- a/libstage/powerpack.cc
+++ b/libstage/powerpack.cc
@@ -6,6 +6,7 @@
 */
 
 #include "stage.hh"
+#include "canvas.hh"
 #include "texture_manager.hh"
 using namespace Stg;
 
@@ -44,7 +45,7 @@ PowerPack::~PowerPack()
 }
 
 /** OpenGL visualization of the powerpack state */
-void PowerPack::Visualize(Camera *cam)
+void PowerPack::Visualize(Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -141,7 +142,7 @@ void PowerPack::Visualize(Camera *cam)
     glColor4f(1, 0, 0, 0.8); // red
     char buf[32];
     snprintf(buf, 32, "%.1fW", watts);
-    Gl::draw_string(-0.05, height + 0.05, 0, buf);
+    canvas->draw_string(-0.05, height + 0.05, 0, buf);
   }
 }
 
@@ -265,7 +266,7 @@ PowerPack::DissipationVis::~DissipationVis()
 {
 }
 
-void PowerPack::DissipationVis::Visualize(Model *mod, Camera *cam)
+void PowerPack::DissipationVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
   (void)cam; // avoid warning about unused var
 
diff --git a/libstage/stage.hh b/libstage/stage.hh
index 2de90605f..7135ae754 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -488,25 +488,22 @@ point_t *unit_square_points_create();
 namespace Gl {
 
 /** Calculate scaled width of the string for current font*/
-float gl_width(const char * string);
+//float gl_width(const char * string);
 /** Calculate font height */
-float gl_height();
+//float gl_height();
 
 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);
 /** Render a string at [x,y,z] in the current color */
-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, int align);
-void draw_speech_bubble(float x, float y, float z, const char *str);
+/// Font rendering is moved to canvas
+//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, int 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);
 /** Draws a rectangle with center at x,y, with sides of length dx,dy */
 void draw_centered_rect(float x, float y, float dx, float dy);
 } // namespace Gl
@@ -527,7 +524,7 @@ public:
   }
 
   virtual ~Visualizer(void) {}
-  virtual void Visualize(Model *mod, Camera *cam) = 0;
+  virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas) = 0;
 
   const std::string &GetMenuName() { return menu_name; }
   const std::string &GetWorldfileName() { return worldfile_name; }
@@ -890,6 +887,9 @@ AddUpdateCallback is not automatically freed. */
 
   /** hint that the world needs to be redrawn if a GUI is attached */
   void NeedRedraw() { dirty = true; }
+
+  bool IsDirty() const;
+  void SetDirty(bool val);
   /** Special model for the floor of the world */
   Model *ground;
 
@@ -1292,7 +1292,7 @@ public:
   virtual void reset() = 0;
   virtual void Load(Worldfile *wf, int sec) = 0;
 
-  Canvas * GetCanvas();
+  //Canvas * GetCanvas();
 
   // 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 )
@@ -1435,7 +1435,7 @@ 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);
+  virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   void AppendValue(float value);
 };
 
@@ -1460,7 +1460,7 @@ protected:
     DissipationVis(meters_t width, meters_t height, meters_t cellsize);
 
     virtual ~DissipationVis();
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
 
     void Accumulate(meters_t x, meters_t y, joules_t amount);
   } event_vis;
@@ -1499,7 +1499,7 @@ public:
   ~PowerPack();
 
   /** OpenGL visualization of the powerpack state */
-  void Visualize(Camera *cam);
+  void Visualize(Camera *cam, Canvas * canvas);
 
   /** Returns the energy capacity minus the current amount stored */
   joules_t RemainingCapacity() const;
@@ -1718,7 +1718,7 @@ initially NULL. */
   public:
     RasterVis();
     virtual ~RasterVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
 
     void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                  meters_t cellheight);
@@ -1903,7 +1903,7 @@ by pose, in local coords */
                 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);
+    world->Raytrace(LocalToGlobal(pose), range, fov, func, this, arg, ztest, results);
   }
 
   virtual void UpdateCharge();
@@ -1923,8 +1923,8 @@ by pose, in local coords */
   virtual void DrawBlocks();
   void DrawBoundingBox();
   void DrawBoundingBoxTree();
-  virtual void DrawStatus(Camera *cam);
-  void DrawStatusTree(Camera *cam);
+  virtual void DrawStatus(Camera *cam, Canvas * canvas);
+  void DrawStatusTree(Camera *cam, Canvas * canvas);
 
   void DrawOriginTree();
   void DrawOrigin();
@@ -1938,14 +1938,14 @@ by pose, in local coords */
 
   virtual void DrawPicker();
   virtual void DataVisualize(Camera *cam);
-  virtual void DrawSelected(void);
+  virtual void DrawSelected(Canvas * canvas);
 
   void DrawTrailFootprint();
   void DrawTrailBlocks();
   void DrawTrailArrows();
-  void DrawGrid();
+  void DrawGrid(Canvas * canvas);
   //	void DrawBlinkenlights();
-  void DataVisualizeTree(Camera *cam);
+  void DataVisualizeTree(Camera *cam, Canvas * canvas);
   void DrawFlagList();
   void DrawPose(Pose pose);
 
@@ -2239,7 +2239,7 @@ public:
   public:
     explicit Vis(World *world);
     virtual ~Vis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   } vis;
 
 private:
@@ -2436,7 +2436,7 @@ protected:
   public:
     BumperVis();
     virtual ~BumperVis();
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   } bumpervis;
 
 private:
@@ -2467,7 +2467,7 @@ private:
   void AddModelIfVisible(Model *him);
 
   virtual void Update();
-  virtual void DataVisualize(Camera *cam);
+  virtual void DataVisualize(Camera *cam, Canvas * canvas);
 
   static Option showData;
   static Option showFov;
@@ -2522,7 +2522,7 @@ public:
 
     explicit Vis(World *world);
     virtual ~Vis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   } vis;
 
   class Sensor {
@@ -2736,14 +2736,14 @@ Models and visualized. */
   public:
     WaypointVis();
     virtual ~WaypointVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   } wpvis;
 
   class PoseVis : public Visualizer {
   public:
     PoseVis();
     virtual ~PoseVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
   } posevis;
 
   /** Set the current pose estimate.*/
diff --git a/libstage/texture_manager.cc b/libstage/texture_manager.cc
index 708fd725c..fd645d3fa 100644
--- a/libstage/texture_manager.cc
+++ b/libstage/texture_manager.cc
@@ -19,31 +19,35 @@ GLuint TextureManager::loadTexture(const char *filename)
   //      return 0;
 
   GLuint texName;
-  Fl_Shared_Image *img = Fl_Shared_Image::get(filename);
+  Image img;
+  //Fl_Shared_Image *img = Fl_Shared_Image::get(filename);
 
-  if (img == NULL) {
+  if (!img.load(filename)) {
     fprintf(stderr, "unable to open image: %s\n", filename);
     // exit(-1);
     return 0;
   }
 
   // TODO display an error for incorrect depths
-  if (img->d() != 3 && img->d() != 4) {
+  int depth = img.getDepth();
+  if (depth != 3 && depth != 4) {
     fprintf(stderr, "unable to open image: %s - incorrect depth - should be 3 or 4\n", filename);
     return 0;
   }
 
   // TODO check for correct width/height - or convert it.
 
-  uint8_t *pixels = (uint8_t *)(img->data()[0]);
+  uint8_t *pixels = (uint8_t *)img.getData();
 
   // vertically flip the image
-  int img_size = img->w() * img->h() * img->d();
+  int w = img.getWidth();
+  int h = img.getHeight();
+  int img_size = w * h * depth;
   uint8_t *img_flip = new uint8_t[img_size];
 
-  const int row_width = img->w() * img->d();
-  for (int i = 0; i < img->h(); i++)
-    memcpy(img_flip + (i * row_width), pixels + ((img->h() - i - 1) * row_width), row_width);
+  const int row_width = w * depth;
+  for (int i = 0; i < h; i++)
+    memcpy(img_flip + (i * row_width), pixels + ((h - i - 1) * row_width), row_width);
 
   // create room for texture
   glGenTextures(1, &texName);
@@ -61,9 +65,7 @@ GLuint TextureManager::loadTexture(const char *filename)
   // glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); //or use
   // GL_DECAL or GL_MODULATE instead of DECAL to mix colours and textures
 
-  gluBuild2DMipmaps(GL_TEXTURE_2D, img->d(), img->w(), img->h(), (img->d() == 3 ? GL_RGB : GL_RGBA),
-                    GL_UNSIGNED_BYTE, img_flip);
-
+  gluBuild2DMipmaps(GL_TEXTURE_2D, depth, w, h, (depth == 3 ? GL_RGB : GL_RGBA), GL_UNSIGNED_BYTE, img_flip);
   glBindTexture(GL_TEXTURE_2D, 0);
   return texName;
 }
diff --git a/libstage/vis_strip.cc b/libstage/vis_strip.cc
index 77821cf04..a03c0cc20 100644
--- a/libstage/vis_strip.cc
+++ b/libstage/vis_strip.cc
@@ -21,11 +21,8 @@ StripPlotVis::~StripPlotVis()
     free(data);
 }
 
-void StripPlotVis::Visualize(Model *mod, Camera *camera)
+void StripPlotVis::Visualize(Model *mod, Camera *camera, Canvas * canvas)
 {
-  //Canvas *canvas = dynamic_cast<WorldGui *>(mod->GetWorld())->GetCanvas();
-	Canvas *canvas = camera->GetCanvas();
-
   if (!canvas->selected(mod)) // == canvas->SelectedVisualizeAll() )
     return;
 
@@ -36,7 +33,7 @@ void StripPlotVis::Visualize(Model *mod, Camera *camera)
   mod->PopColor();
 
   mod->PushColor(fgcolor);
-  Gl::draw_array(x, y, w, h, data, len, count % len, min, max);
+  canvas->draw_array(x, y, w, h, data, len, count % len, min, max);
   mod->PopColor();
 
   canvas->LeaveScreenCS();
diff --git a/libstage/world.cc b/libstage/world.cc
index 67d24ef5e..4dffbcb18 100644
--- a/libstage/world.cc
+++ b/libstage/world.cc
@@ -1156,6 +1156,16 @@ void World::Log(Model *)
   // LogEntry::Print();
 }
 
+bool World::IsDirty() const
+{
+	return dirty;
+}
+
+void World::SetDirty(bool val)
+{
+	dirty = val;
+}
+
 bool World::Event::operator<(const Event &other) const
 {
   return (time > other.time);
diff --git a/libstage/world_gui.hh b/libstage/world_gui.hh
index af5b7c630..7c484434b 100644
--- a/libstage/world_gui.hh
+++ b/libstage/world_gui.hh
@@ -22,6 +22,8 @@ namespace Stg {
 /** Extends World to implement an FLTK / OpenGL graphical user
       interface.
   */
+class CanvasFLTK;
+
 class WorldGui : public World, public Fl_Window {
   friend class Canvas;
   friend class ModelCamera;
@@ -114,7 +116,7 @@ public:
 
   usec_t RealTimeNow(void) const;
 
-  Canvas *GetCanvas(void) const { return canvas; }
+  Canvas *GetCanvas(void) const;
   /** show the window - need to call this if you don't Load(). */
   void Show();
 
diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc
index b66521dc4..b9e5a9844 100644
--- a/libstage/worldgui.cc
+++ b/libstage/worldgui.cc
@@ -152,7 +152,7 @@ then select the option from the menu again to stop.
 #include <set>
 #include <sstream>
 
-#include "canvas.hh"
+#include "canvas_fltk.hh"
 #include "config.h" // build options from CMake
 #include "file_manager.hh"
 #include "options_dlg.hh"
@@ -585,8 +585,7 @@ void WorldGui::Start()
   World::Start();
 
   // start the timer that causes regular redraws
-  Fl::add_timeout(((double)canvas->interval / 1000), (Fl_Timeout_Handler)Canvas::TimerCallback,
-                  canvas);
+  Fl::add_timeout(((double)canvas->interval / 1000), (Fl_Timeout_Handler)CanvasFLTK::TimerCallback, canvas);
 
   SetTimeouts();
 }
@@ -609,7 +608,7 @@ void WorldGui::Stop()
 {
   World::Stop();
 
-  Fl::remove_timeout((Fl_Timeout_Handler)Canvas::TimerCallback);
+  Fl::remove_timeout((Fl_Timeout_Handler)CanvasFLTK::TimerCallback);
   Fl::remove_timeout((Fl_Timeout_Handler)UpdateCallback);
   Fl::remove_idle((Fl_Timeout_Handler)UpdateCallback, this);
 
@@ -858,3 +857,8 @@ bool WorldGui::IsTopView()
 {
   return canvas->IsTopView();
 }
+
+Canvas *WorldGui::GetCanvas(void) const
+{
+	return canvas;
+}

From b71c18c13210f873e325265c6c44f6be8009c52d Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 00:19:03 +0300
Subject: [PATCH 03/22] Fixing runtime errors

---
 libstage/canvas_fltk.cc |  1 -
 libstage/model_draw.cc  | 15 +++++++++------
 libstage/stage.hh       |  2 +-
 libstage/world_gui.hh   |  2 +-
 libstage/worldgui.cc    |  2 +-
 5 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index 252e00790..22540fd81 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -44,7 +44,6 @@ void CanvasFLTK::createMenuItems(Fl_Menu_Bar *menu, std::string path)
 	mm->createMenuItem(showBlocks, path);
 	mm->createMenuItem(showFlags, path);
 	mm->createMenuItem(showClock, path);
-	mm->createMenuItem(showFlags, path);
 	mm->createMenuItem(showFollow, path);
 	mm->createMenuItem(showFootprints, path);
 	mm->createMenuItem(showGrid, path);
diff --git a/libstage/model_draw.cc b/libstage/model_draw.cc
index 8e6d8667d..1c4b68822 100644
--- a/libstage/model_draw.cc
+++ b/libstage/model_draw.cc
@@ -253,12 +253,15 @@ void Model::AddVisualizer(Visualizer *cv, bool on_by_default)
   // register option for all instances which share the same name
   Canvas *canvas = this->GetCanvas();
 
-  std::map<std::string, Option *>::iterator i = canvas->_custom_options.find(cv->GetMenuName());
-  if (i == canvas->_custom_options.end()) {
-    Option *op =
-        new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default);
-    canvas->_custom_options[cv->GetMenuName()] = op;
-    RegisterOption(op);
+  if(canvas != NULL)
+  {
+		std::map<std::string, Option *>::iterator i = canvas->_custom_options.find(cv->GetMenuName());
+		if (i == canvas->_custom_options.end()) {
+			Option *op =
+					new Option(cv->GetMenuName(), cv->GetWorldfileName(), "", on_by_default);
+			canvas->_custom_options[cv->GetMenuName()] = op;
+			RegisterOption(op);
+		}
   }
 }
 
diff --git a/libstage/stage.hh b/libstage/stage.hh
index 7135ae754..b756c2555 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -1126,7 +1126,7 @@ nonexistent */
   /** Return the floor model */
   Model *GetGround() { return ground; }
 
-  Canvas * GetCanvas() {return NULL; }
+  virtual Canvas * GetCanvas() {return NULL; }
 };
 
 class Block {
diff --git a/libstage/world_gui.hh b/libstage/world_gui.hh
index 7c484434b..25220ee4d 100644
--- a/libstage/world_gui.hh
+++ b/libstage/world_gui.hh
@@ -116,7 +116,7 @@ public:
 
   usec_t RealTimeNow(void) const;
 
-  Canvas *GetCanvas(void) const;
+  Canvas *GetCanvas(void);
   /** show the window - need to call this if you don't Load(). */
   void Show();
 
diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc
index b9e5a9844..cb043c9fe 100644
--- a/libstage/worldgui.cc
+++ b/libstage/worldgui.cc
@@ -858,7 +858,7 @@ bool WorldGui::IsTopView()
   return canvas->IsTopView();
 }
 
-Canvas *WorldGui::GetCanvas(void) const
+Canvas *WorldGui::GetCanvas(void)
 {
 	return canvas;
 }

From 876a79ac0305dc55a2fde793f534b67ed6783fc0 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 00:50:08 +0300
Subject: [PATCH 04/22] Fixing runtime errors

---
 libstage/canvas.cc      |  53 +-------------
 libstage/canvas.hh      |   3 +-
 libstage/canvas_fltk.cc | 153 ++++++++++++++++++++++++++--------------
 libstage/canvas_fltk.hh |   8 ++-
 libstage/image.cc       |  24 ++++++-
 5 files changed, 130 insertions(+), 111 deletions(-)

diff --git a/libstage/canvas.cc b/libstage/canvas.cc
index 9b583ed73..c560d0eb6 100644
--- a/libstage/canvas.cc
+++ b/libstage/canvas.cc
@@ -32,8 +32,6 @@ static const int checkImageHeight = 2;
 static GLubyte checkImage[checkImageHeight][checkImageWidth][4];
 static bool blur = true;
 
-static bool texture_load_done = false;
-
 // GLuint glowTex;
 GLuint checkTex;
 
@@ -41,7 +39,7 @@ Canvas::Canvas(World *world, int x, int y, int width, int height)
     : colorstack(), models_sorted(), current_camera(NULL),
       camera(), perspective_camera(), dirty_buffer(false), wf(NULL),
       selected_models(), last_selection(NULL), interval(40), // msec between redraws
-      init_done(false),
+      init_done(false), texture_load_done(false),
       // initialize Option objects
       //  showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ),
       showBBoxes("Debug/Bounding boxes", "show_boundingboxes", "^b", false),
@@ -96,16 +94,9 @@ void Canvas::InitGl()
   glEnableClientState(GL_VERTEX_ARRAY);
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
-  // install a font
-#ifdef FIX_LATER
-  gl_font(FL_HELVETICA, 12);
-#endif
-
   blur = false;
 
   glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
-
-  init_done = true;
 }
 
 void Canvas::InitTextures()
@@ -173,8 +164,6 @@ void Canvas::InitTextures()
 
   glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight, 0, GL_RGBA,
                GL_UNSIGNED_BYTE, checkImage);
-
-  texture_load_done = true;
 }
 
 Canvas::~Canvas()
@@ -866,46 +855,6 @@ void Canvas::Save(Worldfile *wf, int sec)
   pCamOn.Save(wf, sec);
 }
 
-void Canvas::draw()
-{
-  // Enable the following to debug camera model
-  //	if( loaded_texture == true && pCamOn == true )
-  //		return;
-
-  if (!isValid()) {
-    if (!init_done)
-      InitGl();
-    if (!texture_load_done)
-      InitTextures();
-
-    if (pCamOn == true) {
-      perspective_camera.setAspect(static_cast<float>(getWidth()) / static_cast<float>(getHeight()));
-      perspective_camera.SetProjection();
-      current_camera = &perspective_camera;
-    } else {
-      bounds3d_t extent = world->GetExtent();
-      camera.SetProjection(getWidth(), getHeight(), extent.y.min, extent.y.max);
-      current_camera = &camera;
-    }
-
-    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-  }
-
-  // Follow the selected robot
-  if (showFollow && last_selection) {
-    Pose gpose = last_selection->GetGlobalPose();
-    if (pCamOn == true) {
-      perspective_camera.setPose(gpose.x, gpose.y, 0.2);
-      perspective_camera.setYaw(rtod(gpose.a) - 90.0);
-    } else {
-      camera.setPose(gpose.x, gpose.y);
-    }
-  }
-
-  current_camera->Draw();
-  renderFrame();
-}
-
 void Canvas::draw_array(float x, float y, float w, float h, float *data, size_t len, size_t offset,
                          float min, float max)
 {
diff --git a/libstage/canvas.hh b/libstage/canvas.hh
index 3e21b0705..74f905ab9 100644
--- a/libstage/canvas.hh
+++ b/libstage/canvas.hh
@@ -56,6 +56,7 @@ protected:
 
   msec_t interval; // window refresh interval in ms
   bool init_done;
+  bool texture_load_done;
 
 
   void RecordRay(double x1, double y1, double x2, double y2);
@@ -91,8 +92,6 @@ public:
   void DrawBoundingBoxes();
   void resetCamera();
   virtual void renderFrame();
-  virtual void draw();
-
   /** Switch between camera modes
    * @param mode - camera mode:
    *  - true: perspective camera
diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index 22540fd81..6c0b55fc6 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -6,59 +6,6 @@
 namespace Stg
 {
 
-//Fl_Gl_Window(x, y, width, height),
-
-CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
-:Canvas(world, x, y, width, height)
-,Fl_Gl_Window(x, y, width, height), selectedModel(false), startx(-1), starty(-1),
- empty_space_startx(0), empty_space_starty(0),
- showScreenshots("Save screenshots", "screenshots", "", false),
- menu_manager(NULL),
- graphics(true), screenshot_frame_skip(1)
-{
-  end();
-  // show(); // must do this so that the GL context is created before
-  // configuring GL
-  // but that line causes a segfault in Linux/X11! TODO: test in OS X
-  mode( FL_RGB |FL_DOUBLE|FL_DEPTH| FL_MULTISAMPLE | FL_ALPHA );
-}
-
-CanvasFLTK::~CanvasFLTK()
-{
-
-}
-
-void CanvasFLTK::perspectiveCb(Option *opt, void *p)
-{
-  Canvas *canvas = static_cast<Canvas *>(p);
-  canvas->switchCameraMode(opt->val());
-}
-
-void CanvasFLTK::createMenuItems(Fl_Menu_Bar *menu, std::string path)
-{
-	assert(this->menu_manager == NULL);
-	MenuManagerFLTK * mm = new MenuManagerFLTK(this, menu);
-	menu_manager = mm;
-	mm->createMenuItem(showData, path);
-  // mm->createMenuItem(visualizeAll, path );
-	mm->createMenuItem(showBlocks, path);
-	mm->createMenuItem(showFlags, path);
-	mm->createMenuItem(showClock, path);
-	mm->createMenuItem(showFollow, path);
-	mm->createMenuItem(showFootprints, path);
-	mm->createMenuItem(showGrid, path);
-	mm->createMenuItem(showStatus, path);
-	mm->createMenuItem(pCamOn, path);
-	pCamOn.setOptionCallback(perspectiveCb, this);
-	mm->createMenuItem(showOccupancy, path);
-	mm->createMenuItem(showTrailArrows, path);
-	mm->createMenuItem(showTrails, path);
-	mm->createMenuItem(showTrailRise, path); // broken
-	mm->createMenuItem(showBBoxes, path);
-	// mm->createMenuItem(showVoxels, path );
-	mm->createMenuItem(showScreenshots, path);
-}
-
 Fl_Menu_Item *getMenuItem(Fl_Menu_ *menu, int i)
 {
   const Fl_Menu_Item *mArr = menu->menu();
@@ -112,6 +59,57 @@ void MenuManagerFLTK::toggleCb(Fl_Widget *, void *p)
   opt->callValueChanged();
 }
 
+CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
+:Canvas(world, x, y, width, height)
+,Fl_Gl_Window(x, y, width, height), selectedModel(false), startx(-1), starty(-1),
+ empty_space_startx(0), empty_space_starty(0),
+ showScreenshots("Save screenshots", "screenshots", "", false),
+ menu_manager(NULL),
+ graphics(true), screenshot_frame_skip(1)
+{
+  end();
+  // show(); // must do this so that the GL context is created before
+  // configuring GL
+  // but that line causes a segfault in Linux/X11! TODO: test in OS X
+  mode( FL_RGB |FL_DOUBLE|FL_DEPTH| FL_MULTISAMPLE | FL_ALPHA );
+}
+
+CanvasFLTK::~CanvasFLTK()
+{
+
+}
+
+void CanvasFLTK::perspectiveCb(Option *opt, void *p)
+{
+  Canvas *canvas = static_cast<Canvas *>(p);
+  canvas->switchCameraMode(opt->val());
+}
+
+void CanvasFLTK::createMenuItems(Fl_Menu_Bar *menu, std::string path)
+{
+	assert(this->menu_manager == NULL);
+	MenuManagerFLTK * mm = new MenuManagerFLTK(this, menu);
+	menu_manager = mm;
+	mm->createMenuItem(showData, path);
+  // mm->createMenuItem(visualizeAll, path );
+	mm->createMenuItem(showBlocks, path);
+	mm->createMenuItem(showFlags, path);
+	mm->createMenuItem(showClock, path);
+	mm->createMenuItem(showFollow, path);
+	mm->createMenuItem(showFootprints, path);
+	mm->createMenuItem(showGrid, path);
+	mm->createMenuItem(showStatus, path);
+	mm->createMenuItem(pCamOn, path);
+	pCamOn.setOptionCallback(perspectiveCb, this);
+	mm->createMenuItem(showOccupancy, path);
+	mm->createMenuItem(showTrailArrows, path);
+	mm->createMenuItem(showTrails, path);
+	mm->createMenuItem(showTrailRise, path); // broken
+	mm->createMenuItem(showBBoxes, path);
+	// mm->createMenuItem(showVoxels, path );
+	mm->createMenuItem(showScreenshots, path);
+}
+
 double CanvasFLTK::fontWidth(const char * str) const
 {
 	return gl_width(str);
@@ -489,4 +487,53 @@ void CanvasFLTK::draw_string_multiline(float x, float y, float w, float h, const
   gl_draw(str, x, y, w, h, align); // fltk function
 }
 
+void CanvasFLTK::draw()
+{
+  // Enable the following to debug camera model
+  //	if( loaded_texture == true && pCamOn == true )
+  //		return;
+
+  if (!isValid()) {
+    if (!init_done)
+    {
+      InitGl();
+      // install a font
+      gl_font(FL_HELVETICA, 12);
+      init_done = true;
+    }
+    if (!texture_load_done)
+    {
+      InitTextures();
+      texture_load_done = true;
+    }
+
+    if (pCamOn == true) {
+      perspective_camera.setAspect(static_cast<float>(getWidth()) / static_cast<float>(getHeight()));
+      perspective_camera.SetProjection();
+      current_camera = &perspective_camera;
+    } else {
+      bounds3d_t extent = world->GetExtent();
+      camera.SetProjection(getWidth(), getHeight(), extent.y.min, extent.y.max);
+      current_camera = &camera;
+    }
+
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  }
+
+  // Follow the selected robot
+  if (showFollow && last_selection) {
+    Pose gpose = last_selection->GetGlobalPose();
+    if (pCamOn == true) {
+      perspective_camera.setPose(gpose.x, gpose.y, 0.2);
+      perspective_camera.setYaw(rtod(gpose.a) - 90.0);
+    } else {
+      camera.setPose(gpose.x, gpose.y);
+    }
+  }
+
+  current_camera->Draw();
+  renderFrame();
+}
+
+
 }
diff --git a/libstage/canvas_fltk.hh b/libstage/canvas_fltk.hh
index 195cf06fc..0f1c94ed2 100644
--- a/libstage/canvas_fltk.hh
+++ b/libstage/canvas_fltk.hh
@@ -22,9 +22,9 @@ protected:
 	/** Toggle callback from FLTK */
 	static void toggleCb(Fl_Widget *w, void *p);
 	CanvasFLTK * GetCanvas();
-
-	Fl_Menu_ *menu;
+private:
 	CanvasFLTK * canvas;
+	Fl_Menu_ *menu;
 };
 
 class CanvasFLTK : public Canvas, public Fl_Gl_Window {
@@ -47,7 +47,6 @@ public:
   int screenshot_frame_skip;
 
   void Screenshot();
-  void InitTextures();
   void createMenuItems(Fl_Menu_Bar *menu, std::string path);
 
   virtual void renderFrame();
@@ -73,6 +72,9 @@ public:
 
   void Load(Worldfile *wf, int sec);
   void Save(Worldfile *wf, int section);
+
+  /** Overriding draw method from Fl_Gl_Window*/
+  virtual void draw();
 };
 } //namespace Stg
 
diff --git a/libstage/image.cc b/libstage/image.cc
index dfbcc2533..5af42396a 100644
--- a/libstage/image.cc
+++ b/libstage/image.cc
@@ -41,7 +41,6 @@ uint8_t * Image::getData()
 
 bool Image::load(const char * file_name)
 {
-	assert(false);
 	/* open file and test for it being a png */
 	FILE *fp = fopen(file_name, "rb");
 
@@ -95,7 +94,30 @@ bool Image::load(const char * file_name)
 		//number_of_passes = png_set_interlace_handling(png_ptr);
 		png_read_update_info(png_ptr, info_ptr);
 
+		if(bit_depth != 8)
+		{
+			printf("We support channels of 8 bit\n");
+			break;
+		}
+
+		switch(color_type)
+		{
+		case PNG_COLOR_TYPE_GRAY:
+			bpp = bit_depth / 8;
+			break;
+		case PNG_COLOR_TYPE_RGB:
+			bpp = 3 * bit_depth / 8;
+			break;
+		case PNG_COLOR_TYPE_RGB_ALPHA:
+			bpp = 4 * bit_depth / 8;
+			break;
+		default:
+			bpp = 0;
+			printf("Unsupported color type: %d\n", color_type);
+			break;
+		}
 
+		//bpp = bit_depth / 8;
 		/* read file */
 		if (setjmp(png_jmpbuf(png_ptr)))
 		{

From 0ccc30801c390581f7ddd803cc37528676aa200e Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 01:05:34 +0300
Subject: [PATCH 05/22] Stage can properly start and simulate

Fixed image loading
Reworkrd World::Run a bit. Now there is separate WorldGui::Run for scenes with gui support
---
 libstage/image.cc     |  2 +-
 libstage/main.cc      | 12 +++++++++++-
 libstage/stage.hh     |  2 ++
 libstage/world.cc     | 33 +++++++++++++++++++++++----------
 libstage/world_gui.hh |  2 ++
 libstage/worldgui.cc  | 21 +++++++++++++++++++++
 6 files changed, 60 insertions(+), 12 deletions(-)

diff --git a/libstage/image.cc b/libstage/image.cc
index 5af42396a..b652bb7f9 100644
--- a/libstage/image.cc
+++ b/libstage/image.cc
@@ -135,7 +135,7 @@ bool Image::load(const char * file_name)
 			png_bytep row_pointer = &row_data[0];
 			png_read_row(png_ptr, row_pointer, NULL);
 			uint8_t * dst = &this->data[y*width*bpp];
-			memcpy(row_pointer, dst, width*bpp);
+			memcpy(dst, row_pointer, width*bpp);
 		}
 
 		//png_read_image(png_ptr, row_pointers);
diff --git a/libstage/main.cc b/libstage/main.cc
index db473963d..e1f220ffd 100644
--- a/libstage/main.cc
+++ b/libstage/main.cc
@@ -111,7 +111,17 @@ int main( int argc, char* argv[] )
     optindex++;
   }
 
-  World::Run();
+  if(usegui)
+  {
+  	//while (Fl::first_window() && !World::quit_all) {
+  	    //  Fl::wait();
+  	    //}
+  	WorldGui::Run();
+  }
+  else
+  {
+  	World::Run();
+  }
 
   puts("\n[Stage: done]");
 
diff --git a/libstage/stage.hh b/libstage/stage.hh
index b756c2555..b5a71b188 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -1042,6 +1042,8 @@ public:
  *  have been created. This world is then simulated.
  */
   static void Run();
+  static bool ExistsGuiWorld();
+  static bool ExitAll();
 
   World(const std::string &name = "MyWorld", double ppm = DEFAULT_PPM);
 
diff --git a/libstage/world.cc b/libstage/world.cc
index 4dffbcb18..27cea6ad3 100644
--- a/libstage/world.cc
+++ b/libstage/world.cc
@@ -190,23 +190,17 @@ void World::DestroySuperRegion(SuperRegion *sr)
 void World::Run()
 {
   // first check whether there is a single gui world
-  bool found_gui = false;
-  FOR_EACH (world_it, world_set) {
-    found_gui |= (*world_it)->IsGUI();
-  }
-  if (found_gui && (world_set.size() != 1)) {
-    PRINT_WARN("When using the GUI only a single world can be simulated.");
-    exit(-1);
-  }
+  bool found_gui = World::ExistsGuiWorld();
 
-  if (found_gui) {
+  if (found_gui)
+  {
     // roughly equals Fl::run() (see also
     // https://wiki.orfeo-toolbox.org/index.php/How_to_exit_every_fltk_window_in_the_world,
     // FLTK
     // is a piece of crap):
   	assert(false);
   	// Dirty replacement here
-    //while (Fl::first_window() && !World::quit_all) {
+    //while (Fl::first_window() && !World::ExitAll()) {
     //  Fl::wait();
     //}
   } else {
@@ -215,6 +209,25 @@ void World::Run()
   }
 }
 
+bool World::ExistsGuiWorld()
+{
+	bool found_gui = false;
+
+	FOR_EACH (world_it, world_set) {
+		found_gui |= (*world_it)->IsGUI();
+	}
+	if (found_gui && (world_set.size() != 1)) {
+		PRINT_WARN("When using the GUI only a single world can be simulated.");
+		//exit(-1);
+	}
+	return found_gui;
+}
+
+bool World::ExitAll()
+{
+	return quit_all;
+}
+
 bool World::UpdateAll()
 {
   bool quit(true);
diff --git a/libstage/world_gui.hh b/libstage/world_gui.hh
index 25220ee4d..a96ab78fa 100644
--- a/libstage/world_gui.hh
+++ b/libstage/world_gui.hh
@@ -125,6 +125,8 @@ public:
   virtual void RemoveChild(Model *mod);
 
   bool IsTopView();
+
+  static void Run();
 };
 
 } //namespace Stg
diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc
index cb043c9fe..e9b23d7d5 100644
--- a/libstage/worldgui.cc
+++ b/libstage/worldgui.cc
@@ -862,3 +862,24 @@ Canvas *WorldGui::GetCanvas(void)
 {
 	return canvas;
 }
+
+void WorldGui::Run()
+{
+  // first check whether there is a single gui world
+  bool found_gui = World::ExistsGuiWorld();
+
+  if (found_gui) {
+    // roughly equals Fl::run() (see also
+    // https://wiki.orfeo-toolbox.org/index.php/How_to_exit_every_fltk_window_in_the_world,
+    // FLTK
+    // is a piece of crap):
+  	// Dirty replacement here
+    while (Fl::first_window() && !World::ExitAll()) {
+      Fl::wait();
+    }
+  } else {
+    while (!UpdateAll())
+      ;
+  }
+}
+

From edd31364fd6235ece9402bb4f930932313b87124 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 12:46:48 +0300
Subject: [PATCH 06/22] Fixing cmake part

Old libstage with FLTK-GUI remains the same. Now there is libstage_core, that works without FLTK
---
 CMakeLists.txt          |  24 +++++----
 libstage/CMakeLists.txt | 117 +++++++++++++++++++---------------------
 2 files changed, 70 insertions(+), 71 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 21fe8df80..d4f0b79f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,9 +14,7 @@ SET( MIN_PLAYER 2.1.0 )
 OPTION (BUILD_PLAYER_PLUGIN "Build Player plugin" ON)
 OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF)
 OPTION (CPACK_CFG "[release building] generate CPack configuration files" ON)
-
-# todo - this doesn't work yet. Run Stage headless with -g.
-# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) 
+OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON )
 
 IF (CMAKE_MAJOR_VERSION EQUAL 2 AND NOT CMAKE_MINOR_VERSION LESS 6)
 	cmake_policy( SET CMP0003 NEW )
@@ -62,6 +60,7 @@ IF (NOT CMAKE_BUILD_TYPE)
   SET (CMAKE_BUILD_TYPE "release" CACHE STRING 
     "Choose the type of build, options are: release (default) debug profile" FORCE)
 ENDIF (NOT CMAKE_BUILD_TYPE)
+
 STRING(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE)
 
 MESSAGE( STATUS "Build type ${CMAKE_BUILD_TYPE}" )
@@ -88,7 +87,6 @@ include_directories(
 MESSAGE( STATUS "Checking for required libraries..." )
 
 find_package( JPEG REQUIRED )
-
 find_package( PNG REQUIRED )
 
 # deal with new missing X11 on OS X 10.8 Mountain Lion
@@ -97,7 +95,7 @@ find_package( PNG REQUIRED )
 # SET( PNG_INCLUDE_DIR /opt/X11/include )
 
 set (FLTK_SKIP_FLUID TRUE) 
-find_package( FLTK REQUIRED )
+find_package( FLTK )
 find_package( OpenGL REQUIRED )
 
 IF( NOT OPENGL_GLU_FOUND )
@@ -121,12 +119,17 @@ MESSAGE( STATUS "Checking for optional libraries..." )
 SET(PC_LIBRARIES ${FLTK_LIBRARIES} ${OPENGL_LIBRARIES})
 SET(PC_INCLUDE_DIRS ${FLTK_INCLUDE_DIR} ${OPENGL_INCLUDE_DIR})
 
+if(FLTK_FOUND)
+	LIST(APPEND PC_LIBRARIES ${FLTK_LIBRARIES})
+	LIST(APPEND PC_INCLUDE_DIRS ${FLTK_INCLUDE_DIR})
+endif(FLTK_FOUND)
+
 SET(PC_LINK_FLAGS "")
 FOREACH(LIB ${PC_LIBRARIES})
   GET_FILENAME_COMPONENT(LIBNAME ${LIB} NAME_WE)
   STRING(REGEX REPLACE "^lib" "" LINKLIB ${LIBNAME})
   SET(PC_LINK_FLAGS "${PC_LINK_FLAGS} -l${LINKLIB}")
-ENDFOREACH(LIB ${FLTK_LIBRARIES})
+ENDFOREACH(LIB ${PC_LIBRARIES})
 
 SET(PC_INCLUDE_FLAGS "")
 FOREACH(INC ${PC_INCLUDE_DIRS})
@@ -143,12 +146,15 @@ MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}"
 include_directories( . 
 		     libstage 
 		     replace 
-		     ${FLTK_INCLUDE_DIR}
-                     ${PNG_INCLUDE_DIR}
-                     ${JPEG_INCLUDE_DIR}
+             ${PNG_INCLUDE_DIR}
+             ${JPEG_INCLUDE_DIR}
 		     ${CMAKE_INCLUDE_PATH}
 )
 
+if(FLTK_FOUND)
+	include_directories(${FLTK_INCLUDE_DIR})
+endif(FLTK_FOUND)
+
 
 # work through these subdirs
 ADD_SUBDIRECTORY(libstage)
diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index 2c23329a0..1f9e5648e 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -3,7 +3,7 @@ MESSAGE( STATUS "Configuring libstage" )
 # for config.h
 include_directories(${PROJECT_BINARY_DIR})
 
-set( stageSrcs    
+set(stageSrcs
 	block.cc
 	blockgroup.cc
 	camera.cc
@@ -39,72 +39,65 @@ set( stageSrcs
 	ancestor.cc
 	image.cc
 )
-#	worldgui.cc 
 
-#set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS" )
- 
-add_library(stage SHARED ${stageSrcs})
+# Core library. Contains simulation code and most part of platform-independent rendering code
+add_library(stage_core SHARED ${stageSrcs})
+target_link_libraries( stage_core ${OPENGL_LIBRARIES} ${LTDL_LIB} ${JPEG_LIBRARIES} ${PNG_LIBRARIES})
 
-# GUI part of the stage
-add_library(stage_gui SHARED canvas_fltk.cc options_dlg.cc worldgui.cc)
-
-# if fltk-config didn't bring along the OpenGL dependencies (eg. on
-# Debian/Ubuntu), add them explicity 
-IF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
-  target_link_libraries( stage ${OPENGL_LIBRARIES})
-  #target_link_libraries( stage_gui ${OPENGL_LIBRARIES}) 
-ENDIF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
-
-
-# causes the shared library to have a version number
-set_target_properties( stage_gui PROPERTIES
-		       VERSION ${VERSION}
-#           LINK_FLAGS "${FLTK_LDFLAGS}"  
-)
-
-target_link_libraries( stage 
-                       ${LTDL_LIB} 
-                       ${JPEG_LIBRARIES} 
-                       ${PNG_LIBRARIES}
-)
-
-target_link_libraries( stage_gui stage ${FLTK_LIBRARIES})
-
-set( stagebinarySrcs main.cc )
-set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
-
-# TODO: build an app bundle on OS X
-#set(MACOSX_BUNDLE_INFO_STRING "${PROJECT_NAME}")
-#set(MACOSX_BUNDLE_GUI_IDENTIFIER "org.playerstage.stage")
-#set(MACOSX_BUNDLE_LONG_VERSION_STRING "${PROJECT_NAME} Version ${VERSION}")
-#set(MACOSX_BUNDLE_BUNDLE_NAME ${PROJECT_NAME})
-#set(MACOSX_BUNDLE_SHORT_VERSION_STRING ${VERSION})
-#set(MACOSX_BUNDLE_BUNDLE_VERSION ${VERSION})
-#set(MACOSX_BUNDLE_COPYRIGHT "Copyright 2010. All Rights Reserved.")
-# add_executable( stagebinary MACOSX_BUNDLE ${stagebinarySrcs} )
-
-add_executable( stagebinary ${stagebinarySrcs} )
-set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage )
-
-# Apple seems to have trouble when libstage and stagebinary are linked against FLTK
-# Newer Linux distributions won't allow stagebinary to inherit libstage's links to fltk, so we need
-# to explicitly link on Linux
-target_link_libraries( stagebinary stage stage_gui )
-
-# Headless version
+# Headless binary version. Does not need FLTK, but stil references a lot of OpenGL
 add_executable( stagebinary_headless main_headless.cc )
 set_target_properties( stagebinary_headless PROPERTIES OUTPUT_NAME stage_headless )
-target_link_libraries( stagebinary_headless stage )
-
-IF(PROJECT_OS_LINUX)
-  target_link_libraries( stagebinary stage pthread )
-ENDIF(PROJECT_OS_LINUX)
+target_link_libraries( stagebinary_headless stage_core )
 
-INSTALL(TARGETS stagebinary stagebinary_headless stage stage_gui 
-	RUNTIME DESTINATION bin
-	LIBRARY DESTINATION ${PROJECT_LIB_DIR}
+INSTALL(TARGETS stagebinary_headless stage_core
+	RUNTIME DESTINATION bin LIBRARY DESTINATION ${PROJECT_LIB_DIR}
 )
 
-INSTALL(FILES stage.hh 
-        DESTINATION include/${PROJECT_NAME}-${APIVERSION})
+INSTALL(FILES stage.hh DESTINATION include/${PROJECT_NAME}-${APIVERSION})
+
+# GUI part of the stage
+if(FLTK_FOUND AND BUILD_GUI)
+	add_library(stage SHARED canvas_fltk.cc options_dlg.cc worldgui.cc)
+	
+	# if fltk-config didn't bring along the OpenGL dependencies (eg. on
+	# Debian/Ubuntu), add them explicity 
+	IF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
+	  target_link_libraries( stage ${OPENGL_LIBRARIES}) 
+	ENDIF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
+	
+	# causes the shared library to have a version number
+	set_target_properties( stage PROPERTIES
+			       VERSION ${VERSION}
+	#           LINK_FLAGS "${FLTK_LDFLAGS}"  
+	)
+	
+	target_link_libraries( stage stage_core ${FLTK_LIBRARIES})
+	
+	set( stagebinarySrcs main.cc )
+	set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
+	
+	# TODO: build an app bundle on OS X
+	#set(MACOSX_BUNDLE_INFO_STRING "${PROJECT_NAME}")
+	#set(MACOSX_BUNDLE_GUI_IDENTIFIER "org.playerstage.stage")
+	#set(MACOSX_BUNDLE_LONG_VERSION_STRING "${PROJECT_NAME} Version ${VERSION}")
+	#set(MACOSX_BUNDLE_BUNDLE_NAME ${PROJECT_NAME})
+	#set(MACOSX_BUNDLE_SHORT_VERSION_STRING ${VERSION})
+	#set(MACOSX_BUNDLE_BUNDLE_VERSION ${VERSION})
+	#set(MACOSX_BUNDLE_COPYRIGHT "Copyright 2010. All Rights Reserved.")
+	# add_executable( stagebinary MACOSX_BUNDLE ${stagebinarySrcs} )
+	
+	add_executable( stagebinary ${stagebinarySrcs} )
+	set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage )
+	
+	# Apple seems to have trouble when libstage and stagebinary are linked against FLTK
+	# Newer Linux distributions won't allow stagebinary to inherit libstage's links to fltk, so we need
+	# to explicitly link on Linux
+	target_link_libraries( stagebinary stage_core stage )
+	
+	IF(PROJECT_OS_LINUX)
+	  target_link_libraries( stagebinary stage pthread )
+	ENDIF(PROJECT_OS_LINUX)
+	
+	INSTALL(TARGETS stagebinary stage RUNTIME DESTINATION bin LIBRARY DESTINATION ${PROJECT_LIB_DIR})
+endif(FLTK_FOUND AND BUILD_GUI)
 

From 2d6bea719ad823ab07914630de04af20e49c2dfa Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 13:29:39 +0300
Subject: [PATCH 07/22] Fixing stage-config to use headless mode properly

---
 CMakeLists.txt          | 19 ++++++++++++++++---
 libstage/CMakeLists.txt |  4 ++++
 stage-config.cmake.in   | 15 ++++++++++++---
 3 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d4f0b79f4..59b3cfc62 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -122,6 +122,11 @@ SET(PC_INCLUDE_DIRS ${FLTK_INCLUDE_DIR} ${OPENGL_INCLUDE_DIR})
 if(FLTK_FOUND)
 	LIST(APPEND PC_LIBRARIES ${FLTK_LIBRARIES})
 	LIST(APPEND PC_INCLUDE_DIRS ${FLTK_INCLUDE_DIR})
+else(FLTK_FOUND)
+	if(BUILD_GUI)
+		message(STATUS "Could not find FLTK. Turning off BUILD_GUI")
+		set(BUILD_GUI OFF)
+	endif(BUILD_GUI)
 endif(FLTK_FOUND)
 
 SET(PC_LINK_FLAGS "")
@@ -183,13 +188,21 @@ ENDIF ( BUILD_PLAYER_PLUGIN )
 # Create the CMake module files (needs to be run after the stage target was created)
 if (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION} GREATER 2.8.11)
   # Use a generator expression if the version of cmake allows it.
-  set(STAGE_TARGET_NAME "$<TARGET_FILE_NAME:stage>")
+  set(STAGE_CORE_TARGET_NAME "$<TARGET_FILE_NAME:stage_core>")
+  if(BUILD_GUI)
+  	set(STAGE_TARGET_NAME "$<TARGET_FILE_NAME:stage>")
+  endif(BUILD_GUI)
 else()
   # Otherwise use the LOCATION property of the target (this will produce a warning on newer versions of cmake)
-  get_property(location_ TARGET stage PROPERTY LOCATION)
-  get_filename_component(STAGE_TARGET_NAME "${location_}" NAME)
+  get_property(location_ TARGET stage_core PROPERTY LOCATION)
+  get_filename_component(STAGE_CORE_TARGET_NAME "${location_}" NAME)
+  if(BUILD_GUI)
+  	get_property(location_ TARGET stage PROPERTY LOCATION)
+  	get_filename_component(STAGE_TARGET_NAME "${location_}" NAME)
+  endif(BUILD_GUI)
   unset(location_)
 endif()
+
 CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/stage-config.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/stage-config.cmake @ONLY)
 if (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION} GREATER 2.8.11)
   # Also run it through file(GENERATE ...) to expand generator expressions (if the version of cmake supports it).
diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index 1f9e5648e..c23196af7 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -42,6 +42,10 @@ set(stageSrcs
 
 # Core library. Contains simulation code and most part of platform-independent rendering code
 add_library(stage_core SHARED ${stageSrcs})
+set_target_properties( stage_core PROPERTIES
+		       VERSION ${VERSION}
+#           LINK_FLAGS "${FLTK_LDFLAGS}"  
+)
 target_link_libraries( stage_core ${OPENGL_LIBRARIES} ${LTDL_LIB} ${JPEG_LIBRARIES} ${PNG_LIBRARIES})
 
 # Headless binary version. Does not need FLTK, but stil references a lot of OpenGL
diff --git a/stage-config.cmake.in b/stage-config.cmake.in
index 368d0519c..02b14eff6 100644
--- a/stage-config.cmake.in
+++ b/stage-config.cmake.in
@@ -7,8 +7,17 @@ set(STAGE_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME@-@APIVERSIO
   "@FLTK_INCLUDE_DIR@"
   "@OPENGL_INCLUDE_DIR@")
 list(REMOVE_DUPLICATES STAGE_INCLUDE_DIRS)
+
+set(STAGE_USE_GUI @BUILD_GUI@)
 set(STAGE_LIBRARIES
-  "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@"
-  "@FLTK_LIBRARIES@"
-  "@OPENGL_gl_LIBRARIES@")
+  "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_CORE_TARGET_NAME@"
+  "@OPENGL_gl_LIBRARIES@"
+)
+
+
+if(STAGE_USE_GUI)
+	LIST(APPEND STAGE_LIBRARIES "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
+	LIST(APPEND STAGE_LIBRARIES "@FLTK_LIBRARIES@")
+endif(STAGE_USE_GUI)
+
 

From f074ccf58433495bfc66786e75ed3d6dedd4a0fb Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 13:48:16 +0300
Subject: [PATCH 08/22] Removed fltk includes from option.hh

---
 libstage/option.hh | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/libstage/option.hh b/libstage/option.hh
index 038ba9c05..33fbb1bdd 100644
--- a/libstage/option.hh
+++ b/libstage/option.hh
@@ -4,9 +4,6 @@
 #include "worldfile.hh"
 #include <string>
 
-#include <FL/Fl_Menu_Bar.H>
-#include <FL/Fl_Menu_Item.H>
-
 namespace Stg {
 
 class Option;

From 5f8bf66082247c68966700f63ba0e459b11bd704 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 13:53:09 +0300
Subject: [PATCH 09/22] Moved MenuManagerFLTK to a separate files

---
 libstage/CMakeLists.txt       |  2 +-
 libstage/canvas_fltk.cc       | 54 +--------------------------
 libstage/canvas_fltk.hh       | 18 +--------
 libstage/menu_manager_fltk.cc | 70 +++++++++++++++++++++++++++++++++++
 libstage/menu_manager_fltk.hh | 37 ++++++++++++++++++
 5 files changed, 110 insertions(+), 71 deletions(-)
 create mode 100644 libstage/menu_manager_fltk.cc
 create mode 100644 libstage/menu_manager_fltk.hh

diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index c23196af7..95a5b25bc 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -61,7 +61,7 @@ INSTALL(FILES stage.hh DESTINATION include/${PROJECT_NAME}-${APIVERSION})
 
 # GUI part of the stage
 if(FLTK_FOUND AND BUILD_GUI)
-	add_library(stage SHARED canvas_fltk.cc options_dlg.cc worldgui.cc)
+	add_library(stage SHARED canvas_fltk.cc options_dlg.cc menu_manager_fltk.cc worldgui.cc)
 	
 	# if fltk-config didn't bring along the OpenGL dependencies (eg. on
 	# Debian/Ubuntu), add them explicity 
diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index 6c0b55fc6..16abda3cd 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -1,4 +1,5 @@
 #include "canvas_fltk.hh"
+#include "menu_manager_fltk.hh"
 #include <FL/Fl.H>
 #include <FL/gl.h>
 #include "png.h"
@@ -6,59 +7,6 @@
 namespace Stg
 {
 
-Fl_Menu_Item *getMenuItem(Fl_Menu_ *menu, int i)
-{
-  const Fl_Menu_Item *mArr = menu->menu();
-  return const_cast<Fl_Menu_Item *>(&mArr[i]);
-}
-
-MenuManagerFLTK::MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu)
-	:canvas(canvas), menu(menu)
-{
-
-}
-
-CanvasFLTK * MenuManagerFLTK::GetCanvas()
-{
-	return canvas;
-}
-
-void MenuManagerFLTK::createMenuItem(Option & opt, std::string path)
-{
-  path = path + "/" + opt.name();
-  std::string key = opt.key();
-  // create a menu item and save its index
-  size_t index = menu->add(path.c_str(), key.c_str(), toggleCb, &opt, FL_MENU_TOGGLE | (opt.val() ? FL_MENU_VALUE : 0));
-  opt.onAttachGUI(this, reinterpret_cast<void*>(index));
-}
-
-void MenuManagerFLTK::syncMenuState(Option * opt)
-{
-	size_t index = reinterpret_cast<size_t>(opt->getWidgetData());
-
-  if (menu != NULL) {
-    Fl_Menu_Item *item = getMenuItem(menu, index);
-    if(item != NULL) {
-    	opt->val() ? item->set() : item->clear();
-    }
-  }
-
-  CanvasFLTK *canvas = GetCanvas();
-  if (canvas != NULL)
-  {
-    canvas->setInvalidate();
-    canvas->redraw();
-  }
-}
-
-void MenuManagerFLTK::toggleCb(Fl_Widget *, void *p)
-{
-  // Fl_Menu_* menu = static_cast<Fl_Menu_*>( w );
-  Option *opt = static_cast<Option *>(p);
-  opt->invert();
-  opt->callValueChanged();
-}
-
 CanvasFLTK::CanvasFLTK(WorldGui *world, int x, int y, int width, int height)
 :Canvas(world, x, y, width, height)
 ,Fl_Gl_Window(x, y, width, height), selectedModel(false), startx(-1), starty(-1),
diff --git a/libstage/canvas_fltk.hh b/libstage/canvas_fltk.hh
index 0f1c94ed2..48fadb731 100644
--- a/libstage/canvas_fltk.hh
+++ b/libstage/canvas_fltk.hh
@@ -9,23 +9,7 @@
 
 namespace Stg {
 
-class CanvasFLTK;
-
-class MenuManagerFLTK : public MenuManager
-{
-public:
-	MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu);
-	virtual void syncMenuState(Option * opt);
-	/** Creates menu item for an option */
-	void createMenuItem(Option & opt, std::string path);
-protected:
-	/** Toggle callback from FLTK */
-	static void toggleCb(Fl_Widget *w, void *p);
-	CanvasFLTK * GetCanvas();
-private:
-	CanvasFLTK * canvas;
-	Fl_Menu_ *menu;
-};
+class MenuManagerFLTK;
 
 class CanvasFLTK : public Canvas, public Fl_Gl_Window {
   friend class WorldGui; // allow access to private members
diff --git a/libstage/menu_manager_fltk.cc b/libstage/menu_manager_fltk.cc
new file mode 100644
index 000000000..210282e16
--- /dev/null
+++ b/libstage/menu_manager_fltk.cc
@@ -0,0 +1,70 @@
+/*
+ * menu_manager_ftlk.cc
+ *
+ *  Created on: Nov 13, 2017
+ *      Author: vrobot
+ */
+
+#include "menu_manager_fltk.hh"
+#include "canvas_fltk.hh"
+
+
+namespace Stg
+{
+
+Fl_Menu_Item *getMenuItem(Fl_Menu_ *menu, int i)
+{
+  const Fl_Menu_Item *mArr = menu->menu();
+  return const_cast<Fl_Menu_Item *>(&mArr[i]);
+}
+
+MenuManagerFLTK::MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu)
+	:canvas(canvas), menu(menu)
+{
+
+}
+
+CanvasFLTK * MenuManagerFLTK::GetCanvas()
+{
+	return canvas;
+}
+
+void MenuManagerFLTK::createMenuItem(Option & opt, std::string path)
+{
+  path = path + "/" + opt.name();
+  std::string key = opt.key();
+  // create a menu item and save its index
+  size_t index = menu->add(path.c_str(), key.c_str(), toggleCb, &opt, FL_MENU_TOGGLE | (opt.val() ? FL_MENU_VALUE : 0));
+  opt.onAttachGUI(this, reinterpret_cast<void*>(index));
+}
+
+void MenuManagerFLTK::syncMenuState(Option * opt)
+{
+	size_t index = reinterpret_cast<size_t>(opt->getWidgetData());
+
+  if (menu != NULL) {
+    Fl_Menu_Item *item = getMenuItem(menu, index);
+    if(item != NULL) {
+    	opt->val() ? item->set() : item->clear();
+    }
+  }
+
+  CanvasFLTK *canvas = GetCanvas();
+  if (canvas != NULL)
+  {
+    canvas->setInvalidate();
+    canvas->redraw();
+  }
+}
+
+void MenuManagerFLTK::toggleCb(Fl_Widget *, void *p)
+{
+  // Fl_Menu_* menu = static_cast<Fl_Menu_*>( w );
+  Option *opt = static_cast<Option *>(p);
+  opt->invert();
+  opt->callValueChanged();
+}
+
+}
+
+
diff --git a/libstage/menu_manager_fltk.hh b/libstage/menu_manager_fltk.hh
new file mode 100644
index 000000000..2457d44f5
--- /dev/null
+++ b/libstage/menu_manager_fltk.hh
@@ -0,0 +1,37 @@
+/*
+ * menu_manager_fltk.hh
+ *
+ *  Created on: Nov 13, 2017
+ *      Author: vrobot
+ */
+
+#ifndef LIBSTAGE_MENU_MANAGER_FLTK_HH_
+#define LIBSTAGE_MENU_MANAGER_FLTK_HH_
+
+#include "option.hh"
+#include <FL/Fl_Menu_Bar.H>
+
+namespace Stg {
+
+class CanvasFLTK;
+
+class MenuManagerFLTK : public MenuManager
+{
+public:
+	MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu);
+	virtual void syncMenuState(Option * opt);
+	/** Creates menu item for an option */
+	void createMenuItem(Option & opt, std::string path);
+protected:
+	/** Toggle callback from FLTK */
+	static void toggleCb(Fl_Widget *w, void *p);
+	CanvasFLTK * GetCanvas();
+private:
+	CanvasFLTK * canvas;
+	Fl_Menu_ *menu;
+};
+
+}
+
+
+#endif /* LIBSTAGE_MENU_MANAGER_FLTK_HH_ */

From 9d9edc8426325b75b3eafcaf8dea455497cd6282 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 13:59:54 +0300
Subject: [PATCH 10/22] Removed reference to FLTK in texture_manager

---
 libstage/texture_manager.hh | 2 --
 1 file changed, 2 deletions(-)

diff --git a/libstage/texture_manager.hh b/libstage/texture_manager.hh
index 07dbccf16..ecdb1e6e7 100644
--- a/libstage/texture_manager.hh
+++ b/libstage/texture_manager.hh
@@ -9,8 +9,6 @@
 #define _TEXTURE_MANAGER_H_
 
 #include "stage.hh"
-
-#include <FL/Fl_Shared_Image.H>
 #include <iostream>
 
 namespace Stg {

From 315d225c020f3c0309d725eccd3f539aea75aa83 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 14:04:14 +0300
Subject: [PATCH 11/22] Fixing build problems

---
 examples/ctrl/CMakeLists.txt | 2 +-
 libstage/canvas.cc           | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/examples/ctrl/CMakeLists.txt b/examples/ctrl/CMakeLists.txt
index a9c367980..a2bbf2f02 100644
--- a/examples/ctrl/CMakeLists.txt
+++ b/examples/ctrl/CMakeLists.txt
@@ -26,7 +26,7 @@ set_source_files_properties( ${PLUGINS} PROPERTIES
 )
 
 foreach( PLUGIN ${PLUGINS} )
-  TARGET_LINK_LIBRARIES( ${PLUGIN} stage ${OPENGL_LIBRARIES} )
+  TARGET_LINK_LIBRARIES( ${PLUGIN} stage_core ${OPENGL_LIBRARIES} )
 endforeach( PLUGIN )
 
 # delete the "lib" prefix from the plugin libraries
diff --git a/libstage/canvas.cc b/libstage/canvas.cc
index c560d0eb6..48c7b7b73 100644
--- a/libstage/canvas.cc
+++ b/libstage/canvas.cc
@@ -22,7 +22,6 @@
 #include <string>
 
 #include "file_manager.hh"
-#include "options_dlg.hh"
 #include "region.hh"
 
 using namespace Stg;

From 8dbd13a112308f1dd5c1f60441e36c0ce9cae14d Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 14:20:13 +0300
Subject: [PATCH 12/22] Fixed include error

---
 libstage/image.cc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/libstage/image.cc b/libstage/image.cc
index b652bb7f9..7b7ea6611 100644
--- a/libstage/image.cc
+++ b/libstage/image.cc
@@ -2,6 +2,7 @@
 #include <png.h>
 #include <cassert>
 #include <stdlib.h>
+#include <string.h>	// for memcpy
 
 namespace Stg{
 

From ee7e6aa387658d971db44b5dc21e6fc049993514 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 14:28:07 +0300
Subject: [PATCH 13/22] Another linker fix

---
 worlds/benchmark/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/worlds/benchmark/CMakeLists.txt b/worlds/benchmark/CMakeLists.txt
index 398000553..7a31ff16d 100644
--- a/worlds/benchmark/CMakeLists.txt
+++ b/worlds/benchmark/CMakeLists.txt
@@ -1,12 +1,12 @@
 SET( expand_swarmSrcs expand_swarm.cc )
 ADD_LIBRARY( expand_swarm MODULE ${expand_swarmSrcs} )
-TARGET_LINK_LIBRARIES( expand_swarm stage )
+TARGET_LINK_LIBRARIES( expand_swarm stage_core )
 set_source_files_properties( ${expand_swarmSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
 SET_TARGET_PROPERTIES( expand_swarm PROPERTIES PREFIX "" )
 
 SET( expand_pioneerSrcs expand_pioneer.cc )
 ADD_LIBRARY( expand_pioneer MODULE ${expand_pioneerSrcs} )
-TARGET_LINK_LIBRARIES( expand_pioneer stage )
+TARGET_LINK_LIBRARIES( expand_pioneer stage_core )
 set_source_files_properties( ${expand_pioneerSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
 SET_TARGET_PROPERTIES( expand_pioneer PROPERTIES PREFIX "" )
 

From 1e728a1b160394b2991f22fa9485ac745325f7b3 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 14:56:45 +0300
Subject: [PATCH 14/22] Fixing cmake config

---
 stage-config.cmake.in | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/stage-config.cmake.in b/stage-config.cmake.in
index 02b14eff6..da5aadb46 100644
--- a/stage-config.cmake.in
+++ b/stage-config.cmake.in
@@ -4,9 +4,7 @@
 #    STAGE_LIBRARIES    - Stage link libraries
 
 set(STAGE_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME@-@APIVERSION@"
-  "@FLTK_INCLUDE_DIR@"
   "@OPENGL_INCLUDE_DIR@")
-list(REMOVE_DUPLICATES STAGE_INCLUDE_DIRS)
 
 set(STAGE_USE_GUI @BUILD_GUI@)
 set(STAGE_LIBRARIES
@@ -14,10 +12,11 @@ set(STAGE_LIBRARIES
   "@OPENGL_gl_LIBRARIES@"
 )
 
-
 if(STAGE_USE_GUI)
+	LIST(APPEND STAGE_INCLUDE_DIRS "@FLTK_INCLUDE_DIR@")
 	LIST(APPEND STAGE_LIBRARIES "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
 	LIST(APPEND STAGE_LIBRARIES "@FLTK_LIBRARIES@")
 endif(STAGE_USE_GUI)
 
-
+list(REMOVE_DUPLICATES STAGE_INCLUDE_DIRS)
+list(REMOVE_DUPLICATES STAGE_LIBRARIES)

From 01886e2672afeed54380dd3d4797f553cd093ac3 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 15:02:05 +0300
Subject: [PATCH 15/22] Fixing cmake config

---
 stage-config.cmake.in | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/stage-config.cmake.in b/stage-config.cmake.in
index da5aadb46..d8c4d8471 100644
--- a/stage-config.cmake.in
+++ b/stage-config.cmake.in
@@ -3,7 +3,7 @@
 #    STAGE_INCLUDE_DIRS - Stage Include directories
 #    STAGE_LIBRARIES    - Stage link libraries
 
-set(STAGE_INCLUDE_DIRS "@CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME@-@APIVERSION@"
+set(STAGE_INCLUDE_DIRS "${stage_DIR}/../../../include/@PROJECT_NAME@-@APIVERSION@"
   "@OPENGL_INCLUDE_DIR@")
 
 set(STAGE_USE_GUI @BUILD_GUI@)

From 5486f68dde2e9dc1cb3fb83d7e22c50b8e469973 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Mon, 13 Nov 2017 15:14:53 +0300
Subject: [PATCH 16/22] Another fix to stage-config

---
 CMakeLists.txt        | 1 +
 stage-config.cmake.in | 8 +++++---
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 59b3cfc62..1b750c322 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -211,6 +211,7 @@ if (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}.${CMAKE_PATCH_VERSION} GREATER
     INPUT ${CMAKE_CURRENT_BINARY_DIR}/stage-config.cmake)
 endif()
 CONFIGURE_FILE (${CMAKE_CURRENT_SOURCE_DIR}/stage-config-version.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/stage-config-version.cmake @ONLY)
+
 INSTALL (FILES ${CMAKE_CURRENT_BINARY_DIR}/stage-config.cmake ${CMAKE_CURRENT_BINARY_DIR}/stage-config-version.cmake DESTINATION ${PROJECT_LIB_DIR}/cmake/${PROJECT_NAME})
 
 # generate a cpack config file used to create packaged tarballs
diff --git a/stage-config.cmake.in b/stage-config.cmake.in
index d8c4d8471..86b2f1d5f 100644
--- a/stage-config.cmake.in
+++ b/stage-config.cmake.in
@@ -3,18 +3,20 @@
 #    STAGE_INCLUDE_DIRS - Stage Include directories
 #    STAGE_LIBRARIES    - Stage link libraries
 
-set(STAGE_INCLUDE_DIRS "${stage_DIR}/../../../include/@PROJECT_NAME@-@APIVERSION@"
+get_filename_component(STAGE_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+set(STAGE_INCLUDE_DIRS "${STAGE_CMAKE_DIR}/../../../include/@PROJECT_NAME@-@APIVERSION@"
   "@OPENGL_INCLUDE_DIR@")
 
 set(STAGE_USE_GUI @BUILD_GUI@)
 set(STAGE_LIBRARIES
-  "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_CORE_TARGET_NAME@"
+  "${STAGE_CMAKE_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_CORE_TARGET_NAME@"
   "@OPENGL_gl_LIBRARIES@"
 )
 
 if(STAGE_USE_GUI)
 	LIST(APPEND STAGE_INCLUDE_DIRS "@FLTK_INCLUDE_DIR@")
-	LIST(APPEND STAGE_LIBRARIES "${stage_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
+	LIST(APPEND STAGE_LIBRARIES "${STAGE_CMAKE_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
 	LIST(APPEND STAGE_LIBRARIES "@FLTK_LIBRARIES@")
 endif(STAGE_USE_GUI)
 

From 1d754997028cb721e58a8a9eb52bbd53988dea27 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Wed, 6 Dec 2017 18:35:26 +0300
Subject: [PATCH 17/22] Ranger model checks if ranges are empty

---
 libstage/model_ranger.cc | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc
index 1302c035b..398e8d6ce 100644
--- a/libstage/model_ranger.cc
+++ b/libstage/model_ranger.cc
@@ -322,7 +322,7 @@ void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr) con
   
   std::vector<glpoint_t> verts( sample_count );
   
-  if (sample_count == 1) {
+  if (sample_count == 1 && !ranges.empty()) {
     // only one sample, so we fake up some beam width for beauty
     const double sidelen = ranges[0];
     const double da = fov / 2.0;
@@ -334,7 +334,7 @@ void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr) con
     verts[1].y = sidelen * sin(-da);
     verts[2].x = sidelen * cos(+da);
     verts[2].y = sidelen * sin(+da);
-  } else {
+  } else if (sample_count > 0 && !ranges.empty()) {
     for (size_t s(0); s < sample_count; s++) {
       const double ray_angle = (((double)s)-0.5) * sample_fov - fov / 2.0;
       verts[s].x = (float)(ranges[s] * cos(ray_angle));
@@ -342,7 +342,7 @@ void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr) con
     }
   }
   
-  if (vis->showArea) {        
+  if (vis->showArea && !verts.empty()) {
     // draw the filled polygon in transparent blue
     glEnable( GL_BLEND );
     glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
@@ -366,7 +366,7 @@ void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr) con
     glDepthMask(GL_TRUE);
   }
       
-  if (vis->showStrikes) {
+  if (vis->showStrikes && !verts.empty()) {
     rgr->PushColor(Color::blue); // solid color
     glPointSize(2);
 

From 47e8dc47dffc24a680981a3e2a4f84a7a8aab334 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Sun, 11 Feb 2018 16:07:34 +0300
Subject: [PATCH 18/22] Cleaned up rendering chain

Stripped Push/PopColor from Model class. Model often obtains direct pointer to canvas, so it can call this methods directly

Added explicit 'override' keywords for overriden methods. Now we strongly need c++11

Updated CMakeLists.txt and stage-config to properly install headers for GUI
---
 CMakeLists.txt                   |  19 +-
 avonstage/avonstage.cc           |   6 +-
 examples/ctrl/fasr2.cc           |   5 +-
 examples/libstage/stest.cc       |   2 +-
 libstage/CMakeLists.txt          |  13 +-
 libstage/block.cc                |   7 +-
 libstage/blockgroup.cc           |  17 +-
 libstage/canvas.cc               |  38 +-
 libstage/canvas.hh               |   3 +
 libstage/canvas_fltk.cc          |  25 ++
 libstage/canvas_fltk.hh          |  30 +-
 libstage/main_headless.cc        |   4 +-
 libstage/menu_manager_fltk.hh    |   2 +-
 libstage/model.cc                |  16 +-
 libstage/model_blobfinder.cc     |  20 +-
 libstage/model_draw.cc           |  93 ++---
 libstage/model_fiducial.cc       |   8 +-
 libstage/model_gripper.cc        |  13 +-
 libstage/model_lightindicator.cc |   6 +-
 libstage/model_position.cc       |  26 +-
 libstage/model_ranger.cc         | 676 ++++++++++++++++---------------
 libstage/stage.hh                | 170 ++++----
 libstage/vis_strip.cc            |   8 +-
 libstage/world_gui.hh            |  55 +--
 stage-config.cmake.in            |  15 +-
 25 files changed, 669 insertions(+), 608 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1b750c322..ad4c2c679 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,8 +50,8 @@ endif( PROJECT_OS_OSX)
 # Build type cflags
 SET (OPTIMIZE "-O2")
 
-SET (CMAKE_CXX_FLAGS_RELEASE "${FORCE_ARCH} ${OPTIMIZE} -DNDEBUG ${WALL}" CACHE INTERNAL "CXX Flags for release" FORCE)
-SET (CMAKE_CXX_FLAGS_DEBUG "-ggdb  ${FORCE_ARCH} ${WALL} -DDEBUG" CACHE INTERNAL "CXX Flags for debug" FORCE)
+SET (CMAKE_CXX_FLAGS_RELEASE "${FORCE_ARCH} ${OPTIMIZE} -DNDEBUG ${WALL} -Wsuggest-override -std=c++11" CACHE INTERNAL "CXX Flags for release" FORCE)
+SET (CMAKE_CXX_FLAGS_DEBUG "-ggdb  ${FORCE_ARCH} ${WALL} -DDEBUG -Wsuggest-override -std=c++11" CACHE INTERNAL "CXX Flags for debug" FORCE)
 SET (CMAKE_CXX_FLAGS_PROFILE "-ggdb -pg  ${FORCE_ARCH} ${WALL}" CACHE INTERNAL "CXX Flags for profile" FORCE)
 
 #####################################
@@ -72,13 +72,13 @@ SET(RGBFILE ${CMAKE_INSTALL_PREFIX}/share/stage/rgb.txt )
 # Create the config.h file
 # config.h belongs with the source (and not in CMAKE_CURRENT_BINARY_DIR as in Brian's original version)
 CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in 
-					${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
+				${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
 
 message( STATUS "Checking for libtool" )
 find_path( LTDL_INCLUDE_DIR ltdl.h DOC "Libtool include dir" )
 find_library( LTDL_LIB ltdl DOC "Libtool lib" )
 
-include_directories( 
+include_directories(
 	${OPENGL_INCLUDE_DIR}
 	${LTDL_INCLUDE_DIR}
 )
@@ -149,18 +149,17 @@ MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}"
 
 # all targets need these include directories
 include_directories( . 
-		     libstage 
-		     replace 
-             ${PNG_INCLUDE_DIR}
-             ${JPEG_INCLUDE_DIR}
-		     ${CMAKE_INCLUDE_PATH}
+	libstage
+	replace
+	${PNG_INCLUDE_DIR}
+	${JPEG_INCLUDE_DIR}
+	${CMAKE_INCLUDE_PATH}
 )
 
 if(FLTK_FOUND)
 	include_directories(${FLTK_INCLUDE_DIR})
 endif(FLTK_FOUND)
 
-
 # work through these subdirs
 ADD_SUBDIRECTORY(libstage)
 ADD_SUBDIRECTORY(examples)
diff --git a/avonstage/avonstage.cc b/avonstage/avonstage.cc
index 7cb78e52f..c14c19f11 100644
--- a/avonstage/avonstage.cc
+++ b/avonstage/avonstage.cc
@@ -445,12 +445,12 @@ int main(int argc, char *argv[])
     // help options
     case '?':
       puts(USAGE);
-      exit(0);
+      return -1;
       break;
     default:
       printf("unhandled option %c\n", ch);
       puts(USAGE);
-      // exit(0);
+      return -1;
     }
   }
 
@@ -458,7 +458,7 @@ int main(int argc, char *argv[])
 
   if (worldfilename == NULL) {
     puts("[AvonStage] no worldfile specified on command line. Quit.\n");
-    exit(-1);
+    return -1;
   }
 
   puts(""); // end the first start-up line
diff --git a/examples/ctrl/fasr2.cc b/examples/ctrl/fasr2.cc
index acb66fd15..08eb643c6 100644
--- a/examples/ctrl/fasr2.cc
+++ b/examples/ctrl/fasr2.cc
@@ -1,6 +1,7 @@
 #include <pthread.h>
 
 #include "stage.hh"
+#include "canvas.hh"
 using namespace Stg;
 
 // generic planner implementation
@@ -166,9 +167,9 @@ class Robot {
       Color c = mod->GetColor();
       c.a = 0.4;
 
-      mod->PushColor(c);
+      canvas->PushColor(c);
       (*graphpp)->Draw();
-      mod->PopColor();
+      canvas->PopColor();
 
       glPopMatrix();
     }
diff --git a/examples/libstage/stest.cc b/examples/libstage/stest.cc
index b2d1522d7..b4661368a 100644
--- a/examples/libstage/stest.cc
+++ b/examples/libstage/stest.cc
@@ -146,7 +146,7 @@ int main(int argc, char *argv[])
   // check and handle the argumets
   if (argc < 3) {
     puts("Usage: stest <worldfile> <number of robots>");
-    exit(0);
+    return -1;
   }
 
   const int popsize = atoi(argv[2]);
diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index 95a5b25bc..a52605696 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -53,11 +53,11 @@ add_executable( stagebinary_headless main_headless.cc )
 set_target_properties( stagebinary_headless PROPERTIES OUTPUT_NAME stage_headless )
 target_link_libraries( stagebinary_headless stage_core )
 
-INSTALL(TARGETS stagebinary_headless stage_core
+install(TARGETS stagebinary_headless stage_core
 	RUNTIME DESTINATION bin LIBRARY DESTINATION ${PROJECT_LIB_DIR}
 )
 
-INSTALL(FILES stage.hh DESTINATION include/${PROJECT_NAME}-${APIVERSION})
+install(FILES stage.hh DESTINATION include/${PROJECT_NAME}-${APIVERSION})
 
 # GUI part of the stage
 if(FLTK_FOUND AND BUILD_GUI)
@@ -98,10 +98,13 @@ if(FLTK_FOUND AND BUILD_GUI)
 	# to explicitly link on Linux
 	target_link_libraries( stagebinary stage_core stage )
 	
-	IF(PROJECT_OS_LINUX)
+	if(PROJECT_OS_LINUX)
 	  target_link_libraries( stagebinary stage pthread )
-	ENDIF(PROJECT_OS_LINUX)
+	endif(PROJECT_OS_LINUX)
+	install(TARGETS stagebinary stage RUNTIME DESTINATION bin LIBRARY DESTINATION ${PROJECT_LIB_DIR})
 	
-	INSTALL(TARGETS stagebinary stage RUNTIME DESTINATION bin LIBRARY DESTINATION ${PROJECT_LIB_DIR})
+	# All GUI-specific code was stripped from stage.hh
+	# GUI-supported world now is located at world_gui.hh, so it should be installed
+	install(FILES world_gui.hh DESTINATION include/${PROJECT_NAME}-${APIVERSION})
 endif(FLTK_FOUND AND BUILD_GUI)
 
diff --git a/libstage/block.cc b/libstage/block.cc
index 31ff5669a..4eca64d9f 100644
--- a/libstage/block.cc
+++ b/libstage/block.cc
@@ -1,6 +1,8 @@
 #include "region.hh"
 #include "worldfile.hh"
 
+
+
 using namespace Stg;
 using std::vector;
 
@@ -39,8 +41,6 @@ void Block::Translate(double x, double y)
     it->x += x;
     it->y += y;
   }
-
-  group->BuildDisplayList();
 }
 
 /** Return the value half way between the min and max Y position of
@@ -108,9 +108,6 @@ void Block::SetZ(double min, double max)
 {
   local_z.min = min;
   local_z.max = max;
-
-  // force redraw
-  group->BuildDisplayList();
 }
 
 void Block::AppendTouchingModels(std::set<Model *> &touchers)
diff --git a/libstage/blockgroup.cc b/libstage/blockgroup.cc
index 3cf159159..784a4cbcc 100644
--- a/libstage/blockgroup.cc
+++ b/libstage/blockgroup.cc
@@ -1,5 +1,5 @@
-
 #include "stage.hh"
+#include "canvas.hh"
 #include "worldfile.hh"
 
 #include <cmath>
@@ -7,6 +7,7 @@
 #include <limits.h> // for _POSIX_PATH_MAX
 #include <limits>
 
+
 using namespace Stg;
 using namespace std;
 
@@ -173,7 +174,7 @@ static void combineCallback(GLdouble coords[3],
 }
 
 // render each block as a polygon extruded into Z
-void BlockGroup::BuildDisplayList()
+void BlockGroup::BuildDisplayList(Canvas * canvas)
 {
   static GLUtesselator *tobj = NULL;
 
@@ -224,7 +225,7 @@ void BlockGroup::BuildDisplayList()
   glEnable(GL_POLYGON_OFFSET_FILL);
   glPolygonOffset(0.5, 0.5);
 
-  mod.PushColor(mod.color);
+  canvas->PushColor(mod.color);
 
   gluTessBeginPolygon(tobj, NULL);
 
@@ -240,7 +241,7 @@ void BlockGroup::BuildDisplayList()
   FOR_EACH (blk, blocks)
     blk->DrawSides();
 
-  mod.PopColor();
+  canvas->PopColor();
 
   // now outline the polys
   glDisable(GL_POLYGON_OFFSET_FILL);
@@ -251,7 +252,7 @@ void BlockGroup::BuildDisplayList()
   c.r /= 2.0;
   c.g /= 2.0;
   c.b /= 2.0;
-  mod.PushColor(c);
+  canvas->PushColor(c);
 
   gluTessBeginPolygon(tobj, NULL);
 
@@ -270,15 +271,15 @@ void BlockGroup::BuildDisplayList()
   glDepthMask(GL_TRUE);
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
-  mod.PopColor();
+  canvas->PopColor();
 
   glEndList();
 }
 
-void BlockGroup::CallDisplayList()
+void BlockGroup::CallDisplayList(Canvas * canvas)
 {
   if (displaylist == 0 || mod.rebuild_displaylist) {
-    BuildDisplayList();
+    BuildDisplayList(canvas);
     mod.rebuild_displaylist = 0;
   }
 
diff --git a/libstage/canvas.cc b/libstage/canvas.cc
index 48c7b7b73..4313b48cc 100644
--- a/libstage/canvas.cc
+++ b/libstage/canvas.cc
@@ -424,7 +424,7 @@ void Canvas::DrawFloor()
 void Canvas::DrawBlocks()
 {
   FOR_EACH (it, models_sorted)
-    (*it)->DrawBlocksTree();
+    (*it)->DrawBlocksTree(this);
 }
 
 void Canvas::DrawBoundingBoxes()
@@ -435,7 +435,7 @@ void Canvas::DrawBoundingBoxes()
   //glDisable(GL_CULL_FACE);
   const std::vector<Model *> & children = world->GetChildren();
   FOR_EACH (it, children)
-      (*it)->DrawBoundingBoxTree();
+      (*it)->DrawBoundingBoxTree(this);
 
   //glEnable(GL_CULL_FACE);
   glLineWidth(1.0);
@@ -534,7 +534,7 @@ void Canvas::renderFrame()
     GLfloat scale = 1.0 / world->Resolution();
     glScalef(scale, scale, 1.0); // XX TODO - this seems slightly
 
-    world->PushColor(Color(0, 0, 1, 0.5));
+    PushColor(Color(0, 0, 1, 0.5));
 
     glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
@@ -558,17 +558,17 @@ void Canvas::renderFrame()
     glEnd();
 
 #if 1
-    world->PushColor(Color(0, 1, 0, 0.2));
+    PushColor(Color(0, 1, 0, 0.2));
     glBegin(GL_LINE_STRIP);
     for (unsigned int i = 0; i < world->rt_cells.size(); i++) {
       glVertex2f(world->rt_cells[i].x + 0.5, world->rt_cells[i].y + 0.5);
     }
     glEnd();
-    world->PopColor();
+    PopColor();
 #endif
 
     glPopMatrix();
-    world->PopColor();
+    PopColor();
   }
 
   if (!world->rt_candidate_cells.empty()) {
@@ -576,7 +576,7 @@ void Canvas::renderFrame()
     GLfloat scale = 1.0 / world->Resolution();
     glScalef(scale, scale, 1.0); // XX TODO - this seems slightly
 
-    world->PushColor(Color(1, 0, 0, 0.5));
+    PushColor(Color(1, 0, 0, 0.5));
 
     glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
 
@@ -598,16 +598,16 @@ void Canvas::renderFrame()
               world->rt_candidate_cells[i].x + 1, world->rt_candidate_cells[i].y + 1);
     }
 
-    world->PushColor(Color(0, 1, 0, 0.2));
+    PushColor(Color(0, 1, 0, 0.2));
     glBegin(GL_LINE_STRIP);
     for (unsigned int i = 0; i < world->rt_candidate_cells.size(); i++) {
       glVertex2f(world->rt_candidate_cells[i].x + 0.5, world->rt_candidate_cells[i].y + 0.5);
     }
     glEnd();
-    world->PopColor();
+    PopColor();
 
     glPopMatrix();
-    world->PopColor();
+    PopColor();
 
     // world->rt_cells.clear();
   }
@@ -621,7 +621,7 @@ void Canvas::renderFrame()
     glDisable(GL_DEPTH_TEST); // using alpha blending
 
     FOR_EACH (it, models_sorted)
-      (*it)->DrawTrailFootprint();
+      (*it)->DrawTrailFootprint(current_camera, this);
 
     glEnable(GL_DEPTH_TEST);
   }
@@ -638,11 +638,11 @@ void Canvas::renderFrame()
 
   if (showTrailArrows)
     FOR_EACH (it, models_sorted)
-      (*it)->DrawTrailArrows();
+      (*it)->DrawTrailArrows(this);
 
   if (showTrailRise)
     FOR_EACH (it, models_sorted)
-      (*it)->DrawTrailBlocks();
+      (*it)->DrawTrailBlocks(current_camera, this);
 
   if (showBlocks)
     DrawBlocks();
@@ -934,3 +934,15 @@ void Canvas::draw_grid(bounds3d_t vol)
     draw_string(0, i, 0.00, str);
   }
 }
+
+void Canvas::DrawPose(Pose pose)
+{
+  PushColor(0, 0, 0, 1);
+  glPointSize(4);
+
+  glBegin(GL_POINTS);
+  glVertex3f(pose.x, pose.y, pose.z);
+  glEnd();
+
+  PopColor();
+}
diff --git a/libstage/canvas.hh b/libstage/canvas.hh
index 74f905ab9..cab58e353 100644
--- a/libstage/canvas.hh
+++ b/libstage/canvas.hh
@@ -120,8 +120,11 @@ public:
   void draw_speech_bubble(float x, float y, float z, const char *str);
   void draw_grid(bounds3d_t vol);
 
+  void DrawPose(Pose pose);
+
   void CanvasToWorld(int px, int py, double *wx, double *wy, double *wz);
 
+  // Get model under screen coordinates {x,y}
   Model *getModel(int x, int y);
   bool selected(Model *mod);
   void select(Model *mod);
diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index 16abda3cd..1f5e3de72 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -27,6 +27,31 @@ CanvasFLTK::~CanvasFLTK()
 
 }
 
+bool CanvasFLTK::isValid()
+{
+	return valid();
+}
+
+void CanvasFLTK::doRedraw()
+{
+	redraw();
+}
+
+int CanvasFLTK::getWidth() const
+{
+	return w();
+}
+
+int CanvasFLTK::getHeight() const
+{
+	return h();
+}
+
+void CanvasFLTK::setInvalidate()
+{
+	invalidate();
+}
+
 void CanvasFLTK::perspectiveCb(Option *opt, void *p)
 {
   Canvas *canvas = static_cast<Canvas *>(p);
diff --git a/libstage/canvas_fltk.hh b/libstage/canvas_fltk.hh
index 48fadb731..5f7cd7b52 100644
--- a/libstage/canvas_fltk.hh
+++ b/libstage/canvas_fltk.hh
@@ -33,32 +33,32 @@ public:
   void Screenshot();
   void createMenuItems(Fl_Menu_Bar *menu, std::string path);
 
-  virtual void renderFrame();
-  virtual int handle(int event);
-  void resize(int X, int Y, int W, int H);
+  virtual void renderFrame() override;
+  virtual int handle(int event) override;
+  void resize(int X, int Y, int W, int H) override;
 
-  virtual bool isValid() {return valid();}
-	virtual void doRedraw() {redraw();}
-	virtual int getWidth() const {return w();}
-	virtual int getHeight() const {return h();}
-	virtual void setInvalidate() { invalidate(); }
+  virtual bool isValid() override;
+	virtual void doRedraw() override;
+	virtual int getWidth() const override;
+	virtual int getHeight() const override;
+	virtual void setInvalidate() override;
 
-	double fontWidth(const char * str) const;
-	double fontHeight() const;
+	double fontWidth(const char * str) const override;
+	double fontHeight() const override;
 
-	void draw_string(float x, float y, float z, const char *str);
-	void draw_string_multiline(float x, float y, float w, float h, const char *str, int align);
+	void draw_string(float x, float y, float z, const char *str) override;
+	void draw_string_multiline(float x, float y, float w, float h, const char *str, int align) override;
 
   void InvertView(uint32_t invertflags);
 
   static void TimerCallback(Canvas *canvas);
   static void perspectiveCb(Option * opt, void *p);
 
-  void Load(Worldfile *wf, int sec);
-  void Save(Worldfile *wf, int section);
+  void Load(Worldfile *wf, int sec) override;
+  void Save(Worldfile *wf, int section) override;
 
   /** Overriding draw method from Fl_Gl_Window*/
-  virtual void draw();
+  virtual void draw() override;
 };
 } //namespace Stg
 
diff --git a/libstage/main_headless.cc b/libstage/main_headless.cc
index f49e96770..33bbbdf56 100644
--- a/libstage/main_headless.cc
+++ b/libstage/main_headless.cc
@@ -75,12 +75,12 @@ int main( int argc, char* argv[] )
     case 'h':
     case '?':
       puts(USAGE);
-      //			 exit(0);
+      return -1;
       break;
     default:
       printf("unhandled option %c\n", ch);
       puts(USAGE);
-      // exit(0);
+      return -1;
     }
   }
 
diff --git a/libstage/menu_manager_fltk.hh b/libstage/menu_manager_fltk.hh
index 2457d44f5..b170da767 100644
--- a/libstage/menu_manager_fltk.hh
+++ b/libstage/menu_manager_fltk.hh
@@ -19,7 +19,7 @@ class MenuManagerFLTK : public MenuManager
 {
 public:
 	MenuManagerFLTK(CanvasFLTK * canvas, Fl_Menu_Bar *menu);
-	virtual void syncMenuState(Option * opt);
+	virtual void syncMenuState(Option * opt) override;
 	/** Creates menu item for an option */
 	void createMenuItem(Option & opt, std::string path);
 protected:
diff --git a/libstage/model.cc b/libstage/model.cc
index e00a65c72..ec32a7bd0 100644
--- a/libstage/model.cc
+++ b/libstage/model.cc
@@ -891,7 +891,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 
   // go into world coordinates
   glPushMatrix();
-  mod->PushColor(1, 0, 0, 0.5);
+  canvas->PushColor(1, 0, 0, 0.5);
 
   Gl::pose_inverse_shift(mod->GetGlobalPose());
 
@@ -915,7 +915,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
     }
     glEnd();
 
-    mod->PopColor();
+    canvas->PopColor();
 
     glPopMatrix();
   }
@@ -926,7 +926,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 
   glScalef(cellwidth, cellheight, 1);
 
-  mod->PushColor(0, 0, 0, 0.5);
+  canvas->PushColor(0, 0, 0, 0.5);
   glPolygonMode(GL_FRONT, GL_FILL);
   for (unsigned int y = 0; y < height; ++y)
     for (unsigned int x = 0; x < width; ++x) {
@@ -937,7 +937,7 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 
   glTranslatef(0, 0, 0.01);
 
-  mod->PushColor(0, 0, 0, 1);
+  canvas->PushColor(0, 0, 0, 1);
   glPolygonMode(GL_FRONT, GL_LINE);
   for (unsigned int y = 0; y < height; ++y)
     for (unsigned int x = 0; x < width; ++x) {
@@ -951,16 +951,16 @@ void Model::RasterVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 
   glPolygonMode(GL_FRONT, GL_FILL);
 
-  mod->PopColor();
-  mod->PopColor();
+  canvas->PopColor();
+  canvas->PopColor();
 
-  mod->PushColor(0, 0, 0, 1);
+  canvas->PushColor(0, 0, 0, 1);
   char buf[128];
   snprintf(buf, 127, "[%u x %u]", width, height);
   glTranslatef(0, 0, 0.01);
   canvas->draw_string(1, height - 1, 0, buf);
 
-  mod->PopColor();
+  canvas->PopColor();
 
   glPopMatrix();
 }
diff --git a/libstage/model_blobfinder.cc b/libstage/model_blobfinder.cc
index ca8fcd119..331c70cf7 100644
--- a/libstage/model_blobfinder.cc
+++ b/libstage/model_blobfinder.cc
@@ -12,12 +12,10 @@
 //  $Revision$
 //
 ///////////////////////////////////////////////////////////////////////////
-
-//#define DEBUG
-
 #include <sys/time.h>
 
 #include "option.hh"
+#include "canvas.hh"
 #include "stage.hh"
 #include "worldfile.hh"
 using namespace Stg;
@@ -295,7 +293,7 @@ void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
     // draw the FOV
     GLUquadric *quadric = gluNewQuadric();
 
-    bf->PushColor(0, 0, 0, 0.2);
+    canvas->PushColor(0, 0, 0, 0.2);
 
     gluQuadricDrawStyle(quadric, GLU_SILHOUETTE);
     gluPartialDisk(quadric, 0, bf->range,
@@ -305,7 +303,7 @@ void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
                    rtod(-bf->fov)); // sweep angle
 
     gluDeleteQuadric(quadric);
-    bf->PopColor();
+    canvas->PopColor();
   }
 
   if (bf->subs < 1)
@@ -332,16 +330,16 @@ void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
   glScalef(0.025, 0.025, 1);
 
   // draw a white screen with a black border
-  bf->PushColor(1, 1, 1, 1);
+  canvas->PushColor(1, 1, 1, 1);
   glRectf(0, 0, bf->scan_width, bf->scan_height);
-  bf->PopColor();
+  canvas->PopColor();
 
   glTranslatef(0, 0, 0.01);
 
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
-  bf->PushColor(1, 0, 0, 1);
+  canvas->PushColor(1, 0, 0, 1);
   glRectf(0, 0, bf->scan_width, bf->scan_height);
-  bf->PopColor();
+  canvas->PopColor();
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
   // draw the blobs on the screen
@@ -350,12 +348,12 @@ void ModelBlobfinder::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
     // blobfinder_blob_t* b =
     //&g_array_index( blobs, blobfinder_blob_t, s);
 
-    bf->PushColor(b->color);
+    canvas->PushColor(b->color);
     glRectf(b->left, b->top, b->right, b->bottom);
 
     // printf( "%u l %u t%u r %u b %u\n", s, b->left, b->top, b->right,
     // b->bottom );
-    bf->PopColor();
+    canvas->PopColor();
   }
 
   glPopMatrix();
diff --git a/libstage/model_draw.cc b/libstage/model_draw.cc
index 1c4b68822..2a56de722 100644
--- a/libstage/model_draw.cc
+++ b/libstage/model_draw.cc
@@ -20,7 +20,7 @@ void Model::DrawSelected(Canvas * canvas)
   char buf[64];
   snprintf(buf, 63, "%s [%.2f %.2f %.2f %.2f]", Token(), gp.x, gp.y, gp.z, rtod(gp.a));
 
-  PushColor(0, 0, 0, 1); // text color black
+  canvas->PushColor(0, 0, 0, 1); // text color black
   canvas->draw_string(0.5, 0.5, 0.5, buf);
 
   glRotatef(rtod(pose.a), 0, 0, 1);
@@ -30,28 +30,28 @@ void Model::DrawSelected(Canvas * canvas)
   double dx = geom.size.x / 2.0 * 1.6;
   double dy = geom.size.y / 2.0 * 1.6;
 
-  PopColor();
+  canvas->PopColor();
 
-  PushColor(0, 1, 0, 0.4); // highlight color blue
+  canvas->PushColor(0, 1, 0, 0.4); // highlight color blue
   glRectf(-dx, -dy, dx, dy);
-  PopColor();
+  canvas->PopColor();
 
-  PushColor(0, 1, 0, 0.8); // highlight color blue
+  canvas->PushColor(0, 1, 0, 0.8); // highlight color blue
   glLineWidth(1);
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
   glRectf(-dx, -dy, dx, dy);
   glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
-  PopColor();
+  canvas->PopColor();
 
   glPopMatrix();
 }
 
-void Model::DrawTrailFootprint()
+void Model::DrawTrailFootprint(Camera *cam, Canvas * canvas)
 {
   double darkness = 0;
   double fade = 0.5 / (double)(trail.size() + 1);
 
-  PushColor(0, 0, 0, 1); // dummy push just saving the color
+  canvas->PushColor(0, 0, 0, 1); // dummy push just saving the color
 
   // this loop could be faster, but optimzing vis is not a priority
   
@@ -80,10 +80,10 @@ void Model::DrawTrailFootprint()
     glPopMatrix();
   }
 
-  PopColor();
+  canvas->PopColor();
 }
 
-void Model::DrawTrailBlocks()
+void Model::DrawTrailBlocks(Camera *cam, Canvas * canvas)
 {
   double timescale = 0.0000001;
 
@@ -97,19 +97,19 @@ void Model::DrawTrailBlocks()
     Gl::pose_shift(pz);
     Gl::pose_shift(geom.pose);
 
-    DrawBlocks();
+    DrawBlocks(canvas);
 
     glPopMatrix();
   }
 }
 
-void Model::DrawTrailArrows()
+void Model::DrawTrailArrows(Canvas* canvas)
 {
   double dx = 0.2;
   double dy = 0.07;
   double timescale = 1e-7;
 
-  PushColor(0, 0, 0, 1); // dummy push
+  canvas->PushColor(0, 0, 0, 1); // dummy push
 
   FOR_EACH (it, trail) {
     TrailItem &checkpoint = *it;
@@ -134,61 +134,49 @@ void Model::DrawTrailArrows()
     glPopMatrix();
   }
 
-  PopColor();
+  canvas->PopColor();
 }
 
-void Model::DrawOriginTree()
+void Model::DrawOriginTree(Canvas* canvas)
 {
-  DrawPose(GetGlobalPose());
+  canvas->DrawPose(GetGlobalPose());
 
   FOR_EACH (it, children)
-    (*it)->DrawOriginTree();
+    (*it)->DrawOriginTree(canvas);
 }
 
-void Model::DrawBlocksTree()
+void Model::DrawBlocksTree(Canvas * canvas)
 {
   PushLocalCoords();
 
   FOR_EACH (it, children)
-    (*it)->DrawBlocksTree();
+    (*it)->DrawBlocksTree(canvas);
 
-  DrawBlocks();
+  DrawBlocks(canvas);
   PopCoords();
 }
 
-void Model::DrawPose(Pose pose)
+void Model::DrawBlocks(Canvas * canvas)
 {
-  PushColor(0, 0, 0, 1);
-  glPointSize(4);
-
-  glBegin(GL_POINTS);
-  glVertex3f(pose.x, pose.y, pose.z);
-  glEnd();
-
-  PopColor();
+  blockgroup.CallDisplayList(canvas);
 }
 
-void Model::DrawBlocks()
-{
-  blockgroup.CallDisplayList();
-}
-
-void Model::DrawBoundingBoxTree()
+void Model::DrawBoundingBoxTree(Canvas * canvas)
 {
   PushLocalCoords();
 
   FOR_EACH (it, children)
-    (*it)->DrawBoundingBoxTree();
+    (*it)->DrawBoundingBoxTree(canvas);
 
-  DrawBoundingBox();
+  DrawBoundingBox(canvas);
   PopCoords();
 }
 
-void Model::DrawBoundingBox()
+void Model::DrawBoundingBox(Canvas * canvas)
 {
   Gl::pose_shift(geom.pose);
 
-  PushColor(color);
+  canvas->PushColor(color);
 
   glBegin(GL_QUAD_STRIP);
 
@@ -220,7 +208,7 @@ void Model::DrawBoundingBox()
   glVertex2f(0, +0.02);
   glEnd();
 
-  PopColor();
+  canvas->PopColor();
 }
 
 // move into this model's local coordinate frame
@@ -274,7 +262,8 @@ void Model::RemoveVisualizer(Visualizer *cv)
   // attached to different models which have the same name
 }
 
-void Model::DrawStatusTree(Camera *cam, Canvas * canvas)
+
+void Model::DrawStatusTree(Camera* cam, Canvas * canvas)
 {
   PushLocalCoords();
   DrawStatus(cam, canvas);
@@ -283,7 +272,7 @@ void Model::DrawStatusTree(Camera *cam, Canvas * canvas)
   PopCoords();
 }
 
-void Model::DrawStatus(Camera *cam, Canvas * canvas)
+void Model::DrawStatus(Camera* cam, Canvas * canvas)
 {
   if (power_pack || !say_string.empty()) {
     float pitch = -cam->pitch();
@@ -340,28 +329,28 @@ void Model::DrawStatus(Camera *cam, Canvas * canvas)
         const float m = h / 10;
 
         // draw inside of bubble
-        PushColor(BUBBLE_FILL);
+        canvas->PushColor(BUBBLE_FILL);
         glPushAttrib(GL_POLYGON_BIT | GL_LINE_BIT);
         glPolygonMode(GL_FRONT, GL_FILL);
         glEnable(GL_POLYGON_OFFSET_FILL);
         glPolygonOffset(1.0, 1.0);
         Gl::draw_octagon(w, h, m);
         glDisable(GL_POLYGON_OFFSET_FILL);
-        PopColor();
+        canvas->PopColor();
 
         // draw outline of bubble
-        PushColor(BUBBLE_BORDER);
+        canvas->PushColor(BUBBLE_BORDER);
         glLineWidth(1);
         glEnable(GL_LINE_SMOOTH);
         glPolygonMode(GL_FRONT, GL_LINE);
         Gl::draw_octagon(w, h, m);
         glPopAttrib();
-        PopColor();
+        canvas->PopColor();
 
-        PushColor(BUBBLE_TEXT);
+        canvas->PushColor(BUBBLE_TEXT);
         // draw text inside the bubble
         canvas->draw_string(m, 2.5 * m, 0, this->say_string.c_str());
-        PopColor();
+        canvas->PopColor();
       }
     }
     glPopMatrix();
@@ -525,7 +514,7 @@ void Model::DrawPicker(void)
   PopCoords();
 }
 
-void Model::DataVisualize(Camera *cam)
+void Model::DataVisualize(Camera *cam, Canvas* canvas)
 {
   (void)cam; // avoid warning about unused var
 }
@@ -537,7 +526,7 @@ void Model::DataVisualizeTree(Camera *cam, Canvas * canvas)
   //Canvas * canvas = GetCanvas();
 
   if (subs > 0 && canvas != NULL) {
-    DataVisualize(cam); // virtual function overridden by some model types
+    DataVisualize(cam, canvas); // virtual function overridden by some model types
 
     FOR_EACH (it, cv_list) {
       Visualizer *vis = *it;
@@ -566,9 +555,9 @@ void Model::DrawGrid(Canvas * canvas)
     vol.z.min = 0;
     vol.z.max = geom.size.z;
 
-    PushColor(0, 0, 1, 0.4);
+    canvas->PushColor(0, 0, 1, 0.4);
     canvas->draw_grid(vol);
-    PopColor();
+    canvas->PopColor();
     PopCoords();
   }
 }
diff --git a/libstage/model_fiducial.cc b/libstage/model_fiducial.cc
index 59bae8bd5..d487e7afb 100644
--- a/libstage/model_fiducial.cc
+++ b/libstage/model_fiducial.cc
@@ -312,7 +312,7 @@ void ModelFiducial::DataVisualize(Camera *cam, Canvas * canvas)
   (void)cam; // avoid warning about unused var
 
   if (showFov) {
-    PushColor(1, 0, 1, 0.2); // magenta, with a bit of alpha
+    canvas->PushColor(1, 0, 1, 0.2); // magenta, with a bit of alpha
 
     GLUquadric *quadric = gluNewQuadric();
 
@@ -326,11 +326,11 @@ void ModelFiducial::DataVisualize(Camera *cam, Canvas * canvas)
 
     gluDeleteQuadric(quadric);
 
-    PopColor();
+    canvas->PopColor();
   }
 
   if (showData) {
-    PushColor(1, 0, 1, 0.4); // magenta, with a bit of alpha
+    canvas->PushColor(1, 0, 1, 0.4); // magenta, with a bit of alpha
 
     // draw fuzzy dotted lines
     glLineWidth(2.0);
@@ -365,7 +365,7 @@ void ModelFiducial::DataVisualize(Camera *cam, Canvas * canvas)
       glPopMatrix();
     }
 
-    PopColor();
+    canvas->PopColor();
     glLineWidth(1.0);
   }
 }
diff --git a/libstage/model_gripper.cc b/libstage/model_gripper.cc
index 2ab660665..a6132b3f9 100644
--- a/libstage/model_gripper.cc
+++ b/libstage/model_gripper.cc
@@ -52,6 +52,7 @@
 
 #include "stage.hh"
 #include "worldfile.hh"
+#include "canvas.hh"
 #include <sys/time.h>
 using namespace Stg;
 
@@ -212,6 +213,8 @@ void ModelGripper::PositionPaddles()
 
   paddle_left->SetZ(paddle_bottom, paddle_top);
   paddle_right->SetZ(paddle_bottom, paddle_top);
+  // TODO: update display list for child blocks
+  rebuild_displaylist = true;
 
   Map(layer);
 }
@@ -443,7 +446,7 @@ void ModelGripper::UpdateContacts()
   }
 }
 
-void ModelGripper::DataVisualize(Camera *cam)
+void ModelGripper::DataVisualize(Camera *cam, Canvas* canvas)
 {
   (void)cam; // avoid warning about unused var
 
@@ -455,7 +458,7 @@ void ModelGripper::DataVisualize(Camera *cam)
   // return;
 
   // outline the sensor lights in black
-  PushColor(0, 0, 0, 1.0); // black
+  canvas->PushColor(0, 0, 0, 1.0); // black
   glTranslatef(0, 0, geom.size.z * cfg.paddle_size.z);
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
 
@@ -487,7 +490,7 @@ void ModelGripper::DataVisualize(Camera *cam)
 
   // if the gripper detects anything, fill the lights in with yellow
   if (cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1]) {
-    PushColor(1, 1, 0, 1.0); // yellow
+    canvas->PushColor(1, 1, 0, 1.0); // yellow
     glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 
     if (cfg.contact[0])
@@ -506,8 +509,8 @@ void ModelGripper::DataVisualize(Camera *cam)
       Gl::draw_centered_rect(obbx, -bby - led_dx, led_dx, led_dx);
     }
 
-    PopColor(); // yellow
+    canvas->PopColor(); // yellow
   }
 
-  PopColor(); // black
+  canvas->PopColor(); // black
 }
diff --git a/libstage/model_lightindicator.cc b/libstage/model_lightindicator.cc
index 61316b135..1b6ef66ea 100644
--- a/libstage/model_lightindicator.cc
+++ b/libstage/model_lightindicator.cc
@@ -16,10 +16,10 @@ void ModelLightIndicator::SetState(bool isOn)
   m_IsOn = isOn;
 }
 
-void ModelLightIndicator::DrawBlocks()
+void ModelLightIndicator::DrawBlocks(Canvas * canvas)
 {
   if (m_IsOn) {
-    Model::DrawBlocks();
+    Model::DrawBlocks(canvas);
   } else {
     const double scaleFactor = 0.8;
 
@@ -30,7 +30,7 @@ void ModelLightIndicator::DrawBlocks()
     c.b *= scaleFactor;
 
     this->SetColor(c);
-    Model::DrawBlocks();
+    Model::DrawBlocks(canvas);
 
     this->SetColor(keep);
   }
diff --git a/libstage/model_position.cc b/libstage/model_position.cc
index b08f6f0ac..4c06cc532 100644
--- a/libstage/model_position.cc
+++ b/libstage/model_position.cc
@@ -695,13 +695,13 @@ void ModelPosition::PoseVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
   Gl::pose_inverse_shift(pos->GetGlobalPose());
 
   Gl::pose_shift(pos->est_origin);
-  pos->PushColor(1, 0, 0, 1); // origin in red
+  canvas->PushColor(1, 0, 0, 1); // origin in red
   Gl::draw_origin(0.5);
 
   glEnable(GL_LINE_STIPPLE);
   glLineStipple(3, 0xAAAA);
 
-  pos->PushColor(1, 0, 0, 0.5);
+  canvas->PushColor(1, 0, 0, 0.5);
   glBegin(GL_LINE_STRIP);
   glVertex2f(0, 0);
   glVertex2f(pos->est_pose.x, 0);
@@ -717,25 +717,25 @@ void ModelPosition::PoseVis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
   snprintf(label, 64, "y:%.3f", pos->est_pose.y);
   canvas->draw_string(pos->est_pose.x + 0.5, pos->est_pose.y / 2.0, 0, (const char *)label);
 
-  pos->PopColor();
+  canvas->PopColor();
 
   Gl::pose_shift(pos->est_pose);
-  pos->PushColor(0, 1, 0, 1); // pose in green
+  canvas->PushColor(0, 1, 0, 1); // pose in green
   Gl::draw_origin(0.5);
-  pos->PopColor();
+  canvas->PopColor();
 
   Gl::pose_shift(pos->geom.pose);
-  pos->PushColor(0, 0, 1, 1); // offset in blue
+  canvas->PushColor(0, 0, 1, 1); // offset in blue
   Gl::draw_origin(0.5);
-  pos->PopColor();
+  canvas->PopColor();
 
   Color c = pos->color;
   c.a = 0.5;
-  pos->PushColor(c);
+  canvas->PushColor(c);
 
   glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
   pos->blockgroup.DrawFootPrint(pos->geom);
-  pos->PopColor();
+  canvas->PopColor();
 
   glPopMatrix();
 }
@@ -757,7 +757,7 @@ void ModelPosition::WaypointVis::Visualize(Model *mod, Camera *cam, Canvas * can
 
   glPointSize(5);
   glPushMatrix();
-  pos->PushColor(pos->color);
+  canvas->PushColor(pos->color);
 
   Gl::pose_inverse_shift(pos->pose);
   Gl::pose_shift(pos->est_origin);
@@ -773,7 +773,7 @@ void ModelPosition::WaypointVis::Visualize(Model *mod, Camera *cam, Canvas * can
   // draw lines connecting the waypoints
   const size_t num(waypoints.size());
   if (num > 1) {
-    pos->PushColor(1, 0, 0, 0.3);
+    canvas->PushColor(1, 0, 0, 0.3);
     glBegin(GL_LINES);
 
     for (size_t i(1); i < num; i++) {
@@ -786,10 +786,10 @@ void ModelPosition::WaypointVis::Visualize(Model *mod, Camera *cam, Canvas * can
 
     glEnd();
 
-    pos->PopColor();
+    canvas->PopColor();
   }
 
-  pos->PopColor();
+  canvas->PopColor();
   glPopMatrix();
 }
 
diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc
index 398e8d6ce..a090410a4 100644
--- a/libstage/model_ranger.cc
+++ b/libstage/model_ranger.cc
@@ -14,69 +14,68 @@
 ///////////////////////////////////////////////////////////////////////////
 
 /**
-   @ingroup model
-   @defgroup model_ranger Ranger model
-   The ranger model simulates an array of sonar or infra-red (IR) range sensors.
-
-   API: Stg::ModelRanger
-
-   <h2>Worldfile properties</h2>
-
-   @par Summary and default values
-
-   @verbatim
-   ranger
-   (
-   # ranger-specific properties
-
-   sensor (
-   pose [ x y z a ]
-   size [  x y z ]
-   fov a
-   range [min max]
-   noise [range_const range_prop angular]
-   )
-
-   # generic model properties with non-default values
-   watts 2.0
-   color_rgba [ 0 1 0 0.15 ]
-   )
-
-   @endverbatim
-
-   @par Notes
-
-   The ranger model allows configuration of the pose, size and view parameters
-   of each transducer seperately (using spose[index], ssize[index] and
-   sview[index]). However, most users will set a common size and view (using
-   ssize and sview), and just specify individual transducer poses.
-
-   @par Details
-
-   - pose[\<transducer index\>] [ x:<float> y:<float> theta:<float> ]\n
-   pose of the transducer relative to its parent.
-   - ssize [float float]
-   - [x y]
-   - size in meters. Has no effect on the data, but controls how the sensor
-   looks in the Stage window.
-   - size[\<transducer index\>] [float float]
-   -  per-transducer version of the ssize property. Overrides the common
-   setting.
-   - sview [float float float]
-   - [range_min range_max fov]
-   - minimum range and maximum range in meters, field of view angle in degrees.
-   Currently fov has no effect on the sensor model, other than being shown in
-   the confgiuration graphic for the ranger device.
-   - noise [range_const range_prop angular]
-   - noise for range readings: constant noise in meters, proportional noise,
-   angular noise in degrees
-   - sview[\<transducer index\>] [float float float]
-   - per-transducer version of the sview property. Overrides the common setting.
-
-*/
+ @ingroup model
+ @defgroup model_ranger Ranger model
+ The ranger model simulates an array of sonar or infra-red (IR) range sensors.
+
+ API: Stg::ModelRanger
+
+ <h2>Worldfile properties</h2>
+
+ @par Summary and default values
+
+ @verbatim
+ ranger
+ (
+ # ranger-specific properties
+
+ sensor (
+ pose [ x y z a ]
+ size [  x y z ]
+ fov a
+ range [min max]
+ noise [range_const range_prop angular]
+ )
+
+ # generic model properties with non-default values
+ watts 2.0
+ color_rgba [ 0 1 0 0.15 ]
+ )
+
+ @endverbatim
+
+ @par Notes
+
+ The ranger model allows configuration of the pose, size and view parameters
+ of each transducer seperately (using spose[index], ssize[index] and
+ sview[index]). However, most users will set a common size and view (using
+ ssize and sview), and just specify individual transducer poses.
+
+ @par Details
+
+ - pose[\<transducer index\>] [ x:<float> y:<float> theta:<float> ]\n
+ pose of the transducer relative to its parent.
+ - ssize [float float]
+ - [x y]
+ - size in meters. Has no effect on the data, but controls how the sensor
+ looks in the Stage window.
+ - size[\<transducer index\>] [float float]
+ -  per-transducer version of the ssize property. Overrides the common
+ setting.
+ - sview [float float float]
+ - [range_min range_max fov]
+ - minimum range and maximum range in meters, field of view angle in degrees.
+ Currently fov has no effect on the sensor model, other than being shown in
+ the confgiuration graphic for the ranger device.
+ - noise [range_const range_prop angular]
+ - noise for range readings: constant noise in meters, proportional noise,
+ angular noise in degrees
+ - sview[\<transducer index\>] [float float float]
+ - per-transducer version of the sview property. Overrides the common setting.
+
+ */
 
 //#define DEBUG 1
-
 #include "option.hh"
 #include "stage.hh"
 #include "worldfile.hh"
@@ -92,29 +91,32 @@ static const Color RANGER_CONFIG_COLOR(0, 0, 0.5);
 // static const Color RANGER_GEOM_COLOR( 1,0,1 );
 
 // static members
-Option ModelRanger::Vis::showTransducers("Ranger transducers", "show_ranger_transducers", "", false);
-Option ModelRanger::Vis::showArea("Ranger area", "show_ranger_ranges", "", true);
-Option ModelRanger::Vis::showStrikes("Ranger strikes", "show_ranger_strikes", "", false);
+Option ModelRanger::Vis::showTransducers("Ranger transducers",
+		"show_ranger_transducers", "", false);
+Option ModelRanger::Vis::showArea("Ranger area", "show_ranger_ranges", "",
+		true);
+Option ModelRanger::Vis::showStrikes("Ranger strikes", "show_ranger_strikes",
+		"", false);
 Option ModelRanger::Vis::showFov("Ranger FOV", "show_ranger_fov", "", false);
 // Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "",
 // false, NULL );
 
-ModelRanger::ModelRanger(World *world, Model *parent, const std::string &type)
-    : Model(world, parent, type), vis(world)
+ModelRanger::ModelRanger(World *world, Model *parent, const std::string &type) :
+		Model(world, parent, type), vis(world)
 {
- PRINT_DEBUG2("Constructing ModelRanger %u (%s)\n", id, type.c_str());
+	PRINT_DEBUG2("Constructing ModelRanger %u (%s)\n", id, type.c_str());
 
- // assert that Update() is reentrant for this derived model
- thread_safe = true;
+	// assert that Update() is reentrant for this derived model
+	thread_safe = true;
 
-  this->SetColor(RANGER_CONFIG_COLOR);
+	this->SetColor(RANGER_CONFIG_COLOR);
 
-  // remove the polygon: ranger has no body
-  this->ClearBlocks();
+	// remove the polygon: ranger has no body
+	this->ClearBlocks();
 
-  this->SetGeom(Geom(Pose(), RANGER_SIZE));
+	this->SetGeom(Geom(Pose(), RANGER_SIZE));
 
-  AddVisualizer(&vis, true);
+	AddVisualizer(&vis, true);
 }
 
 ModelRanger::~ModelRanger()
@@ -123,328 +125,352 @@ ModelRanger::~ModelRanger()
 
 void ModelRanger::Startup(void)
 {
-  Model::Startup();
-  this->SetWatts(RANGER_WATTSPERSENSOR * sensors.size());
+	Model::Startup();
+	this->SetWatts(RANGER_WATTSPERSENSOR * sensors.size());
 }
 
 void ModelRanger::Shutdown(void)
 {
-  PRINT_DEBUG("ranger shutdown");
+	PRINT_DEBUG("ranger shutdown");
 
-  this->SetWatts(0);
+	this->SetWatts(0);
 
-  Model::Shutdown();
+	Model::Shutdown();
 }
 
 void ModelRanger::LoadSensor(Worldfile *wf, int entity)
 {
-  Sensor s;
-  s.Load(wf, entity);
-  sensors.push_back(s);
+	Sensor s;
+	s.Load(wf, entity);
+	sensors.push_back(s);
 }
 
 void ModelRanger::Sensor::Load(Worldfile *wf, int entity)
 {
-  pose.Load(wf, entity, "pose");
-  size.Load(wf, entity, "size");
-  range.Load(wf, entity, "range");
-  fov = wf->ReadAngle(entity, "fov", fov);
-  sample_count = wf->ReadInt(entity, "samples", sample_count);
-
-  wf->ReadTuple(entity, "noise", 0, 3, "lfa", &range_noise_const, &range_noise, &angle_noise);
-  color.Load(wf, entity);
+	pose.Load(wf, entity, "pose");
+	size.Load(wf, entity, "size");
+	range.Load(wf, entity, "range");
+	fov = wf->ReadAngle(entity, "fov", fov);
+	sample_count = wf->ReadInt(entity, "samples", sample_count);
+
+	wf->ReadTuple(entity, "noise", 0, 3, "lfa", &range_noise_const, &range_noise,
+			&angle_noise);
+	color.Load(wf, entity);
 }
 
 static bool ranger_match(Model *hit, const Model *finder, const void *dummy)
 {
-  (void)dummy; // avoid warning about unused var
+	(void) dummy; // avoid warning about unused var
 
-  // Ignore the model that's looking and things that are invisible to
-  // rangers
+	// Ignore the model that's looking and things that are invisible to
+	// rangers
 
-  // small optimization to avoid recursive Model::IsRelated call in common cases
-  if ((hit == finder->Parent()) || (hit == finder))
-    return false;
+	// small optimization to avoid recursive Model::IsRelated call in common cases
+	if ((hit == finder->Parent()) || (hit == finder))
+		return false;
 
-  return ((!hit->IsRelated(finder)) && (sgn(hit->vis.ranger_return) != -1));
+	return ((!hit->IsRelated(finder)) && (sgn(hit->vis.ranger_return) != -1));
 }
 
 // Returns random numbers in range [-1.0, 1.0)
 double simpleNoise()
 {
-  return 2 * (rand() % 1000 * 0.001 - 0.5);
+	return 2 * (rand() % 1000 * 0.001 - 0.5);
 }
 
 #define TWO_PI (M_PI + M_PI)
 // Returns gaussian noise
 // taken from http://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
-template <class Scalar_t> Scalar_t generateGaussianNoise(Scalar_t variance)
+template<class Scalar_t> Scalar_t generateGaussianNoise(Scalar_t variance)
 {
-  static bool haveSpare = false;
-  static Scalar_t rand1, rand2;
+	static bool haveSpare = false;
+	static Scalar_t rand1, rand2;
 
-  if (haveSpare) {
-    haveSpare = false;
-    return sqrt(variance * rand1) * sin(rand2);
-  }
+	if (haveSpare)
+	{
+		haveSpare = false;
+		return sqrt(variance * rand1) * sin(rand2);
+	}
 
-  haveSpare = true;
+	haveSpare = true;
 
-  rand1 = rand() / ((Scalar_t)RAND_MAX);
-  if (rand1 < 1e-100)
-    rand1 = 1e-100;
-  rand1 = -2 * log(rand1);
-  rand2 = (rand() / ((Scalar_t)RAND_MAX)) * TWO_PI;
+	rand1 = rand() / ((Scalar_t) RAND_MAX);
+	if (rand1 < 1e-100)
+		rand1 = 1e-100;
+	rand1 = -2 * log(rand1);
+	rand2 = (rand() / ((Scalar_t) RAND_MAX)) * TWO_PI;
 
-  return sqrt(variance * rand1) * cos(rand2);
+	return sqrt(variance * rand1) * cos(rand2);
 }
 
 void ModelRanger::Update(void)
 {
-  // raytrace new range data for all sensors
-  FOR_EACH (it, sensors)
-    it->Update(this);
+	// raytrace new range data for all sensors
+	FOR_EACH (it, sensors)
+		it->Update(this);
 
-  Model::Update();
+	Model::Update();
 }
 
 void ModelRanger::Sensor::Update(ModelRanger *mod)
 {
-  // these sizes change very rarely, so this is very cheap
-  ranges.resize(sample_count);
-  intensities.resize(sample_count);
-  bearings.resize(sample_count);
-
-  // printf( "update sensor, has ranges size %u\n", (unsigned int)ranges.size()
-  // );
-  // make the first and last rays exactly at the extremes of the FOV
-  const double sample_incr(fov / std::max(sample_count - 1, (unsigned int)1));
-
-  // find the global origin of our first emmitted ray
-  const double start_angle = (sample_count > 1 ? -fov / 2.0 : 0.0);
-
-  // find the origin and heading of the first ray
-  Pose rayorg(pose);
-  rayorg.a += start_angle;
-  rayorg.z += size.z / 2.0;
-  rayorg = mod->LocalToGlobal(rayorg);
-
-  // set up a ray to trace
-  Ray ray(mod, rayorg, range.max, ranger_match, NULL, true);
-
-  // trace the ray, incrementing its heading for each sample
-  for (size_t t(0); t < sample_count; t++) {
-    float savedAngle = ray.origin.a;
-    float distortedAngle = ray.origin.a + sample_incr * angle_noise * simpleNoise() * 0.5;
-    ray.origin.a = distortedAngle;
-    const RaytraceResult res = mod->world->Raytrace(ray);
-    ray.origin.a = savedAngle;
-
-    /// Apply noise only if it is in valid range
-    if (res.range < this->range.max)
-      ranges[t] = res.range + res.range * range_noise * simpleNoise()
-                  + generateGaussianNoise(range_noise_const);
-    else
-      ranges[t] = res.range;
-
-    intensities[t] = res.mod ? res.mod->vis.ranger_return : 0.0;
-    bearings[t] = start_angle + ((double)t) * sample_incr;
-
-    // point the ray to the next angle:
-    ray.origin.a += sample_incr;
-  }
+	// these sizes change very rarely, so this is very cheap
+	ranges.resize(sample_count);
+	intensities.resize(sample_count);
+	bearings.resize(sample_count);
+
+	// printf( "update sensor, has ranges size %u\n", (unsigned int)ranges.size()
+	// );
+	// make the first and last rays exactly at the extremes of the FOV
+	const double sample_incr(fov / std::max(sample_count - 1, (unsigned int) 1));
+
+	// find the global origin of our first emmitted ray
+	const double start_angle = (sample_count > 1 ? -fov / 2.0 : 0.0);
+
+	// find the origin and heading of the first ray
+	Pose rayorg(pose);
+	rayorg.a += start_angle;
+	rayorg.z += size.z / 2.0;
+	rayorg = mod->LocalToGlobal(rayorg);
+
+	// set up a ray to trace
+	Ray ray(mod, rayorg, range.max, ranger_match, NULL, true);
+
+	// trace the ray, incrementing its heading for each sample
+	for (size_t t(0); t < sample_count; t++)
+	{
+		float savedAngle = ray.origin.a;
+		float distortedAngle = ray.origin.a
+				+ sample_incr * angle_noise * simpleNoise() * 0.5;
+		ray.origin.a = distortedAngle;
+		const RaytraceResult res = mod->world->Raytrace(ray);
+		ray.origin.a = savedAngle;
+
+		/// Apply noise only if it is in valid range
+		if (res.range < this->range.max)
+			ranges[t] = res.range + res.range * range_noise * simpleNoise()
+					+ generateGaussianNoise(range_noise_const);
+		else
+			ranges[t] = res.range;
+
+		intensities[t] = res.mod ? res.mod->vis.ranger_return : 0.0;
+		bearings[t] = start_angle + ((double) t) * sample_incr;
+
+		// point the ray to the next angle:
+		ray.origin.a += sample_incr;
+	}
 }
 
 std::string ModelRanger::Sensor::String() const
 {
-  char buf[256];
-  snprintf(buf, 256, "[ samples %u, range [%.2f %.2f] fov %.2f color [%.2f %.2f %.2f %.2f]",
-           sample_count, range.min, range.max, fov, color.r, color.g, color.b, color.a);
-  return (std::string(buf));
+	char buf[256];
+	snprintf(buf, 256,
+			"[ samples %u, range [%.2f %.2f] fov %.2f color [%.2f %.2f %.2f %.2f]",
+			sample_count, range.min, range.max, fov, color.r, color.g, color.b,
+			color.a);
+	return (std::string(buf));
 }
 
-typedef struct { GLfloat x; GLfloat y; } glpoint_t;
-  
-void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr) const
+typedef struct
+{
+	GLfloat x;
+	GLfloat y;
+} glpoint_t;
+
+void ModelRanger::Sensor::Visualize(ModelRanger::Vis *vis, ModelRanger *rgr,
+		Canvas * canvas) const
 {
-  // glTranslatef( 0,0, ranger->GetGeom().size.z/2.0 ); // shoot the ranger beam
-  // out at the right height
-
-  glPushMatrix();
-
-  Gl::pose_shift(pose);
-
-  const double sample_fov = fov / sample_count;
-  
-  if (vis->showTransducers) {
-    rgr->PushColor(color);    
-    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
-    glRectf(-size.x / 2.0, -size.y / 2.0, size.x / 2.0, size.y / 2.0);
-    rgr->PopColor();
-  }
-  
-  if (vis->showFov)
-    {
-      if (sample_count == 1)
+	// glTranslatef( 0,0, ranger->GetGeom().size.z/2.0 ); // shoot the ranger beam
+	// out at the right height
+
+	glPushMatrix();
+
+	Gl::pose_shift(pose);
+
+	const double sample_fov = fov / sample_count;
+
+	if (vis->showTransducers)
+	{
+		canvas->PushColor(color);
+		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+		glRectf(-size.x / 2.0, -size.y / 2.0, size.x / 2.0, size.y / 2.0);
+		canvas->PopColor();
+	}
+
+	if (vis->showFov)
+	{
+		if (sample_count == 1)
+		{
+			canvas->PushColor(color);
+			glBegin (GL_LINES);
+			glVertex2f(0, 0);
+			glVertex2f(range.max, 0);
+			glEnd();
+			canvas->PopColor();
+		}
+		else
+		{
+			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+			Color c = color;
+			c.a = 0.5;
+			canvas->PushColor(c);
+			glBegin (GL_POLYGON);
+
+			glVertex2f(0, 0);
+
+			for (size_t s(0); s < sample_count; s++)
+			{
+				const double ray_angle = (((double) s) - 0.5) * sample_fov - fov / 2.0;
+				const GLfloat x = range.max * cos(ray_angle);
+				const GLfloat y = range.max * sin(ray_angle);
+
+				glVertex2f(x, y);
+			}
+
+			glVertex2f(0, 0);
+
+			glEnd();
+
+			canvas->PopColor();
+		}
+	}
+
+	std::vector<glpoint_t> verts(sample_count);
+
+	if (sample_count == 1 && !ranges.empty())
+	{
+		// only one sample, so we fake up some beam width for beauty
+		const double sidelen = ranges[0];
+		const double da = fov / 2.0;
+
+		verts.resize(3);
+		verts[0].x = 0;
+		verts[0].y = 0;
+		verts[1].x = sidelen * cos(-da);
+		verts[1].y = sidelen * sin(-da);
+		verts[2].x = sidelen * cos(+da);
+		verts[2].y = sidelen * sin(+da);
+	}
+	else if (sample_count > 0 && !ranges.empty())
+	{
+		for (size_t s(0); s < sample_count; s++)
+		{
+			const double ray_angle = (((double) s) - 0.5) * sample_fov - fov / 2.0;
+			verts[s].x = (float) (ranges[s] * cos(ray_angle));
+			verts[s].y = (float) (ranges[s] * sin(ray_angle));
+		}
+	}
+
+	if (vis->showArea && !verts.empty())
 	{
-	  rgr->PushColor(color);
-	  glBegin( GL_LINES );
-	  glVertex2f( 0,0 );
-	  glVertex2f( range.max, 0 );
-	  glEnd();
-	  rgr->PopColor();
+		// draw the filled polygon in transparent blue
+		glEnable (GL_BLEND);
+		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+		glDepthMask (GL_FALSE);
+		glPolygonMode(GL_FRONT, GL_FILL);
+		Color c = color;
+		c.a = 0.1; // some alpha
+		canvas->PushColor(c);
+
+		glBegin (GL_POLYGON);
+		glVertex2f(0, 0);
+
+		FOR_EACH( it, verts )
+			glVertex2f(it->x, it->y);
+
+		glVertex2f(0, 0);
+		glEnd();
+
+		canvas->PopColor();
+		glDepthMask (GL_TRUE);
 	}
-      else
-	{    
-	  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
-	  Color c = color;
-	  c.a = 0.5;
-	  rgr->PushColor(c);
-	  glBegin( GL_POLYGON );
-	  
-	  glVertex2f( 0,0 );
-	  
-	  for (size_t s(0); s < sample_count; s++) {
-	    const double ray_angle = (((double)s)-0.5) * sample_fov - fov / 2.0;      
-	    const GLfloat x = range.max * cos(ray_angle);
-	    const GLfloat y = range.max * sin(ray_angle);
-	    
-	    glVertex2f( x, y );
-	  }
-	  
-	  glVertex2f( 0,0 );
-	  
-	  glEnd();
-	  
-	  rgr->PopColor();
+
+	if (vis->showStrikes && !verts.empty())
+	{
+		canvas->PushColor(Color::blue); // solid color
+		glPointSize(2);
+
+		glBegin (GL_POINTS);
+		FOR_EACH( it, verts )
+			glVertex2f(it->x, it->y);
+		glEnd();
+
+		canvas->PopColor();
 	}
-    }
-  
-  std::vector<glpoint_t> verts( sample_count );
-  
-  if (sample_count == 1 && !ranges.empty()) {
-    // only one sample, so we fake up some beam width for beauty
-    const double sidelen = ranges[0];
-    const double da = fov / 2.0;
-    
-    verts.resize(3);
-    verts[0].x = 0;
-    verts[0].y = 0;
-    verts[1].x = sidelen * cos(-da);
-    verts[1].y = sidelen * sin(-da);
-    verts[2].x = sidelen * cos(+da);
-    verts[2].y = sidelen * sin(+da);
-  } else if (sample_count > 0 && !ranges.empty()) {
-    for (size_t s(0); s < sample_count; s++) {
-      const double ray_angle = (((double)s)-0.5) * sample_fov - fov / 2.0;
-      verts[s].x = (float)(ranges[s] * cos(ray_angle));
-      verts[s].y = (float)(ranges[s] * sin(ray_angle));
-    }
-  }
-  
-  if (vis->showArea && !verts.empty()) {
-    // draw the filled polygon in transparent blue
-    glEnable( GL_BLEND );
-    glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
-    
-    glDepthMask(GL_FALSE);
-    glPolygonMode(GL_FRONT, GL_FILL);
-    Color c = color;
-    c.a = 0.1; // some alpha
-    rgr->PushColor(c);
-    
-    glBegin( GL_POLYGON );
-    glVertex2f( 0,0 );
-    
-    FOR_EACH( it, verts )
-      glVertex2f( it->x, it->y );
-    
-    glVertex2f( 0,0 );    
-    glEnd();	
-    
-    rgr->PopColor();
-    glDepthMask(GL_TRUE);
-  }
-      
-  if (vis->showStrikes && !verts.empty()) {
-    rgr->PushColor(Color::blue); // solid color
-    glPointSize(2);
-
-    glBegin( GL_POINTS );
-    FOR_EACH( it, verts )
-      glVertex2f( it->x, it->y );
-    glEnd();
-      
-    rgr->PopColor();
-  }
- 
-  glPopMatrix();
+
+	glPopMatrix();
 }
 
 void ModelRanger::Print(char *prefix) const
 {
-  Model::Print(prefix);
-
-  printf("\tRanges ");
-  for (size_t i(0); i < sensors.size(); i++) {
-    printf("[ ");
-    for (size_t j(0); j < sensors[i].ranges.size(); j++)
-      printf("%.2f ", sensors[i].ranges[j]);
-
-    printf(" ]");
-  }
-
-  printf("\n\tIntensities ");
-  for (size_t i(0); i < sensors.size(); i++) {
-    printf("[ ");
-    for (size_t j(0); j < sensors[i].intensities.size(); j++)
-      printf("%.2f ", sensors[i].intensities[j]);
-
-    printf(" ]");
-  }
-  puts("");
+	Model::Print(prefix);
+
+	printf("\tRanges ");
+	for (size_t i(0); i < sensors.size(); i++)
+	{
+		printf("[ ");
+		for (size_t j(0); j < sensors[i].ranges.size(); j++)
+			printf("%.2f ", sensors[i].ranges[j]);
+
+		printf(" ]");
+	}
+
+	printf("\n\tIntensities ");
+	for (size_t i(0); i < sensors.size(); i++)
+	{
+		printf("[ ");
+		for (size_t j(0); j < sensors[i].intensities.size(); j++)
+			printf("%.2f ", sensors[i].intensities[j]);
+
+		printf(" ]");
+	}
+	puts("");
 }
 
 // VIS -------------------------------------------------------------------
 
-ModelRanger::Vis::Vis(World *world) : Visualizer("Ranger", "ranger_vis")
+ModelRanger::Vis::Vis(World *world) :
+		Visualizer("Ranger", "ranger_vis")
 {
-  world->RegisterOption(&showArea);
-  world->RegisterOption(&showStrikes);
-  world->RegisterOption(&showFov);
-  // world->RegisterOption( &showBeams );
-  world->RegisterOption(&showTransducers);
+	world->RegisterOption(&showArea);
+	world->RegisterOption(&showStrikes);
+	world->RegisterOption(&showFov);
+	// world->RegisterOption( &showBeams );
+	world->RegisterOption(&showTransducers);
 }
 
 void ModelRanger::Vis::Visualize(Model *mod, Camera *cam, Canvas * canvas)
 {
-  (void)cam; // avoid warning about unused var
+	(void) cam; // avoid warning about unused var
 
-  ModelRanger *ranger(dynamic_cast<ModelRanger *>(mod));
+	ModelRanger *ranger(dynamic_cast<ModelRanger *>(mod));
 
-  const std::vector<Sensor> &sensors(ranger->GetSensors());
+	const std::vector<Sensor> &sensors(ranger->GetSensors());
 
-  FOR_EACH (it, sensors)
-    it->Visualize(this, ranger);
+	FOR_EACH (it, sensors)
+		it->Visualize(this, ranger, canvas);
 
-  const size_t sensor_count = sensors.size();
+	const size_t sensor_count = sensors.size();
 
-  if (showTransducers) {
-    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
-    ranger->PushColor(0, 0, 0, 1);
-
-    for (size_t s(0); s < sensor_count; s++) {
-      const Sensor &rngr(sensors[s]);
-
-      glPointSize(4);
-      glBegin(GL_POINTS);
-      glVertex3f(rngr.pose.x, rngr.pose.y, rngr.pose.z);
-      glEnd();
-
-      char buf[8];
-      snprintf(buf, 8, "%d", (int)s);
-      canvas->draw_string(rngr.pose.x, rngr.pose.y, rngr.pose.z, buf);
-    }
-    ranger->PopColor();
-  }
+	if (showTransducers)
+	{
+		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+		canvas->PushColor(0, 0, 0, 1);
+
+		for (size_t s(0); s < sensor_count; s++)
+		{
+			const Sensor &rngr(sensors[s]);
+
+			glPointSize(4);
+			glBegin (GL_POINTS);
+			glVertex3f(rngr.pose.x, rngr.pose.y, rngr.pose.z);
+			glEnd();
+
+			char buf[8];
+			snprintf(buf, 8, "%d", (int) s);
+			canvas->draw_string(rngr.pose.x, rngr.pose.y, rngr.pose.z, buf);
+		}
+		canvas->PopColor();
+	}
 }
diff --git a/libstage/stage.hh b/libstage/stage.hh
index b5a71b188..a6aaac00f 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -1,4 +1,3 @@
-
 #ifndef STG_H
 #define STG_H
 /*
@@ -354,7 +353,7 @@ prefix string
 
 @param prefix Character string to prepend to output, or NULL.
   */
-  virtual void Print(const char *prefix) const
+  virtual void Print(const char *prefix) const override
   {
     if (prefix)
       printf("%s", prefix);
@@ -656,7 +655,7 @@ protected:
   /** array contains the number of each type of child model */
   std::map<std::string, unsigned int> child_type_counts;
 
-  std::vector<Model *> children;
+  std::vector<Model*> children;
 
   bool debug;
 
@@ -922,18 +921,6 @@ grid pixels */
     return point_int_t(MetersToPixels(pt.x), MetersToPixels(pt.y));
   }
 
-  /// dummy implementations to be overloaded by GUI subclasses
-  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);
 
@@ -1255,10 +1242,10 @@ as blocks to this group.*/
   /** Re-create the display list for drawing this blockgroup. This
 is required whenever a member block or the owning model
 changes its appearance.*/
-  void BuildDisplayList();
+  void BuildDisplayList(Canvas * canvas);
 
   /** Draw the blockgroup from the cached displaylist. */
-  void CallDisplayList();
+  void CallDisplayList(Canvas * canvas);
 
 public:
   explicit BlockGroup(Model &mod);
@@ -1292,7 +1279,9 @@ public:
   double y(void) const { return _y; }
   double z(void) const { return _z; }
   virtual void reset() = 0;
+
   virtual void Load(Worldfile *wf, int sec) = 0;
+  virtual void Save(Worldfile *wf, int sec){};
 
   //Canvas * GetCanvas();
 
@@ -1312,8 +1301,8 @@ private:
 public:
   PerspectiveCamera(void);
 
-  virtual void Draw(void) const;
-  virtual void SetProjection(void) const;
+  virtual void Draw(void) const override;
+  virtual void SetProjection(void) const override;
   // void SetProjection( double aspect ) const;
   void update(void);
 
@@ -1369,14 +1358,14 @@ public:
     _z_near = near;
   }
 
-  void reset()
+  void reset() override
   {
     setPitch(70);
     setYaw(0);
   }
 
-  void Load(Worldfile *wf, int sec);
-  void Save(Worldfile *wf, int sec);
+  void Load(Worldfile *wf, int sec) override;
+  void Save(Worldfile *wf, int sec) override;
 };
 
 class OrthoCamera : public Camera {
@@ -1389,11 +1378,11 @@ private:
 
 public:
   OrthoCamera(void) : _scale(15), _pixels_width(0), _pixels_height(0), _y_min(0), _y_max(0) {}
-  virtual void Draw() const;
+  virtual void Draw() const override;
 
   virtual void SetProjection(double pixels_width, double pixels_height, double y_min, double y_max);
 
-  virtual void SetProjection(void) const;
+  virtual void SetProjection(void) const override;
 
   void move(double x, double y);
 
@@ -1417,10 +1406,11 @@ public:
   }
 
   void scale(double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0);
-  void reset(void) { _pitch = _yaw = 0; }
+  void reset(void) override { _pitch = _yaw = 0; }
   double scale() const { return _scale; }
-  void Load(Worldfile *wf, int sec);
-  void Save(Worldfile *wf, int sec);
+
+  void Load(Worldfile *wf, int sec) override;
+  void Save(Worldfile *wf, int sec) override;
 };
 
 class StripPlotVis : public Visualizer {
@@ -1437,7 +1427,8 @@ 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, Canvas * canvas);
+
+  void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   void AppendValue(float value);
 };
 
@@ -1462,8 +1453,8 @@ protected:
     DissipationVis(meters_t width, meters_t height, meters_t cellsize);
 
     virtual ~DissipationVis();
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
 
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
     void Accumulate(meters_t x, meters_t y, joules_t amount);
   } event_vis;
 
@@ -1720,8 +1711,7 @@ initially NULL. */
   public:
     RasterVis();
     virtual ~RasterVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
-
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
     void SetData(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                  meters_t cellheight);
 
@@ -1798,7 +1788,7 @@ watts_give >0 */
   //WorldGui *world_gui; //!< Pointer to the GUI world - NULL if running in non-gui mode
 
 public:
-  virtual void SetToken(const std::string &str)
+  virtual void SetToken(const std::string &str) override
   {
     // printf( "Model::SetToken( %s )\n", str.c_str() );
 
@@ -1836,8 +1826,10 @@ it doesn't exist in this model. */
   usec_t GetEnergyInterval() const { return interval_energy; }
   //    usec_t GetPoseInterval() const { return interval_pose; }
 
-  /** Render the model's blocks as an occupancy grid into the
-preallocated array of width by height pixels */
+  /**
+   * Render the model's blocks as an occupancy grid into the
+   * preallocated array of width by height pixels
+   */
   void Rasterize(uint8_t *data, unsigned int width, unsigned int height, meters_t cellwidth,
                  meters_t cellheight);
 
@@ -1921,14 +1913,14 @@ by pose, in local coords */
 
   meters_t ModelHeight() const;
 
-  void DrawBlocksTree();
-  virtual void DrawBlocks();
-  void DrawBoundingBox();
-  void DrawBoundingBoxTree();
-  virtual void DrawStatus(Camera *cam, Canvas * canvas);
-  void DrawStatusTree(Camera *cam, Canvas * canvas);
+  void DrawBlocksTree(Canvas * canvas);
+  virtual void DrawBlocks(Canvas * canvas);
+  void DrawBoundingBox(Canvas * canvas);
+  void DrawBoundingBoxTree(Canvas * canvas);
+  virtual void DrawStatus(Camera* cam, Canvas * canvas);
+  void DrawStatusTree(Camera* cam, Canvas * canvas);
 
-  void DrawOriginTree();
+  void DrawOriginTree(Canvas * canvas);
   void DrawOrigin();
 
   void PushLocalCoords();
@@ -1939,22 +1931,19 @@ by pose, in local coords */
                  double height = 1.0);
 
   virtual void DrawPicker();
-  virtual void DataVisualize(Camera *cam);
+  virtual void DataVisualize(Camera *cam, Canvas* canvas);
   virtual void DrawSelected(Canvas * canvas);
 
-  void DrawTrailFootprint();
-  void DrawTrailBlocks();
-  void DrawTrailArrows();
+  void DrawTrailFootprint(Camera *cam, Canvas * canvas);
+  void DrawTrailBlocks(Camera *cam, Canvas * canvas);
+  void DrawTrailArrows(Canvas* canvas);
   void DrawGrid(Canvas * canvas);
   //	void DrawBlinkenlights();
   void DataVisualizeTree(Camera *cam, Canvas * canvas);
   void DrawFlagList();
-  void DrawPose(Pose pose);
+  //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,
@@ -2096,7 +2085,7 @@ model has no parent */
   bool IsRelated(const Model *testmod) const;
 
   /** get the pose of a model in the global CS */
-  Pose GetGlobalPose() const;
+  Pose GetGlobalPose() const override;
 
   /** subscribe to a model's data */
   void Subscribe();
@@ -2241,7 +2230,7 @@ public:
   public:
     explicit Vis(World *world);
     virtual ~Vis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   } vis;
 
 private:
@@ -2270,10 +2259,10 @@ public:
   /// Destructor
   ~ModelBlobfinder();
 
-  virtual void Startup();
-  virtual void Shutdown();
-  virtual void Update();
-  virtual void Load();
+  void Startup() override;
+  void Shutdown() override;
+  void Update() override;
+  void Load() override;
 
   /** Returns a non-mutable const reference to the detected blob
 data. Use this if you don't need to modify the model's
@@ -2303,7 +2292,7 @@ public:
   void SetState(bool isOn);
 
 protected:
-  virtual void DrawBlocks();
+  virtual void DrawBlocks(Canvas* canvas) override;
 
 private:
   bool m_IsOn;
@@ -2354,8 +2343,8 @@ public:
   };
 
 private:
-  virtual void Update();
-  virtual void DataVisualize(Camera *cam);
+  void Update() override;
+  void DataVisualize(Camera *cam, Canvas* canvas) override;
 
   void FixBlocks();
   void PositionPaddles();
@@ -2378,8 +2367,8 @@ public:
   /// destructor
   virtual ~ModelGripper();
 
-  virtual void Load();
-  virtual void Save();
+  void Load() override;
+  void Save() override;
 
   /** Configure the gripper */
   void SetConfig(config_t &newcfg)
@@ -2422,23 +2411,23 @@ public:
   ModelBumper(World *world, Model *parent, const std::string &type);
   virtual ~ModelBumper();
 
-  virtual void Load();
+  void Load() override;
 
   uint32_t bumper_count;
   BumperConfig *bumpers;
   BumperSample *samples;
 
 protected:
-  virtual void Startup();
-  virtual void Shutdown();
-  virtual void Update();
-  virtual void Print(char *prefix) const;
+  virtual void Startup() override;
+  virtual void Shutdown() override;
+  virtual void Update() override;
+  virtual void Print(char *prefix) const override;
 
   class BumperVis : public Visualizer {
   public:
     BumperVis();
     virtual ~BumperVis();
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   } bumpervis;
 
 private:
@@ -2468,8 +2457,8 @@ private:
   /// if neighbor is visible, add him to the fiducial scan
   void AddModelIfVisible(Model *him);
 
-  virtual void Update();
-  virtual void DataVisualize(Camera *cam, Canvas * canvas);
+  void Update() override;
+  void DataVisualize(Camera *cam, Canvas * canvas) override;
 
   static Option showData;
   static Option showFov;
@@ -2480,8 +2469,8 @@ public:
   ModelFiducial(World *world, Model *parent, const std::string &type);
   virtual ~ModelFiducial();
 
-  virtual void Load();
-  void Shutdown(void);
+  void Load() override;
+  void Shutdown(void) override;
 
   meters_t max_range_anon; ///< maximum detection range
   meters_t max_range_id; ///< maximum range at which the ID can be read
@@ -2512,7 +2501,7 @@ public:
   ModelRanger(World *world, Model *parent, const std::string &type);
   virtual ~ModelRanger();
 
-  virtual void Print(char *prefix) const;
+  virtual void Print(char *prefix) const override;
 
   class Vis : public Visualizer {
   public:
@@ -2524,7 +2513,7 @@ public:
 
     explicit Vis(World *world);
     virtual ~Vis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
+    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   } vis;
 
   class Sensor {
@@ -2551,7 +2540,7 @@ public:
     }
 
     void Update(ModelRanger *rgr);
-    void Visualize(Vis *vis, ModelRanger *rgr) const;
+    void Visualize(Vis *vis, ModelRanger *rgr, Canvas * canvas) const;
     std::string String() const;
     void Load(Worldfile *wf, int entity);
   };
@@ -2566,9 +2555,9 @@ private:
   std::vector<Sensor> sensors;
 
 protected:
-  virtual void Startup();
-  virtual void Shutdown();
-  virtual void Update();
+  virtual void Startup() override;
+  virtual void Shutdown() override;
+  virtual void Update() override;
 };
 
 // BLINKENLIGHT MODEL ----------------------------------------------------
@@ -2586,8 +2575,8 @@ public:
 
   ~ModelBlinkenlight();
 
-  virtual void Load();
-  virtual void Update();
+  void Load() override;
+  void Update() override;
   virtual void DataVisualize(Camera *cam);
 };
 
@@ -2634,10 +2623,10 @@ public:
 
   ~ModelCamera();
 
-  virtual void Load();
+  void Load() override;
 
   /// Capture a new frame ( calls GetFrame )
-  virtual void Update();
+  void Update() override;
 
   /// Draw Camera Model - TODO
   // virtual void Draw( uint32_t flags, Canvas* canvas );
@@ -2738,14 +2727,15 @@ Models and visualized. */
   public:
     WaypointVis();
     virtual ~WaypointVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
+
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   } wpvis;
 
   class PoseVis : public Visualizer {
   public:
     PoseVis();
     virtual ~PoseVis(void) {}
-    virtual void Visualize(Model *mod, Camera *cam, Canvas * canvas);
+    void Visualize(Model *mod, Camera *cam, Canvas * canvas) override;
   } posevis;
 
   /** Set the current pose estimate.*/
@@ -2779,10 +2769,10 @@ a (radians per second squared) */
 
 protected:
   virtual void Move();
-  virtual void Startup();
-  virtual void Shutdown();
-  virtual void Update();
-  virtual void Load();
+  virtual void Startup() override;
+  virtual void Shutdown() override;
+  virtual void Update() override;
+  virtual void Load() override;
 };
 
 // ACTUATOR MODEL --------------------------------------------------------
@@ -2817,10 +2807,10 @@ public:
   /// Destructor
   ~ModelActuator();
 
-  virtual void Startup();
-  virtual void Shutdown();
-  virtual void Update();
-  virtual void Load();
+  virtual void Startup() override;
+  virtual void Shutdown() override;
+  virtual void Update() override;
+  virtual void Load() override;
 
   /** Sets the control_mode to CONTROL_VELOCITY and sets
 the goal velocity. */
diff --git a/libstage/vis_strip.cc b/libstage/vis_strip.cc
index a03c0cc20..cbf07244d 100644
--- a/libstage/vis_strip.cc
+++ b/libstage/vis_strip.cc
@@ -28,13 +28,13 @@ void StripPlotVis::Visualize(Model *mod, Camera *camera, Canvas * canvas)
 
   canvas->EnterScreenCS();
 
-  mod->PushColor(bgcolor);
+  canvas->PushColor(bgcolor);
   glRectf(x, y, w, h);
-  mod->PopColor();
+  canvas->PopColor();
 
-  mod->PushColor(fgcolor);
+  canvas->PushColor(fgcolor);
   canvas->draw_array(x, y, w, h, data, len, count % len, min, max);
-  mod->PopColor();
+  canvas->PopColor();
 
   canvas->LeaveScreenCS();
 }
diff --git a/libstage/world_gui.hh b/libstage/world_gui.hh
index a96ab78fa..468c45da2 100644
--- a/libstage/world_gui.hh
+++ b/libstage/world_gui.hh
@@ -9,7 +9,6 @@
 #define STAGE_LIBSTAGE_STAGE_GUI_HH_
 
 #include "stage.hh"
-#include "canvas_fltk.hh"
 
 // FLTK Gui includes
 #include <FL/Fl.H>
@@ -18,13 +17,20 @@
 #include <FL/fl_draw.H>
 #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
 
-namespace Stg {
-/** Extends World to implement an FLTK / OpenGL graphical user
-      interface.
-  */
+// Some FLTK forwardings
+class Fl_Menu_Bar;
+class Fl_Widget;
+
+namespace Stg
+{
+
 class CanvasFLTK;
 
-class WorldGui : public World, public Fl_Window {
+/**
+ * Extends World to implement an FLTK / OpenGL graphical user interface.
+ */
+class WorldGui : public World, public Fl_Window
+{
   friend class Canvas;
   friend class ModelCamera;
   friend class Model;
@@ -36,8 +42,10 @@ private:
   FileManager *fileMan; ///< Used to load and save worldfiles
   std::vector<usec_t> interval_log;
 
-  /** Stage attempts to run this many times faster than real
-time. If -1, Stage runs as fast as possible. */
+  /**
+   * Stage attempts to run this many times faster than real
+	 *	time. If -1, Stage runs as fast as possible.
+	 */
   double speedup;
 
   bool confirm_on_quit; ///< if true, show save dialog on (GUI) exit (default)
@@ -48,7 +56,8 @@ time. If -1, Stage runs as fast as possible. */
   std::string caption_prefix; //!< prefix of window caption (default PROJECT name constant)
 
   /** The amount of real time elapsed between $timing_interval
-timesteps. */
+    * timesteps.
+    */
   usec_t real_time_interval;
 
   /** The current real time in microseconds. */
@@ -83,7 +92,7 @@ updates. */
   bool saveAsDialog();
   bool closeWindowQuery();
 
-  virtual void AddModel(Model *mod);
+  virtual void AddModel(Model *mod) override;
 
   void SetTimeouts();
 
@@ -99,30 +108,30 @@ public:
   ~WorldGui();
 
   /** Forces the window to be redrawn, even if paused.*/
-  virtual void Redraw(void);
+  virtual void Redraw(void) override;
 
-  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 std::string ClockString() const override;
+  virtual bool Update() override;
+  virtual bool Load(const std::string &worldfile_path) override;
+  virtual bool Load(std::istream &world_content, const std::string &worldfile_path = std::string()) override;
 
-  virtual void UnLoad();
-  virtual bool Save(const char *filename);
-  virtual bool IsGUI() const { return true; }
-  virtual Model *RecentlySelectedModel() const;
+  virtual void UnLoad() override;
+  virtual bool Save(const char *filename) override;
+  virtual bool IsGUI() const override { return true; }
+  virtual Model *RecentlySelectedModel() const override;
 
-  virtual void Start();
-  virtual void Stop();
+  virtual void Start() override;
+  virtual void Stop() override;
 
   usec_t RealTimeNow(void) const;
 
-  Canvas *GetCanvas(void);
+  Canvas *GetCanvas(void) override;
   /** show the window - need to call this if you don't Load(). */
   void Show();
 
   /** Get human readable string that describes the current global energy state. */
   std::string EnergyString(void) const;
-  virtual void RemoveChild(Model *mod);
+  virtual void RemoveChild(Model *mod) override;
 
   bool IsTopView();
 
diff --git a/stage-config.cmake.in b/stage-config.cmake.in
index 86b2f1d5f..6d01c782e 100644
--- a/stage-config.cmake.in
+++ b/stage-config.cmake.in
@@ -1,7 +1,8 @@
 # - Config file for Stage
 # It defines the following variables:
 #    STAGE_INCLUDE_DIRS - Stage Include directories
-#    STAGE_LIBRARIES    - Stage link libraries
+#    STAGE_LIBRARIES    - Stage link libraries. Contains simulation code and Fltk UI
+#    STAGE_CORE_LIBRARIES - Stage core libraries. Contains simulation code, without UI
 
 get_filename_component(STAGE_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 
@@ -9,15 +10,19 @@ set(STAGE_INCLUDE_DIRS "${STAGE_CMAKE_DIR}/../../../include/@PROJECT_NAME@-@APIV
   "@OPENGL_INCLUDE_DIR@")
 
 set(STAGE_USE_GUI @BUILD_GUI@)
-set(STAGE_LIBRARIES
+
+set(STAGE_CORE_LIBRARIES
   "${STAGE_CMAKE_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_CORE_TARGET_NAME@"
   "@OPENGL_gl_LIBRARIES@"
 )
 
+set(STAGE_LIBRARIES ${STAGE_CORE_LIBRARIES})
+
 if(STAGE_USE_GUI)
-	LIST(APPEND STAGE_INCLUDE_DIRS "@FLTK_INCLUDE_DIR@")
-	LIST(APPEND STAGE_LIBRARIES "${STAGE_CMAKE_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
-	LIST(APPEND STAGE_LIBRARIES "@FLTK_LIBRARIES@")
+	list(APPEND STAGE_INCLUDE_DIRS "@FLTK_INCLUDE_DIR@")
+	# Using relative path to make cross compilation easier
+	list(APPEND STAGE_LIBRARIES "${STAGE_CMAKE_DIR}/../../../@PROJECT_LIB_DIR@/@STAGE_TARGET_NAME@")
+	list(APPEND STAGE_LIBRARIES "@FLTK_LIBRARIES@")
 endif(STAGE_USE_GUI)
 
 list(REMOVE_DUPLICATES STAGE_INCLUDE_DIRS)

From f0a80ef14ccb4cd5934d3364a941ab60f111fadd Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Wed, 14 Feb 2018 17:34:34 +0300
Subject: [PATCH 19/22] Removed extra c++ warnings for override keyword

---
 CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ad4c2c679..02b6074ac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,8 +50,8 @@ endif( PROJECT_OS_OSX)
 # Build type cflags
 SET (OPTIMIZE "-O2")
 
-SET (CMAKE_CXX_FLAGS_RELEASE "${FORCE_ARCH} ${OPTIMIZE} -DNDEBUG ${WALL} -Wsuggest-override -std=c++11" CACHE INTERNAL "CXX Flags for release" FORCE)
-SET (CMAKE_CXX_FLAGS_DEBUG "-ggdb  ${FORCE_ARCH} ${WALL} -DDEBUG -Wsuggest-override -std=c++11" CACHE INTERNAL "CXX Flags for debug" FORCE)
+SET (CMAKE_CXX_FLAGS_RELEASE "${FORCE_ARCH} ${OPTIMIZE} -DNDEBUG ${WALL} -std=c++11" CACHE INTERNAL "CXX Flags for release" FORCE)
+SET (CMAKE_CXX_FLAGS_DEBUG "-ggdb  ${FORCE_ARCH} ${WALL} -DDEBUG -std=c++11" CACHE INTERNAL "CXX Flags for debug" FORCE)
 SET (CMAKE_CXX_FLAGS_PROFILE "-ggdb -pg  ${FORCE_ARCH} ${WALL}" CACHE INTERNAL "CXX Flags for profile" FORCE)
 
 #####################################

From eec1964405cbcf3bc86d9388ae63193ac9856d61 Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Wed, 16 May 2018 01:26:54 +0300
Subject: [PATCH 20/22] Trying to fix compiler error from travis

---
 libstage/canvas_fltk.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libstage/canvas_fltk.cc b/libstage/canvas_fltk.cc
index 1f5e3de72..2eb319377 100644
--- a/libstage/canvas_fltk.cc
+++ b/libstage/canvas_fltk.cc
@@ -457,7 +457,7 @@ void CanvasFLTK::draw_string_multiline(float x, float y, float w, float h, const
 {
   // printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str),
   // str );
-  gl_draw(str, x, y, w, h, align); // fltk function
+  gl_draw(str, x, y, w, h, (Fl_Align)align); // fltk function
 }
 
 void CanvasFLTK::draw()

From d0945c092accca782280d020057141d67c13ca4a Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Wed, 16 May 2018 01:33:57 +0300
Subject: [PATCH 21/22] Fixed DataVisualize methods

All classes should use void DataVisualize(Camera *cam, Canvas* canvas) signature.
Seems like I've missed camera and blinkenlight during my fix.
---
 libstage/model_blinkenlight.cc | 2 +-
 libstage/model_camera.cc       | 2 +-
 libstage/stage.hh              | 4 ++--
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/libstage/model_blinkenlight.cc b/libstage/model_blinkenlight.cc
index f4ebeed8c..2a7dfbb75 100644
--- a/libstage/model_blinkenlight.cc
+++ b/libstage/model_blinkenlight.cc
@@ -99,7 +99,7 @@ void ModelBlinkenlight::Update(void)
   Model::Update();
 }
 
-void ModelBlinkenlight::DataVisualize(Camera *cam)
+void ModelBlinkenlight::DataVisualize(Camera *cam, Canvas* canvas)
 {
   (void)cam; // avoid warning about unused var
 
diff --git a/libstage/model_camera.cc b/libstage/model_camera.cc
index 0d8556e64..cc9f33eba 100644
--- a/libstage/model_camera.cc
+++ b/libstage/model_camera.cc
@@ -220,7 +220,7 @@ bool ModelCamera::GetFrame(Canvas * canvas)
 
 // TODO create lines outlining camera frustrum, then iterate over each depth
 // measurement and create a square
-void ModelCamera::DataVisualize(Camera *)
+void ModelCamera::DataVisualize(Camera*, Canvas*)
 {
   if (_frame_data == NULL || !showCameraData)
     return;
diff --git a/libstage/stage.hh b/libstage/stage.hh
index a6aaac00f..e77d6110f 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -2577,7 +2577,7 @@ public:
 
   void Load() override;
   void Update() override;
-  virtual void DataVisualize(Camera *cam);
+  virtual void DataVisualize(Camera *cam, Canvas* canvas) override;
 };
 
 // CAMERA MODEL ----------------------------------------------------
@@ -2632,7 +2632,7 @@ public:
   // virtual void Draw( uint32_t flags, Canvas* canvas );
 
   /// Draw camera visualization
-  virtual void DataVisualize(Camera *cam);
+  virtual void DataVisualize(Camera *cam, Canvas* canvas) override;
 
   /// width of captured image
   int getWidth(void) const { return _width; }

From 804ab46839d52452e44ec6cff4936d23cd460dee Mon Sep 17 00:00:00 2001
From: Dmitry Kargin <dv_frost@mail.ru>
Date: Sat, 9 Feb 2019 16:19:05 +0300
Subject: [PATCH 22/22] Fixed image loader to handle png with palette

---
 libstage/image.cc | 27 +++++++++++++++------------
 1 file changed, 15 insertions(+), 12 deletions(-)

diff --git a/libstage/image.cc b/libstage/image.cc
index 7b7ea6611..f79015f96 100644
--- a/libstage/image.cc
+++ b/libstage/image.cc
@@ -83,11 +83,8 @@ bool Image::load(const char * file_name)
 
 		png_init_io(png_ptr, fp);
 		png_set_sig_bytes(png_ptr, 8);
-
 		png_read_info(png_ptr, info_ptr);
 
-		;
-
 		width = png_get_image_width(png_ptr, info_ptr);
 		height = png_get_image_height(png_ptr, info_ptr);
 		png_byte color_type = png_get_color_type(png_ptr, info_ptr);
@@ -103,22 +100,31 @@ bool Image::load(const char * file_name)
 
 		switch(color_type)
 		{
-		case PNG_COLOR_TYPE_GRAY:
+		case PNG_COLOR_TYPE_GRAY:	// 0
 			bpp = bit_depth / 8;
 			break;
-		case PNG_COLOR_TYPE_RGB:
+		case PNG_COLOR_TYPE_RGB:	// 2
 			bpp = 3 * bit_depth / 8;
 			break;
-		case PNG_COLOR_TYPE_RGB_ALPHA:
+		case PNG_COLOR_TYPE_RGB_ALPHA:	// 6
 			bpp = 4 * bit_depth / 8;
 			break;
+		case PNG_COLOR_TYPE_PALETTE:	// 3
+			// TODO: In fact we do not well with the palette
+			bpp = bit_depth / 8;
+			break;
 		default:
 			bpp = 0;
 			printf("Unsupported color type: %d\n", color_type);
 			break;
 		}
 
-		//bpp = bit_depth / 8;
+		if (bpp == 0)
+		{
+			printf("Failed to determine bits per pixel for color type: %d\n", color_type);
+			break;
+		}
+
 		/* read file */
 		if (setjmp(png_jmpbuf(png_ptr)))
 		{
@@ -129,17 +135,14 @@ bool Image::load(const char * file_name)
 		int row_length = png_get_rowbytes(png_ptr, info_ptr);
 		std::vector<uint8_t> row_data(row_length);
 
-		this->data.resize(width*height*bpp);
+		data.resize(width*height*bpp);
 		for (int y=0; y<height; y++)
 		{
-			//row_pointers[y] = (png_byte*) malloc(png_get_rowbytes(png_ptr,info_ptr));
 			png_bytep row_pointer = &row_data[0];
 			png_read_row(png_ptr, row_pointer, NULL);
-			uint8_t * dst = &this->data[y*width*bpp];
+			uint8_t * dst = &data[y*width*bpp];
 			memcpy(dst, row_pointer, width*bpp);
 		}
-
-		//png_read_image(png_ptr, row_pointers);
 		done = true;
 	}while(false);