stage.hh

Go to the documentation of this file.

#ifndef STG_H
#define STG_H
/*
 *  Stage : a multi-robot simulator. Part of the Player Project.
 * 
 *  Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
 *  Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
 *  Asher, Pooya Karimian
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

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

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

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

namespace Stg 
{
  // forward declare
  class Block;
  class Canvas;
  class Cell;
  class Worldfile;
  class World;
  class WorldGui;
  class Model;
  class OptionsDlg;
  class Camera;
  class FileManager;
  class Option;
  
  typedef Model* (*creator_t)( World*, Model*, const std::string& type );
  
  void Init( int* argc, char** argv[] );

  bool InitDone();
  
  const char* Version();

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

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

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

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

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

  const double thousand = 1e3;

  const double million = 1e6;

  const double billion = 1e9;

  inline double rtod( double r ){ return( r*180.0/M_PI ); }
  
  inline double dtor( double d ){ return( d*M_PI/180.0 ); }
  
  inline double normalize( double a )
  {
    while( a < -M_PI ) a += 2.0*M_PI;
    while( a >  M_PI ) a -= 2.0*M_PI;    
    return a;
  };
    
  inline int sgn( int a){ return( a<0 ? -1 : 1); }
    
  inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
  
  enum { FiducialNone = 0 };

  typedef uint32_t id_t;

  typedef double meters_t;

  typedef double radians_t;

  typedef struct timeval time_t;

  typedef unsigned long msec_t;

  typedef uint64_t usec_t;

  typedef double kg_t; // Kilograms (mass)

  typedef double joules_t;

  typedef double watts_t;
  
  class Color
  {
  public:
    double r,g,b,a;
    
    Color( double r, double g, double b, double a=1.0 );
    
    Color( const std::string& name );   
    
    Color();
    
    bool operator!=( const Color& other ) const;
    bool operator==( const Color& other ) const;
    static Color RandomColor();
    void Print( const char* prefix ) const;

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

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

    void GLSet( void ) { glColor4f( r,g,b,a ); }
  };
  
  class Size
  {
  public:
    meters_t x, y, z;
    
    Size( meters_t x, 
      meters_t y, 
      meters_t z )
      : x(x), y(y), z(z)
    {/*empty*/}
    
    Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
    {/*empty*/} 
    
    Size& Load( Worldfile* wf, int section, const char* keyword );
    void Save( Worldfile* wf, int section, const char* keyword ) const;
     
    void Zero()
    { x=y=z=0.0; }
  };
  
  class Pose
  {
  public:
    meters_t x, y, z;
    radians_t a;
    
    Pose( meters_t x, 
      meters_t y, 
      meters_t z,
      radians_t a ) 
      : x(x), y(y), z(z), a(a)
    { /*empty*/ }
    
    Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
    { /*empty*/ }        
    
    virtual ~Pose(){};
    
    static Pose Random( meters_t xmin, meters_t xmax, 
            meters_t ymin, meters_t ymax )
    {        
      return Pose( xmin + drand48() * (xmax-xmin),
           ymin + drand48() * (ymax-ymin),
           0, 
           normalize( drand48() * (2.0 * M_PI) ));
    }
    
    virtual void Print( const char* prefix ) const
    {
      printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
          prefix, x,y,z,a );
    }
    
    std::string String() const
    {
      char buf[256];
      snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
        x,y,z,a );
      return std::string(buf);
    }
    
    /* returns true iff all components of the velocity are zero. */
    bool IsZero() const 
    { return( !(x || y || z || a )); };
    
    void Zero()
    { x=y=z=a=0.0; }
    
    Pose& Load( Worldfile* wf, int section, const char* keyword );
    void Save( Worldfile* wf, int section, const char* keyword );
    
    inline Pose operator+( const Pose& p ) const
    {
      const double cosa = cos(a);
      const double sina = sin(a);
      
      return Pose( x + p.x * cosa - p.y * sina,
           y + p.x * sina + p.y * cosa,
           z + p.z,
           normalize(a + p.a) );     
    }   
     
    // a < b iff a is closer to the origin than b
    bool operator<( const Pose& other ) const
    {
      return( hypot( y, x ) < hypot( other.y, other.x ));
    }
     
    bool operator==( const Pose& other ) const
    {
      return(  x==other.x && 
           y==other.y && 
           z==other.z && 
           a==other.a );
    }

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

  };
  
  
  class Velocity : public Pose
  {
  public:
    Velocity( double x, 
          double y, 
          double z,
          double a ) :
      Pose( x, y, z, a )
    { /*empty*/ }
    
    Velocity()
    { /*empty*/ }        
    
    Velocity& Load( Worldfile* wf, int section, const char* keyword )
    {
      Pose::Load( wf, section, keyword );
      return *this;
    }
    
    virtual void Print( const char* prefix ) const
    {
      if( prefix )
    printf( "%s", prefix );
      
      printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
          x,y,z,a );        
    }    
  };
  
 
  class Geom
  {
  public:
    Pose pose;
    Size size;
    
    void Print( const char* prefix ) const
    {
      if( prefix )
    printf( "%s", prefix );
        
      printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
          pose.x,
          pose.y,
          pose.a,
          size.x,
          size.y );
    }
     
    Geom() : pose(), size() {}

    Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}

    void Zero()
    {
      pose.Zero();
      size.Zero();
    }
  };
  
  class Bounds
  {
  public:
    double min;
    double max; 
    
    Bounds() : min(0), max(0) { /* empty*/  }
    Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }

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

    // returns value, but no smaller than min and no larger than max.
    double Constrain( double value );
  };
    
  class bounds3d_t
  {
  public:
    Bounds x; 
    Bounds y; 
    Bounds z; 

    bounds3d_t() : x(), y(), z() {}
    bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) 
      : x(x), y(y), z(z) {}
  };
  
  typedef struct
  {
    Bounds range; 
    radians_t angle; 
  } fov_t;
  
  class point_t
  {
  public:
    meters_t x, y;
    point_t( meters_t x, meters_t y ) : x(x), y(y){}     
    point_t() : x(0.0), y(0.0){}
    
    bool operator+=( const point_t& other ) 
    { return ((x += other.x) && (y += other.y) ); }  
  };
  
  class point3_t
  {
  public:
    meters_t x,y,z;
    point3_t( meters_t x, meters_t y, meters_t z ) 
      : x(x), y(y), z(z) {}  
  
    point3_t() : x(0.0), y(0.0), z(0.0) {}
  };
  
  class point_int_t
  {
  public:
    int x,y;
    point_int_t( int x, int y ) : x(x), y(y){}   
    point_int_t() : x(0), y(0){}
        
    bool operator<( const point_int_t& other ) const
    {
      if( x < other.x ) return true;
      if( other.x < x ) return false;
      return y < other.y;
    }
        
    bool operator==( const point_int_t& other ) const
    { return ((x == other.x) && (y == other.y) ); }
  };
  
  point_t* unit_square_points_create();
  
  namespace Gl
  {
    void pose_shift( const Pose &pose );
    void pose_inverse_shift( const Pose &pose );
    void coord_shift( double x, double y, double z, double a  );
    void draw_grid( bounds3d_t vol );
    void draw_string( float x, float y, float z, const char *string);
    void draw_string_multiline( float x, float y, float w, float h, 
                const char *string, Fl_Align align );
    void draw_speech_bubble( float x, float y, float z, const char* str );
    void draw_octagon( float w, float h, float m );
    void draw_octagon( float x, float y, float w, float h, float m );
    void draw_vector( double x, double y, double z );
    void draw_origin( double len );
    void draw_array( float x, float y, float w, float h, 
             float* data, size_t len, size_t offset, 
             float min, float max );
    void draw_array( float x, float y, float w, float h, 
             float* data, size_t len, size_t offset );
    void draw_centered_rect( float x, float y, float dx, float dy );
  } // namespace Gl
  
  void RegisterModels();
  
  
  class Visualizer {
  private:
    const std::string menu_name;
    const std::string worldfile_name;
     
  public:
    Visualizer( const std::string& menu_name, 
        const std::string& worldfile_name ) 
      : menu_name( menu_name ),
    worldfile_name( worldfile_name )
    { }
     
    virtual ~Visualizer( void ) { }
    virtual void Visualize( Model* mod, Camera* cam ) = 0;
     
    const std::string& GetMenuName() { return menu_name; }
    const std::string& GetWorldfileName() { return worldfile_name; }     
  };


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

  typedef int(*world_callback_t)(World* world, void* user );
  
  // return val, or minval if val < minval, or maxval if val > maxval
  double constrain( double val, double minval, double maxval );
    
  typedef struct 
  {
    int enabled;
    Pose pose;
    meters_t size; 
    Color color;
    msec_t period; 
    double duty_cycle; 
  } blinkenlight_t;

  
  typedef struct
  {
    Pose pose;
    Size size;
  } rotrect_t; // rotated rectangle
  
  int rotrects_from_image_file( const std::string& filename, 
                std::vector<rotrect_t>& rects );
  
  typedef bool (*ray_test_func_t)(Model* candidate, 
                  Model* finder, 
                  const void* arg );

  // STL container iterator macros - __typeof is a gcc extension, so
  // this could be an issue one day.
#define VAR(V,init) __typeof(init) V=(init)

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

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

  template <class T, class C>
  void EraseAll( T thing, C& cont )
  { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
  
  // Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
#define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
#define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

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

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

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

  class Block;
  class Model;


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

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

    std::vector<Model*> children;

    bool debug;

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

    std::string token;

    Ancestor& Load( Worldfile* wf, int section );
    void Save( Worldfile* wf, int section );     
             
  public:   
    Ancestor();
    virtual ~Ancestor();
     
    std::vector<Model*>& GetChildren(){ return children;}
    
    void ForEachDescendant( model_callback_t func, void* arg );
    
    virtual void AddChild( Model* mod );
    virtual void RemoveChild( Model* mod );
    virtual Pose GetGlobalPose() const;
    
    const char* Token() const { return token.c_str(); }
    const std::string& TokenStr() const { return token; }
    
    virtual void SetToken( const std::string& str )
    { 
      //printf( "Ancestor::SetToken( %s )\n", str.c_str() );

      if( str.size() > 0 ) 
    token = str;
      else
    PRINT_ERR( "Ancestor::SetToken() called with zero length string. Ignored." );
    } 
     
    void SetProperty( std::string& key, void* value ){ props[ key ] = value; }
     
    void* GetProperty( std::string& key )
    {
      std::map<std::string,void*>::iterator it = props.find( key );     
      return( it == props.end() ? NULL : it->second );
    }
  };

  class RaytraceResult
  {
  public:
    Pose pose; 
    meters_t range; 
    Model* mod; 
    Color color; 
     
    RaytraceResult() : pose(), range(0), mod(NULL), color() {}
    RaytraceResult( const Pose& pose, 
            meters_t range ) 
      : pose(pose), range(range), mod(NULL), color() {}  
  };
    
  class Ray
  {
  public:
    Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
      mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
    {}

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

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

  class LogEntry
  {
    usec_t timestamp;
    Model* mod;
    Pose pose;
     
  public:
    LogEntry( usec_t timestamp, Model* mod );
     
    static std::vector<LogEntry> log;
     
    static size_t Count(){ return log.size(); }
     
    static void Clear(){ log.clear(); }

    static void Print();
  };

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

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

  class ModelPosition;

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

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

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

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

    std::map<int,Model*> models_by_wfentity;
        
    std::vector<Model*> models_with_fiducials;
        
    struct ltx
    {
      bool operator()(const Model* a, const Model* b) const;
    };
        
    struct lty
    {
      bool operator()(const Model* a, const Model* b) const;
    };
        
    std::set<Model*,ltx> models_with_fiducials_byx;
        
    std::set<Model*,lty> models_with_fiducials_byy;
                     
    void FiducialInsert( Model* mod )
    { 
      FiducialErase( mod ); // make sure it's not there already
      models_with_fiducials.push_back( mod ); 
    }
     
    void FiducialErase( Model* mod )
    { 
      EraseAll( mod, models_with_fiducials );
    }

    double ppm; 
    bool quit; 
     
    bool show_clock; 
    unsigned int show_clock_interval; 
        
    pthread_mutex_t sync_mutex; 
    unsigned int threads_working; 
    pthread_cond_t threads_start_cond; 
    pthread_cond_t threads_done_cond; 
    int total_subs; 
    unsigned int worker_threads; 
    
  protected:     

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

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

    usec_t quit_time;
    std::list<float*> ray_list;
    usec_t sim_time; 
    std::map<point_int_t,SuperRegion*> superregions;
     
    std::vector< std::vector<Model*> > update_lists;  
     
    uint64_t updates; 
    Worldfile* wf; 

    void CallUpdateCallbacks(); 

  public:

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

    virtual void Start(){ paused = false; };
    virtual void Stop(){ paused = true; };
    virtual void TogglePause(){ paused ? Start() : Stop(); };

    bool Paused() const { return( paused ); };
        
    virtual void Redraw( void ){ }; // does nothing

    std::vector<point_int_t> rt_cells;
    std::vector<point_int_t> rt_candidate_cells;

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

    void AddUpdateCallback( world_callback_t cb, void* user );

    int RemoveUpdateCallback( world_callback_t cb, void* user );

    void Log( Model* mod );

    void NeedRedraw(){ dirty = true; };
    
    Model* ground;
    
    virtual std::string ClockString( void ) const;
        
    Model* CreateModel( Model* parent, const std::string& typestr );     
    void LoadModel( Worldfile* wf, int entity );
    void LoadBlock( Worldfile* wf, int entity );
    void LoadBlockGroup( Worldfile* wf, int entity );
    
    void LoadSensor( Worldfile* wf, int entity );

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

    SuperRegion* AddSuperRegion( const point_int_t& coord );
    SuperRegion* GetSuperRegion( const point_int_t& org );
    SuperRegion* GetSuperRegionCreate( const point_int_t& org );
    //void ExpireSuperRegion( SuperRegion* sr );
        
    int32_t MetersToPixels( meters_t x ) const
    { return (int32_t)floor(x * ppm); };
        
    point_int_t MetersToPixels( const point_t& pt ) const
    { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
        
    // 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 );
        
    RaytraceResult Raytrace( const Ray& ray );

    RaytraceResult Raytrace( const Pose& pose,           
                 const meters_t range,
                 const ray_test_func_t func,
                 const Model* finder,
                 const void* arg,
                 const bool ztest );
        
    void Raytrace( const Pose &pose,             
           const meters_t range,
           const radians_t fov,
           const ray_test_func_t func,
           const Model* finder,
           const void* arg,
           RaytraceResult* samples,
           const uint32_t sample_count,
           const bool ztest );
        
        
    inline void Extend( point3_t pt );
  
    virtual void AddModel( Model* mod );
    virtual void RemoveModel( Model* mod );

    void AddModelName( Model* mod, const std::string& name );
        
    void AddPowerPack( PowerPack* pp );
    void RemovePowerPack( PowerPack* pp );
        
    void ClearRays();
  
    void RecordRay( double x1, double y1, double x2, double y2 );
        
    bool PastQuitTime();
                
    static void* update_thread_entry( std::pair<World*,int>* info );
    
    class Event
    {
    public:
      
      Event( usec_t time, Model* mod, model_callback_t cb, void* arg ) 
    : time(time), mod(mod), cb(cb), arg(arg) {}
            
      usec_t time; 
      Model* mod; 
      model_callback_t cb;
      void* arg;
            
      bool operator<( const Event& other ) const;
    };
     
    std::vector<std::priority_queue<Event> > event_queues;

    std::vector<std::queue<Model*> > pending_update_callbacks;
        
    void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg )
    {  event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); }
        
    std::set<Model*> active_energy;
    void EnableEnergy( Model* m ) { active_energy.insert( m ); };
    void DisableEnergy( Model* m ) { active_energy.erase( m ); };
    
    std::set<ModelPosition*> active_velocity;
    
    usec_t sim_interval;
        
    // debug instrumentation - making sure the number of update callbacks
    // in each thread is consistent with the number that have been
    // registered globally
    int update_cb_count;

    void ConsumeQueue( unsigned int queue_num );

    unsigned int GetEventQueue( Model* mod ) const;

  public:
    static bool UpdateAll(); 
     
  static void Run();
     
    World( const std::string& name = "MyWorld", 
       double ppm = DEFAULT_PPM );
        
    virtual ~World();
    
    usec_t SimTimeNow(void) const { return sim_time; }
        
    Worldfile* GetWorldFile()   { return wf; };
        
    virtual bool IsGUI() const { return false; }
     
    virtual void Load( const std::string& worldfile_path );

    virtual void UnLoad();

    virtual void Reload();

    virtual bool Save( const char* filename );

    virtual bool Update(void);
     
    bool TestQuit() const { return( quit || quit_all );  }
        
    void Quit(){ quit = true; }

    void QuitAll(){ quit_all = true; }

    void CancelQuit(){ quit = false; }

    void CancelQuitAll(){ quit_all = false; }
     
    void TryCharge( PowerPack* pp, const Pose& pose );

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

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

    void RegisterOption( Option* opt ); 
     
    void ShowClock( bool enable ){ show_clock = enable; };

    Model* GetGround() {return ground;};
    
  };
  
  class Block
  {
    friend class BlockGroup;
    friend class Model;
    friend class SuperRegion;
    friend class World;
    friend class Canvas;
    friend class Cell;
  public:
        
    Block( Model* mod,  
       const std::vector<point_t>& pts,
       meters_t zmin,
       meters_t zmax,
       Color color,
       bool inherit_color,
       bool wheel );
        
    Block(  Model* mod,  Worldfile* wf, int entity);
        
    ~Block();
     
    void Map( unsigned int layer );      
        
    void UnMap( unsigned int layer );    
        
    void DrawSolid(bool topview);

    void DrawFootPrint(); 

    void Translate( double x, double y );    

    double CenterX();

    double CenterY();

    void SetCenterX( double y );

    void SetCenterY( double y );

    void SetCenter( double x, double y);     

    void SetZ( double min, double max );
        
    void AppendTouchingModels( std::set<Model*>& touchers );
     
    Model* TestCollision(); 

    void Load( Worldfile* wf, int entity );  
    Model* GetModel(){ return mod; };  
    const Color& GetColor();        
    void Rasterize( uint8_t* data, 
            unsigned int width, unsigned int height,        
            meters_t cellwidth, meters_t cellheight );
        
  private:
    Model* mod; 
    std::vector<point_t> mpts; 
    size_t pt_count; 
    std::vector<point_t> pts; 
    Size size;   
    Bounds local_z; 
    Color color;
    bool inherit_color;
    bool wheel;

    void DrawTop();
    void DrawSides();
     
    Bounds global_z;     
    bool mapped;
        
    std::vector< std::list<Block*>::iterator > list_entries;
        
    std::vector<Cell*> rendered_cells[2];
        
    point_t BlockPointToModelMeters( const point_t& bpt );
    
    void InvalidateModelPointCache();
        
  };

    
  class BlockGroup
  {
    friend class Model;
    friend class Block;
        
  private:
    int displaylist;
        
    void BuildDisplayList( Model* mod );
        
    std::vector<Block*> blocks;
    Size size;
    point3_t offset;
    meters_t minx, maxx, miny, maxy;
        
  public:
    BlockGroup();
    ~BlockGroup();
        
    uint32_t GetCount(){ return blocks.size(); };
    const Size& GetSize(){ return size; };
    const point3_t& GetOffset(){ return offset; };
        
    void CalcSize();
     
    void AppendBlock( Block* block );
    void CallDisplayList( Model* mod );
    void Clear() ; 
    void AppendTouchingModels( std::set<Model*>& touchers );
     
    Model* TestCollision();
 
    void Map( unsigned int layer );
    void UnMap( unsigned int layer );
        
    void DrawSolid( const Geom &geom); 

    void DrawFootPrint( const Geom &geom);

    void LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile *wf );
    void LoadBlock( Model* mod, Worldfile* wf, int entity );
     
    void Rasterize( uint8_t* data, 
            unsigned int width, unsigned int height,
            meters_t cellwidth, meters_t cellheight );
     
    void InvalidateModelPointCache()
    {
      FOR_EACH( it, blocks )
    (*it)->InvalidateModelPointCache();
    }

  };

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

    virtual void Draw( void ) const = 0;
    virtual void SetProjection( void ) const = 0;

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

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

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

  public:
    PerspectiveCamera( void );

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

    void strafe( double amount );
    void forward( double amount );
    
    void setPose( double x, double y, double z ) { _x = x; _y = y; _z = z; }
    void addPose( double x, double y, double z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
    void move( double x, double y, double z );
    void setFov( double horiz_fov, double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
    void setAspect( double aspect ) { _aspect = aspect; }
    void setYaw( double yaw ) { _yaw = yaw; }
    double horizFov( void ) const { return _horiz_fov; }
    double vertFov( void ) const { return _vert_fov; }
    void addYaw( double yaw ) { _yaw += yaw; }
    void setPitch( double pitch ) { _pitch = pitch; }
    void addPitch( double pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
    
    double realDistance( double z_buf_val ) const {
      return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
    }
    void scroll( double dy ) { _z += dy; }
    double nearClip( void ) const { return _z_near; }
    double farClip( void ) const { return _z_far; }
    void setClip( double near, double far ) { _z_far = far; _z_near = near; }
    
    void reset() { setPitch( 70 ); setYaw( 0 ); }
    
    void Load( Worldfile* wf, int sec );
    void Save( Worldfile* wf, int sec );
  };
  
  class OrthoCamera : public Camera
  {
  private:
    double _scale;
    double _pixels_width;
    double _pixels_height;
    double _y_min;
    double _y_max;
  
  public:
    OrthoCamera( void ) : 
        _scale( 15 ),
        _pixels_width(0),
        _pixels_height(0),
        _y_min(0),
        _y_max(0)
     { }
     
    virtual void Draw() const;

    virtual void SetProjection( double pixels_width, 
                                          double pixels_height, 
                                          double y_min,
                                          double y_max );
     
    virtual void SetProjection( void ) const;
     
    void move( double x, double y );

    void setYaw( double yaw ) { _yaw = yaw; }

    void setPitch( double pitch ) { _pitch = pitch; }

    void addYaw( double yaw ) { _yaw += yaw;    }

    void addPitch( double pitch ) {
      _pitch += pitch;
      if( _pitch > 90 )
    _pitch = 90;
      else if( _pitch < 0 )
    _pitch = 0;
    }
  
    void setScale( double scale ) { _scale = scale; }
    void setPose( double x, double y) { _x = x; _y = y; }
  
    void scale( double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0 ); 
    void reset( void ) { _pitch = _yaw = 0; }
  
    double scale() const { return _scale; }
  
    void Load( Worldfile* wf, int sec );
    void Save( Worldfile* wf, int sec );
  };


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

  private:

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

    Fl_Menu_Bar* mbar;
    OptionsDlg* oDlg;
    bool pause_time;

    usec_t real_time_interval;
     
    usec_t real_time_now; 

    usec_t real_time_recorded;
     
    uint64_t timing_interval;

    // static callback functions
    static void windowCb( Fl_Widget* w, WorldGui* wg ); 
    static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
    static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
    static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
    static void fileExitCb( Fl_Widget* w, WorldGui* wg );
    static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
    static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
    static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
    static void pauseCb( Fl_Widget* w, WorldGui* wg );
    static void onceCb( Fl_Widget* w, WorldGui* wg );
    static void fasterCb( Fl_Widget* w, WorldGui* wg );
    static void slowerCb( Fl_Widget* w, WorldGui* wg );
    static void realtimeCb( Fl_Widget* w, WorldGui* wg );
    static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
    static void resetViewCb( Fl_Widget* w, WorldGui* wg );
    static void moreHelptCb( Fl_Widget* w, WorldGui* wg );
    
    // GUI functions
    bool saveAsDialog();
    bool closeWindowQuery();
    
    virtual void AddModel( Model* mod );
    
    void SetTimeouts();

  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 W,int H,const char*L=0);
    ~WorldGui();

    virtual void Redraw( void );

    virtual std::string ClockString() const;
    virtual bool Update();  
    virtual void Load( const std::string& filename );
    virtual void UnLoad();
    virtual bool Save( const char* filename );  
    virtual bool IsGUI() const { return true; };    
    virtual Model* RecentlySelectedModel() const;

    virtual void Start();
    virtual void Stop();
    
    usec_t RealTimeNow(void) const;
 
    void DrawBoundingBoxTree();
    
    Canvas* GetCanvas( void ) const { return canvas; } 

    void Show(); 

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

    bool IsTopView();
  };


  class StripPlotVis : public Visualizer
  {
  private:
     
    Model* mod;
    float* data;
    size_t len;
    size_t count;
    unsigned int index;
    float x,y,w,h,min,max;
    Color fgcolor, bgcolor;
     
  public:
    StripPlotVis( float x, float y, float w, float h, 
          size_t len, 
          Color fgcolor, Color bgcolor,
          const char* name, const char* wfname );
    virtual ~StripPlotVis();
    virtual void Visualize( Model* mod, Camera* cam );      
    void AppendValue( float value );
  };


  class PowerPack
  {
    friend class WorldGui;
    friend class Canvas;
     
  protected:
     
    class DissipationVis : public Visualizer
    {
    private:
      unsigned int columns, rows;
      meters_t width, height;
         
      std::vector<joules_t> cells;
         
      joules_t peak_value;
      double cellsize;
         
      static joules_t global_peak_value; 

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

      virtual ~DissipationVis();
      virtual void Visualize( Model* mod, Camera* cam );        
        
      void Accumulate( meters_t x, meters_t y, joules_t amount );
    } event_vis;
     

    StripPlotVis output_vis;
    StripPlotVis stored_vis;

    Model* mod;
    
    joules_t stored;
     
    joules_t capacity;
     
    bool charging;
     
    joules_t dissipated;
     
    // these are used to visualize the power draw
    usec_t last_time;
    joules_t last_joules;
    watts_t last_watts;

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

  public:
    PowerPack( Model* mod );
    ~PowerPack();
     
    void Visualize( Camera* cam );

    joules_t RemainingCapacity() const;
     
    void Add( joules_t j );
        
    void Subtract( joules_t j );
        
    void TransferTo( PowerPack* dest, joules_t amount );     

    double ProportionRemaining() const
    { return( stored / capacity ); }

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

      printf( "PowerPack %.2f/%.2f J\n", stored, capacity ); 
    }       
     
    joules_t GetStored() const;
    joules_t GetCapacity() const;
    joules_t GetDissipated() const;
    void SetCapacity( joules_t j );
    void SetStored( joules_t j );   

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

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

   
  class Model : public Ancestor
  {
    friend class Ancestor;
    friend class World;
    friend class World::Event;
    friend class WorldGui;
    friend class Canvas;
    friend class Block;
    friend class Region;
    friend class BlockGroup;
    friend class PowerPack;
    friend class Ray;
    friend class ModelFiducial;
        
  private:
    static uint32_t count;
    static std::map<id_t,Model*> modelsbyid;

    bool mapped;

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

    bool alwayson;

    BlockGroup blockgroup;
    int blocks_dl;

    int boundary;
        
  public:
    class cb_t
    {
    public:
      model_callback_t callback;
      void* arg;
            
      cb_t( model_callback_t cb, void* arg ) 
    : callback(cb), arg(arg) {}
            
      cb_t( world_callback_t cb, void* arg ) 
    : callback(NULL), arg(arg) { (void)cb; }
            
      cb_t() : callback(NULL), arg(NULL) {}
            
      bool operator<( const cb_t& other ) const
      {
    if( callback == other.callback )
      return( arg < other.arg );
    //else
    return ((void*)(callback)) < ((void*)(other.callback)); 
      }
         
      bool operator==( const cb_t& other ) const
      { return( callback == other.callback);  }         
    };
        
    class Flag
    {
    private:
      Color color;
      double size;
      int displaylist;

    public:
      void SetColor( const Color& col );
      void SetSize( double sz );
         
      Color GetColor(){ return color; }
      double GetSize(){ return size; }
         
      Flag( const Color& color, double size );
      Flag* Nibble( double portion );
        
      void Draw(  GLUquadric* quadric );
    };

    typedef enum {
      CB_FLAGDECR,
      CB_FLAGINCR,
      CB_GEOM,
      CB_INIT,
      CB_LOAD,
      CB_PARENT,
      CB_POSE,
      CB_SAVE,
      CB_SHUTDOWN,
      CB_STARTUP,
      CB_UPDATE,
      CB_VELOCITY,
      //CB_POSTUPDATE,
      __CB_TYPE_COUNT // must be the last entry: counts the number of types
    } callback_type_t;
        
  protected:
    std::vector<std::set<cb_t> > callbacks;
        
    Color color;
        
    bool data_fresh;

    bool disabled; 

    std::list<Visualizer*> cv_list;

    std::list<Flag*> flag_list;
        
    double friction;
        
    Geom geom;

    class GuiState
    {
    public:
      bool grid;
      bool move;
      bool nose;
      bool outline;
        
      GuiState();
      GuiState& Load( Worldfile* wf, int wf_entity );
    } gui;
     
    bool has_default_block;
                  
    uint32_t id;    
    usec_t interval; 
    usec_t interval_energy; 
    //    usec_t interval_pose; ///< time between updates of pose due to velocity in usec

    usec_t last_update; 
    bool log_state; 
    meters_t map_resolution;
    kg_t mass;

    Model* parent; 

    Pose pose;

    PowerPack* power_pack;

    std::list<PowerPack*> pps_charging;
        
    class RasterVis : public Visualizer
    {
    private:
      uint8_t* data;
      unsigned int width, height;
      meters_t cellwidth, cellheight;
      std::vector<point_t> pts;
      
    public:
      RasterVis();
      virtual ~RasterVis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
      
      void SetData( uint8_t* data, 
            unsigned int width, 
            unsigned int height,
            meters_t cellwidth,
            meters_t cellheight );
      
      int subs;     //< the number of subscriptions to this model
      int used;     //< the number of connections to this model
      
      void AddPoint( meters_t x, meters_t y );
      void ClearPts();
      
    } rastervis;
     
    bool rebuild_displaylist; 
    std::string say_string;   
        
    bool stack_children; 

    bool stall; 
    int subs;    

    bool thread_safe;
     
    class TrailItem 
    {                                                                                           
    public:
      usec_t time;
      Pose pose;
      Color color;
        
      TrailItem() 
    : time(0), pose(), color(){}
         
      //TrailItem( usec_t time, Pose pose, Color color ) 
      //: time(time), pose(pose), color(color){}
    };
    
    std::vector<TrailItem> trail;

    unsigned int trail_index;

    static unsigned int trail_length;
     
    static uint64_t trail_interval;
     
    void UpdateTrail();

    //model_type_t type;  
    const std::string type;
    unsigned int event_queue_num; 
    bool used;   
    
    watts_t watts;
     
    watts_t watts_give;
    
    watts_t watts_take;
    
    Worldfile* wf;
    int wf_entity;
    World* world; // pointer to the world in which this model exists
    WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode

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

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

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

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

      Visibility& Load( Worldfile* wf, int wf_entity );
      void Save( Worldfile* wf, int wf_entity );
    } vis;
     
    usec_t GetUpdateInterval() const { return interval; }
    usec_t GetEnergyInterval() const { return interval_energy; }
    //    usec_t GetPoseInterval() const { return interval_pose; }
     
    void Rasterize( uint8_t* data, 
            unsigned int width, unsigned int height,
            meters_t cellwidth, meters_t cellheight );
    
  private: 
    explicit Model(const Model& original);

    Model& operator=(const Model& original);

  protected:

    void RegisterOption( Option* opt );

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

    void Map( unsigned int layer );

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

    void UnMap( unsigned int layer );

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

    void MapWithChildren( unsigned int layer );
    void UnMapWithChildren( unsigned int layer );
  
    // Find the root model, and map/unmap the whole tree.
    void MapFromRoot( unsigned int layer );
    void UnMapFromRoot( unsigned int layer );

    RaytraceResult Raytrace( const Pose &pose,
                 const meters_t range, 
                 const ray_test_func_t func,
                 const void* arg,
                 const bool ztest = true );
  
    void Raytrace( const Pose &pose,
           const meters_t range, 
           const radians_t fov, 
           const ray_test_func_t func,
           const void* arg,
           RaytraceResult* samples,
           const uint32_t sample_count,
           const bool ztest = true  );
  
    RaytraceResult Raytrace( const radians_t bearing,            
                 const meters_t range,
                 const ray_test_func_t func,
                 const void* arg,
                 const bool ztest = true );
  
    void Raytrace( const radians_t bearing,              
           const meters_t range,
           const radians_t fov,
           const ray_test_func_t func,
           const void* arg,
           RaytraceResult* samples,
           const uint32_t sample_count,
           const bool ztest = true );
  
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void UpdateCharge();
        
    static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }

    void CallUpdateCallbacks( void );

    meters_t ModelHeight() const;

    void DrawBlocksTree();
    virtual void DrawBlocks();
    void DrawBoundingBox();
    void DrawBoundingBoxTree();
    virtual void DrawStatus( Camera* cam );
    void DrawStatusTree( Camera* cam );
  
    void DrawOriginTree();
    void DrawOrigin();
  
    void PushLocalCoords();
    void PopCoords();
  
    void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
    
    virtual void DrawPicker();
    virtual void DataVisualize( Camera* cam );  
    virtual void DrawSelected(void);
    
    void DrawTrailFootprint();
    void DrawTrailBlocks();
    void DrawTrailArrows();
    void DrawGrid();
    //  void DrawBlinkenlights();
    void DataVisualizeTree( Camera* cam );
    void DrawFlagList();
    void DrawPose( Pose pose );
    
  public:
    virtual void PushColor( Color col ){ world->PushColor( col ); } 
    virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }    
    virtual void PopColor() { world->PopColor(); }
    
    PowerPack* FindPowerPack() const;
    
    //void RecordRenderPoint( GSList** head, GSList* link, 
    //                  unsigned int* c1, unsigned int* c2 );

    void PlaceInFreeSpace( meters_t xmin, meters_t xmax, 
               meters_t ymin, meters_t ymax );
    
    std::string PoseString()
    { return pose.String(); }
    
    static Model* LookupId( uint32_t id )
    { return modelsbyid[id]; }
     
    Model( World* world, 
       Model* parent = NULL, 
       const std::string& type = "model",
       const std::string& name = "" );
     
    virtual ~Model();
        
     Model() 
        : mapped(false), alwayson(false), blocks_dl(0),
          boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(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)
     {}
        
    void Say( const std::string& str );
     
    void AddVisualizer( Visualizer* custom_visual, bool on_by_default );

    void RemoveVisualizer( Visualizer* custom_visual );

    void BecomeParentOf( Model* child );

    void Load( Worldfile* wf, int wf_entity )
    {
      SetWorldfile( wf, wf_entity );
      Load(); // call virtual load
    }
    
    void SetWorldfile( Worldfile* wf, int wf_entity )
    { this->wf = wf; this->wf_entity = wf_entity; }
    
    virtual void Load();
    
    virtual void Save();
    
    void InitControllers();

    void AddFlag(  Flag* flag );
    void RemoveFlag( Flag* flag );
    
    void PushFlag( Flag* flag );
    Flag* PopFlag();
    
    unsigned int GetFlagCount() const { return flag_list.size(); }
    
    void Disable(){ disabled = true; };

    void Enable(){ disabled = false; };
  
    void LoadControllerModule( const char* lib );
    
    void NeedRedraw();
        
    void Redraw();

    void LoadBlock( Worldfile* wf, int entity );

    Block*  AddBlockRect( meters_t x, meters_t y, 
                  meters_t dx, meters_t dy, 
                  meters_t dz );
        
    void ClearBlocks();
  
    Model* Parent() const { return this->parent; }

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

    Pose GetGlobalPose() const;
    
    Velocity GetGlobalVelocity()  const;
    
    /* set the velocity of a model in the global coordinate system */
    void SetGlobalVelocity( const Velocity& gvel );
    
    void Subscribe();
    
    void Unsubscribe();
    
    void SetGlobalPose(  const Pose& gpose );
    
    //    void VelocityEnable();

    //void VelocityDisable();

    void SetPose(  const Pose& pose );
    
    void AddToPose(  const Pose& pose );
    
    void AddToPose(  double dx, double dy, double dz, double da );
    
    void SetGeom(  const Geom& src );
  
    void SetFiducialReturn(  int fid );
  
    int GetFiducialReturn()  const { return vis.fiducial_return; }
  
    void SetFiducialKey(  int key );
    
    Color GetColor() const { return color; }
     
    uint32_t GetId()  const { return id; }
     
    kg_t GetTotalMass() const;
     
    kg_t GetMassOfChildren() const;

    int SetParent( Model* newparent);
    
    Geom GetGeom() const { return geom; }
    
    Pose GetPose() const { return pose; }
    
    
    // guess what these do?
    void SetColor( Color col );
    void SetMass( kg_t mass );
    void SetStall( bool stall );
    void SetGravityReturn( bool val );
    void SetGripperReturn( bool val );
    void SetStickyReturn( bool val );
    void SetRangerReturn( double val );
    void SetObstacleReturn( bool val );
    void SetBlobReturn( bool val );
    void SetRangerReturn( bool val );
    void SetBoundary( bool val );
    void SetGuiNose( bool val );
    void SetGuiMove( bool val );
    void SetGuiGrid( bool val );
    void SetGuiOutline( bool val );
    void SetWatts( watts_t watts );
    void SetMapResolution( meters_t res );
    void SetFriction( double friction );
    
    bool DataIsFresh() const { return this->data_fresh; }
        
    /* attach callback functions to data members. The function gets
       called when the member is changed using SetX() accessor method */
        
    void AddCallback( callback_type_t type, 
              model_callback_t cb, 
              void* user );
        
    int RemoveCallback( callback_type_t type,
            model_callback_t callback );
        
    int CallCallbacks(  callback_type_t type );
        
        
    virtual void Print( char* prefix ) const;
    virtual const char* PrintWithPose() const;
    
    Pose GlobalToLocal( const Pose& pose ) const;
     
    Pose LocalToGlobal( const Pose& pose ) const
    {  
      return( ( GetGlobalPose() + geom.pose ) + pose );
    }
    
    std::vector<point_int_t>  LocalToPixels( const std::vector<point_t>& local ) const;
        
    point_t LocalToGlobal( const point_t& pt) const;        

    Model* GetUnsubscribedModelOfType( const std::string& type ) const;
    
    Model* GetUnusedModelOfType( const std::string& type );
  
    bool Stalled() const { return this->stall; }
     
    unsigned int GetSubscriptionCount() const { return subs; }

    bool HasSubscribers() const { return( subs > 0 ); }  

    static std::map< std::string, creator_t> name_map;   

    //      class Neighbors
    //      {
    //          Model *left, *right, *up, *down;
    //      public:
    //          Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {}
    //      } nbors; // instance

                            
  };


  // BLOBFINDER MODEL --------------------------------------------------------


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

    class Vis : public Visualizer 
    {
    private:
      //static Option showArea;
    public:
      Vis( World* world );
      virtual ~Vis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
    } vis;

  private:
    std::vector<Blob> blobs;
    std::vector<Color> colors;

    // predicate for ray tracing
    static bool BlockMatcher( Block* testblock, Model* finder );

  public:
    radians_t fov;
    radians_t pan;
    meters_t range;
    unsigned int scan_height;
    unsigned int scan_width;
     
    // constructor
    ModelBlobfinder( World* world,
             Model* parent,
             const std::string& type );
    // destructor
    ~ModelBlobfinder();
    
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Load();
        
    Blob* GetBlobs( unsigned int* count )
    { 
      if( count ) *count = blobs.size();
      return &blobs[0];
    }

    std::vector<Blob> GetBlobs() const { return blobs; }

    void AddColor( Color col );

    void RemoveColor( Color col );

    void RemoveAllColors();
  };



  

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

  protected:
    virtual void DrawBlocks();

  private:
    bool m_IsOn;
  };

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


  class ModelGripper : public Model
  {
  public:

    enum paddle_state_t {
      PADDLE_OPEN = 0, // default state
      PADDLE_CLOSED, 
      PADDLE_OPENING,
      PADDLE_CLOSING,
    };
     
    enum lift_state_t {
      LIFT_DOWN = 0, // default state
      LIFT_UP, 
      LIFT_UPPING, // verbed these to match the paddle state
      LIFT_DOWNING, 
    };
     
    enum cmd_t {
      CMD_NOOP = 0, // default state
      CMD_OPEN, 
      CMD_CLOSE,
      CMD_UP, 
      CMD_DOWN    
    };
     
     
    struct config_t
    {
      Size paddle_size; 
      paddle_state_t paddles;
      lift_state_t lift;        
      double paddle_position; 
      double lift_position; 
      Model* gripped;
      bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
      double close_limit; 
      bool autosnatch; 
      double break_beam_inset[2]; 
      Model* beam[2]; 
      Model* contact[2]; 
    };
     
  private:
    virtual void Update();
    virtual void DataVisualize( Camera* cam );
     
    void FixBlocks();
    void PositionPaddles();
    void UpdateBreakBeams();
    void UpdateContacts();

    config_t cfg;
    cmd_t cmd;
     
    Block* paddle_left;
    Block* paddle_right;

    static Option showData;

  public:    
    static const Size size;

    // constructor
    ModelGripper( World* world,
          Model* parent,
          const std::string& type );
    // destructor
    virtual ~ModelGripper();
  
    virtual void Load();
    virtual void Save();

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


  // \todo BUMPER MODEL --------------------------------------------------------

  //   typedef struct
  //   {
  //     Pose pose;
  //     meters_t length;
  //   } bumper_config_t;

  //   typedef struct
  //   {
  //     Model* hit;
  //     point_t hit_point;
  //   } bumper_sample_t;


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

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

  private:
    // if neighbor is visible, add him to the fiducial scan
    void AddModelIfVisible( Model* him );

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

    static Option showData;
    static Option showFov;
     
    std::vector<Fiducial> fiducials;
        
  public:       
    ModelFiducial( World* world, 
           Model* parent,
           const std::string& type );
    virtual ~ModelFiducial();
    
    virtual void Load();
    void Shutdown( void );

    meters_t max_range_anon;
    meters_t max_range_id; 
    meters_t min_range; 
    radians_t fov; 
    radians_t heading; 
    int key; 
    bool ignore_zloc;  
        
    std::vector<Fiducial>& GetFiducials() { return fiducials; }
        
    Fiducial* GetFiducials( unsigned int* count )
    {
      if( count ) *count = fiducials.size();
      return &fiducials[0];
    }
  };
    
    
  // RANGER MODEL --------------------------------------------------------
    
  class ModelRanger : public Model
  {
  public:
  public:
    ModelRanger( World* world, Model* parent,
         const std::string& type );
    virtual ~ModelRanger();
        
    virtual void Load();
    virtual void Print( char* prefix ) const;
        
    class Vis : public Visualizer 
    {
    public:
      static Option showArea;
      static Option showStrikes;
      static Option showFov;
      static Option showBeams;
      static Option showTransducers;        
            
      Vis( World* world );      
      virtual ~Vis( void ){} 
      virtual void Visualize( Model* mod, Camera* cam );
    } vis;
        
    class Sensor
    {       
    public:
      Pose pose; 
      Size size;
      Bounds range;
      radians_t fov;
      unsigned int sample_count;
      Color col;
            
      std::vector<meters_t> ranges;
      std::vector<double> intensities;
      std::vector<double> bearings;
            
      Sensor() : pose( 0,0,0,0 ), 
         size( 0.02, 0.02, 0.02 ), // teeny transducer
         range( 0.0, 5.0 ),
         fov( 0.1 ), 
         sample_count(1),
         col( 0,1,0,0.3 ),
         ranges(),
         intensities(),
         bearings()
      {}
            
      void Update( ModelRanger* rgr );          
      void Visualize( Vis* vis, ModelRanger* rgr ) const;
      std::string String() const;           
      void Load( Worldfile* wf, int entity );
    };

    const std::vector<Sensor>& GetSensors() const
    { return sensors; }

    std::vector<Sensor>& GetSensorsMutable() 
    { return sensors; }
     
    void LoadSensor( Worldfile* wf, int entity );
        
  private:
    std::vector<Sensor> sensors;        
    
  protected:
        
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();      
  };
    
  // BLINKENLIGHT MODEL ----------------------------------------------------
  class ModelBlinkenlight : public Model
  {
  private:
    double dutycycle;
    bool enabled;
    msec_t period;
    bool on;

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

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

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

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

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

    bool _valid_vertexbuf_cache;
    ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
    
    int _width;         //width of buffer
    int _height;        //height of buffer
    static const int _depth = 4;
    
    int _camera_quads_size;
    GLfloat* _camera_quads;
    GLubyte* _camera_colors;
    
    static Option showCameraData;
    
    PerspectiveCamera _camera;
    double _yaw_offset; //position camera is mounted at
    double _pitch_offset;
        
    bool GetFrame();
    
  public:
    ModelCamera( World* world,
         Model* parent,
         const std::string& type ); 
      
    ~ModelCamera();
  
    virtual void Load();
    
    virtual void Update();
    
    //virtual void Draw( uint32_t flags, Canvas* canvas );
    
    virtual void DataVisualize( Camera* cam );
    
    int getWidth( void ) const { return _width; }
    
    int getHeight( void ) const { return _height; }
    
    const PerspectiveCamera& getCamera( void ) const { return _camera; }
    
    const GLfloat* FrameDepth() const { return _frame_data; }
    
    const GLubyte* FrameColor() const { return _frame_color_data; }
    
    void setPitch( double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
    
    void setYaw( double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
  };

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

  class ModelPosition : public Model
  {
    friend class Canvas;

  public:
    typedef enum
      { CONTROL_ACCELERATION,
    CONTROL_VELOCITY, 
    CONTROL_POSITION 
      } ControlMode;
     
    typedef enum
      { LOCALIZATION_GPS, 
    LOCALIZATION_ODOM 
      } LocalizationMode;
     
    typedef enum
      { DRIVE_DIFFERENTIAL, 
    DRIVE_OMNI, 
    DRIVE_CAR 
      } DriveMode;
     
  private:
    Velocity velocity;
    Pose goal;
    ControlMode control_mode;
    DriveMode drive_mode;    
    LocalizationMode localization_mode; 
    Velocity integration_error; 
    double wheelbase;
    
  public:
    Bounds acceleration_bounds[4];
    
    Bounds velocity_bounds[4];

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

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

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

    class Waypoint
    {
    public:
      Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
      Waypoint( const Pose& pose, Color color ) ;
      Waypoint();
      void Draw() const;
        
      Pose pose;
      Color color;
    };
     
    std::vector<Waypoint> waypoints;

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

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

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

    // localization state
    Pose est_pose; 
    Pose est_pose_error; 
    Pose est_origin; 
  };


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

  class ModelActuator : public Model
  {
  public:
    typedef enum
      { CONTROL_VELOCITY,
    CONTROL_POSITION
      } ControlMode;
  
    typedef enum
      { TYPE_LINEAR,
    TYPE_ROTATIONAL
      } ActuatorType;
  
  private:
    double goal; //< the current velocity or pose to reach depending on the value of control_mode
    double pos;
    double max_speed;
    double min_position;
    double max_position;
    double start_position;
    double cosa;
    double sina;
    ControlMode control_mode;
    ActuatorType actuator_type;
    point3_t axis;
  
    Pose InitialPose;
  public:  
    // constructor
    ModelActuator( World* world,
           Model* parent,
           const std::string& type );
    // destructor
    ~ModelActuator();
  
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Load();
  
    void SetSpeed( double speed );
  
    double GetSpeed() const {return goal;}
  
    void GoTo( double pose );
  
    double GetPosition() const {return pos;};
    double GetMaxPosition() const {return max_position;};
    double GetMinPosition() const {return min_position;};
        
    ActuatorType GetType() const { return actuator_type; }
    point3_t GetAxis() const { return axis; }
  };


}; // end namespace stg

#endif