Stage

• Frontpage
• Release
• Authors
• Contents

User

• Installation
• Help

  ---

• World
• WorldGUI
• Model
  • blobfinder
  • camera
  • fiducial
  • laser
  • position
  • ranger

Player

libstageplugin

Developer

libstage

Online

Homepage
FAQ (Wiki)
Download
Project
Bugs
Mailing Lists

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-2008 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
 *
 */

#include <unistd.h>
#include <stdint.h> // for portable int types eg. uint32_t
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
#include <iostream>
#include <vector>

// we use GLib's data structures extensively. Perhaps we'll move to
// C++ STL types to lose this dependency one day.
#include <glib.h> 

// 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
//#include <FL/glut.H>

#ifdef __APPLE__
#include <OpenGL/glu.h>
#else
#include <GL/glu.h>
#endif 

#include "option.hh"

namespace Stg 
{
    // foreward declare
    class StgCanvas;
    class Worldfile;
    class StgWorld;
    class StgWorldGui;
    class StgModel;
    class FileManager;
    class OptionsDlg;
    class StgCamera;
  
    void Init( int* argc, char** argv[] );

    bool InitDone();
  
  typedef enum {
     MODEL_TYPE_PLAIN = 0,
     MODEL_TYPE_LASER,
     MODEL_TYPE_FIDUCIAL,
     MODEL_TYPE_RANGER,
     MODEL_TYPE_POSITION,
     MODEL_TYPE_BLOBFINDER,
     MODEL_TYPE_BLINKENLIGHT,
     MODEL_TYPE_CAMERA,
     MODEL_TYPE_COUNT // must be the last entry, to count the number of
     // types
  } stg_model_type_t;


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

    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-2008 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 uint32_t TOKEN_MAX = 64;

    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 )
    {
        //optimized version of return( atan2(sin(a), cos(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); }

    // forward declare
    class StgModel;

    typedef enum {
        STG_IMAGE_FORMAT_PNG,
        STG_IMAGE_FORMAT_JPG
    } stg_image_format_t;

    enum { FiducialNone = 0 };

    typedef uint32_t stg_id_t;

    typedef double stg_meters_t;

    typedef double stg_radians_t;

    typedef struct timeval stg_time_t;

    typedef unsigned long stg_msec_t;

    typedef uint64_t stg_usec_t;

    typedef double stg_kg_t; // Kilograms (mass)

    typedef double stg_joules_t;

    typedef double stg_watts_t;

    typedef uint32_t stg_bool_t;

    //typedef double stg_friction_t;

    typedef uint32_t stg_color_t;

    stg_color_t stg_color_pack( double r, double g, double b, double a );

    void stg_color_unpack( stg_color_t col, 
            double* r, double* g, double* b, double* a );

    //typedef int stg_obstacle_return_t;

    //typedef int stg_blob_return_t;

    //typedef int stg_fiducial_return_t;

    //typedef int stg_ranger_return_t;

    //typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t;

    typedef struct 
    {
        stg_meters_t x, y, z;
    } stg_size_t;

    typedef struct
    {
        stg_meters_t x, y, z; //< location in 3 axes
        stg_radians_t a; //< rotation about the z axis. 
    } stg_pose_t;

    typedef stg_pose_t stg_velocity_t;

    typedef struct
    {
        stg_pose_t pose; //< position
        stg_size_t size; //< extent
    } stg_geom_t;

    typedef struct
    {
        double min; //< smallest value in range
        double max; //< largest value in range
    } stg_bounds_t;

    typedef struct
    {
        stg_bounds_t x; //< volume extent along x axis
        stg_bounds_t y; //< volume extent along y axis
        stg_bounds_t z; //< volume extent along z axis 
    } stg_bounds3d_t;

    typedef struct
    {
        stg_meters_t min; //< smallest value in range
        stg_meters_t max; //< largest value in range
    } stg_range_bounds_t;

    typedef struct
    {
        stg_bounds_t x, y, z;
    } stg_bbox3d_t;

    typedef struct
    {
        stg_bounds_t range; //< min and max range of sensor
        stg_radians_t angle; //< width of viewing angle of sensor
    } stg_fov_t;


    typedef struct
    {
        stg_meters_t x, y;
    } stg_point_t;

    typedef struct
    {
        float x, y, z;
    } stg_vertex_t;

    typedef struct
    {
        float x, y, z, r, g, b, a;
    } stg_colorvertex_t;

    typedef struct
    {
        stg_meters_t x, y, z;
    } stg_point3_t;

    typedef struct
    {
        int32_t x,y;
    } stg_point_int_t;

    stg_point_t* stg_points_create( size_t count );

    void stg_points_destroy( stg_point_t* pts );

    stg_point_t* stg_unit_square_points_create();


    typedef int(*stg_line3d_func_t)(int32_t x, int32_t y, int32_t z,
            void* arg );


    int stg_line_3d( int32_t x, int32_t y, int32_t z,
            int32_t dx, int32_t dy, int32_t dz,
            stg_line3d_func_t visit_voxel,
            void* arg );

    int stg_polygon_3d( stg_point_int_t* pts, 
            unsigned int pt_count, 
            stg_line3d_func_t visit_voxel, 
            void* arg );

  stg_pose_t new_pose( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a );

  stg_pose_t random_pose( stg_meters_t xmin, stg_meters_t xmax, 
                                  stg_meters_t ymin, stg_meters_t ymax );
  
    const uint32_t STG_MOVE_TRANS = (1 << 0);
    const uint32_t STG_MOVE_ROT   = (1 << 1);
    const uint32_t STG_MOVE_SCALE = (1 << 2);

    typedef uint32_t stg_movemask_t;

    const char STG_MP_PREFIX[] =             "_mp_";
    const char STG_MP_POSE[] =               "_mp_pose";
    const char STG_MP_VELOCITY[] =           "_mp_velocity";
    const char STG_MP_GEOM[] =               "_mp_geom";
    const char STG_MP_COLOR[] =              "_mp_color";
    const char STG_MP_WATTS[] =              "_mp_watts";
    const char STG_MP_FIDUCIAL_RETURN[] =    "_mp_fiducial_return";
    const char STG_MP_LASER_RETURN[] =       "_mp_laser_return";
    const char STG_MP_OBSTACLE_RETURN[] =    "_mp_obstacle_return";
    const char STG_MP_RANGER_RETURN[] =      "_mp_ranger_return";
    const char STG_MP_GRIPPER_RETURN[] =     "_mp_gripper_return";
    const char STG_MP_MASS[] =               "_mp_mass";


    typedef enum 
    {
        LaserTransparent, 
        LaserVisible, 
        LaserBright  
    } stg_laser_return_t;


    namespace Draw
    {
        typedef struct {
            double x, y, z;
        } vertex_t;

        typedef struct {
            vertex_t min, max;
        } bounds3_t;

        typedef enum {
            STG_D_DRAW_POINTS,
            STG_D_DRAW_LINES,
            STG_D_DRAW_LINE_STRIP,
            STG_D_DRAW_LINE_LOOP,
            STG_D_DRAW_TRIANGLES,
            STG_D_DRAW_TRIANGLE_STRIP,
            STG_D_DRAW_TRIANGLE_FAN,
            STG_D_DRAW_QUADS,
            STG_D_DRAW_QUAD_STRIP,
            STG_D_DRAW_POLYGON,
            STG_D_PUSH,
            STG_D_POP,
            STG_D_ROTATE,
            STG_D_TRANSLATE,
        } type_t;

        typedef struct
        {
            type_t type;
        } hdr_t;

        typedef hdr_t push_t;
        typedef hdr_t pop_t;

        typedef struct
        {
            type_t type;
            size_t vert_count;
            vertex_t verts[];
        } draw_t;

        typedef struct
        {
            type_t type;
            vertex_t vector;
        } translate_t;

        typedef  struct
        {
            type_t type;
            vertex_t vector;
            stg_radians_t angle;
        } rotate_t;

        draw_t* create( type_t type,  
                vertex_t* verts,
                size_t vert_count );

        void destroy( draw_t* d );
    } // end namespace draw


    // MACROS ------------------------------------------------------
    // Some useful macros


    stg_color_t stg_lookup_color(const char *name);

    void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 );

    stg_pose_t pose_sum( stg_pose_t p1, stg_pose_t p2 );

    // PRETTY PRINTING -------------------------------------------------

    void stg_print_err( const char* err );
    void stg_print_geom( stg_geom_t* geom );
    void stg_print_pose( stg_pose_t* pose );
    void stg_print_velocity( stg_velocity_t* vel );

//  const uint32_t STG_SHOW_BLOCKS =     (1<<0);
//  const uint32_t STG_SHOW_DATA =       (1<<1);
//  const uint32_t STG_SHOW_GEOM =       (1<<2);
//  const uint32_t STG_SHOW_GRID =       (1<<3);
//  const uint32_t STG_SHOW_OCCUPANCY =  (1<<4);
//  const uint32_t STG_SHOW_TRAILS =     (1<<5);
//  const uint32_t STG_SHOW_FOLLOW =     (1<<6);
//  const uint32_t STG_SHOW_CLOCK =      (1<<7);
//  const uint32_t STG_SHOW_QUADTREE =   (1<<8);
//  const uint32_t STG_SHOW_ARROWS =     (1<<9);
//  const uint32_t STG_SHOW_FOOTPRINT =  (1<<10);
//  const uint32_t STG_SHOW_BLOCKS_2D =  (1<<10);
//  const uint32_t STG_SHOW_TRAILRISE =  (1<<11);
//  const uint32_t STG_SHOW_STATUS =     (1<<12);

    // forward declare
    class StgWorld;
    class StgModel;


  typedef StgModel* (*stg_creator_t)( StgWorld*, StgModel* );
  
    typedef struct 
    {
        const char* token;
        stg_creator_t creator;
    } stg_typetable_entry_t;
  
  extern stg_typetable_entry_t typetable[MODEL_TYPE_COUNT];

  void RegisterModel( stg_model_type_t type, 
                             const char* name, 
                             stg_creator_t creator );

  void RegisterModels();

    void gl_draw_grid( stg_bounds3d_t vol );
    void gl_pose_shift( stg_pose_t* pose );
    void gl_coord_shift( double x, double y, double z, double a  );

    class StgFlag
    {
        public:
            stg_color_t color;
            double size;

            StgFlag( stg_color_t color, double size );
            StgFlag* Nibble( double portion );
    };

    void gl_draw_string( float x, float y, float z, const char *string);
    void gl_speech_bubble( float x, float y, float z, const char* str );
    void gl_draw_octagon( float w, float h, float m );

    void stg_block_list_scale( GList* blocks, 
            stg_size_t* size );
    void stg_block_list_destroy( GList* list );


    typedef int(*stg_model_callback_t)(StgModel* mod, void* user );

    // return val, or minval if val < minval, or maxval if val > maxval
    double constrain( double val, double minval, double maxval );


    typedef struct
    {
        stg_model_callback_t callback;
        void* arg;
    } stg_cb_t;

    stg_cb_t* cb_create( stg_model_callback_t callback, void* arg );
    void cb_destroy( stg_cb_t* cb );

    typedef struct
    {
        stg_pose_t pose;
        stg_size_t size;
    } stg_rotrect_t; // rotated rectangle

    void stg_rotrects_normalize( stg_rotrect_t* rects, int num );

    int stg_rotrects_from_image_file( const char* filename, 
            stg_rotrect_t** rects,
            unsigned int* rect_count,
            unsigned int* widthp, 
            unsigned int* heightp );

    // /** matching function should return 0 iff the candidate block is
    //     acceptable */
    class StgBlock;

    typedef bool (*stg_block_match_func_t)(StgBlock* candidate, StgModel* finder, const void* arg );

    // TODO - some of this needs to be implemented, the rest junked.

    /*   //  -------------------------------------------------------------- */

    /*   // standard energy consumption of some devices in W. */
    /*   // */
    /*   // The MOTIONKG value is a hack to approximate cost of motion, as */
    /*   // Stage does not yet have an acceleration model. */
    /*   // */
    /* #define STG_ENERGY_COST_LASER 20.0 // 20 Watts! (LMS200 - from SICK web site) */
    /* #define STG_ENERGY_COST_FIDUCIAL 10.0 // 10 Watts */
    /* #define STG_ENERGY_COST_RANGER 0.5 // 500mW (estimate) */
    /* #define STG_ENERGY_COST_MOTIONKG 10.0 // 10 Watts per KG when moving  */
    /* #define STG_ENERGY_COST_BLOB 4.0 // 4W (estimate) */

    /*   typedef struct */
    /*   { */
    /*     stg_joules_t joules; // current energy stored in Joules/1000 */
    /*     stg_watts_t watts; // current power expenditure in mW (mJoules/sec) */
    /*     int charging; // 1 if we are receiving energy, -1 if we are */
    /*     // supplying energy, 0 if we are neither charging nor */
    /*     // supplying energy. */
    /*     stg_meters_t range; // the range that our charging probe hit a charger */
    /*   } stg_energy_data_t; */

    /*   typedef struct */
    /*   { */
    /*     stg_joules_t capacity; // maximum energy we can store (we start fully charged) */
    /*     stg_meters_t probe_range; // the length of our recharge probe */
    /*     //stg_pose_t probe_pose; // TODO - the origin of our probe */

    /*     stg_watts_t give_rate; // give this many Watts to a probe that hits me (possibly 0) */

    /*     stg_watts_t trickle_rate; // this much energy is consumed or */
    /*     // received by this device per second as a */
    /*     // baseline trickle. Positive values mean */
    /*     // that the device is just burning energy */
    /*     // stayting alive, which is appropriate */
    /*     // for most devices. Negative values mean */
    /*     // that the device is receiving energy */
    /*     // from the environment, simulating a */
    /*     // solar cell or some other ambient energy */
    /*     // collector */

    /*   } stg_energy_config_t; */


    /*   // BLINKENLIGHT ------------------------------------------------------------ */

    /*   // a number of milliseconds, used for example as the blinkenlight interval */
    /* #define STG_LIGHT_ON UINT_MAX */
    /* #define STG_LIGHT_OFF 0 */

    //typedef int stg_interval_ms_t;


    /*   // TOKEN ----------------------------------------------------------------------- */
    /*   // token stuff for parsing worldfiles - this may one day replace
         the worldfile c++ code */

    /* #define CFG_OPEN '(' */
    /* #define CFG_CLOSE ')' */
    /* #define STR_OPEN '\"' */
    /* #define STR_CLOSE '\"' */
    /* #define TPL_OPEN '[' */
    /* #define TPL_CLOSE ']' */

    /*   typedef enum { */
    /*     STG_T_NUM = 0, */
    /*     STG_T_BOOLEAN, */
    /*     STG_T_MODELPROP, */
    /*     STG_T_WORLDPROP, */
    /*     STG_T_NAME, */
    /*     STG_T_STRING, */
    /*     STG_T_KEYWORD, */
    /*     STG_T_CFG_OPEN, */
    /*     STG_T_CFG_CLOSE, */
    /*     STG_T_TPL_OPEN, */
    /*     STG_T_TPL_CLOSE, */
    /*   } stg_token_type_t; */




    /* typedef struct stg_token  */
    /* { */
    /*   char* token; ///< the text of the token */
    /*   stg_token_type_t type; ///< the type of the token */
    /*   unsigned int line; ///< the line on which the token appears */

    /*   struct stg_token* next; ///< linked list support */
    /*   struct stg_token* child; ///< tree support */

    /* } stg_token_t; */

    /*   stg_token_t* stg_token_next( stg_token_t* tokens ); */
    /*   /// print [token] formatted for a human reader, with a string [prefix] */
    /*   void stg_token_print( char* prefix,  stg_token_t* token ); */

    /*   /// print a token array suitable for human reader */
    /*   void stg_tokens_print( stg_token_t* tokens ); */
    /*   void stg_tokens_free( stg_token_t* tokens ); */

    /*   /// create a new token structure from the arguments */
    /*   stg_token_t* stg_token_create( const char* token, stg_token_type_t type, int line ); */

    /*   /// add a token to a list */
    /*   stg_token_t* stg_token_append( stg_token_t* head, */
    /*               char* token, stg_token_type_t type, int line ); */

    /*   const char* stg_token_type_string( stg_token_type_t type ); */

    /*   const char* stg_model_type_string( stg_model_type_t type ); */

    /*   //  functions for parsing worldfiles */
    /*   stg_token_t* stg_tokenize( FILE* wf ); */
    /*   //StgWorld* sc_load_worldblock( stg_client_t* cli, stg_token_t** tokensptr ); */
    /*   //stg_model_t* sc_load_modelblock( StgWorld* world, stg_model_t* parent, */
    /*   //            stg_token_t** tokensptr ); */




    //#ifdef __cplusplus
    //}
    //#endif 


    // list iterator macros
#define LISTFUNCTION( LIST, TYPE, FUNC ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data);

#define LISTMETHOD( LIST, TYPE, METHOD ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD();

#define LISTFUNCTIONARG( LIST, TYPE, FUNC, ARG ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data, ARG);

#define LISTMETHODARG( LIST, TYPE, METHOD, ARG ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD(ARG);


    // 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 StgBlock;
    class StgModel;


    stg_msec_t stg_realtime();
    stg_msec_t stg_realtime_since_start( void );



    // ANCESTOR CLASS

    typedef int (*stg_model_callback_t)( StgModel* mod, void* user );


    class StgAncestor
{
    friend class StgCanvas; // allow StgCanvas access to our private members

    protected:
    GList* children;

    char* token;
    bool debug;

    public:
    
    void ForEachDescendant( stg_model_callback_t func, void* arg );

  unsigned int child_type_counts[MODEL_TYPE_COUNT];

    StgAncestor();
    virtual ~StgAncestor();

  //    unsigned int GetNumChildrenOfType( const char* typestr );
  //    void IncrementNumChildrenOfType( const char* typestr );

    virtual void AddChild( StgModel* mod );
    virtual void RemoveChild( StgModel* mod );
    virtual stg_pose_t GetGlobalPose();

    const char* Token()
    { return this->token; }

  void SetToken( const char* str )
  {
     this->token = strdup( str );
  }
  
//  // PURE VIRTUAL - descendents must implement
//  virtual void PushColor( stg_color_t col ) = 0;
//  virtual void PushColor( double r, double g, double b, double a ) = 0;
//  virtual void PopColor() = 0; // does nothing
};


typedef struct 
{
    int enabled;
    stg_pose_t pose;
    stg_meters_t size; 
    stg_color_t color;
    stg_msec_t period; 
    double duty_cycle; 
} stg_blinkenlight_t;

typedef struct
{
    uint32_t counter;
    GSList** lists;
} stg_bigblock_t;

typedef struct
{
    StgWorld* world;
    StgBlock* block;
} stg_render_info_t;


class StgBlockGrid 
{
    private:
        //stg_bigblock_t* map;
        GSList** cells;

        //GTrashStack* trashstack;
        uint32_t width, height;// bwidth, bheight;

    public:
        //uint32_t numbits;
        StgBlockGrid( uint32_t width, uint32_t height );
        ~StgBlockGrid();
        void AddBlock( uint32_t x, uint32_t y, StgBlock* block );
        void RemoveBlock( uint32_t x, uint32_t y, StgBlock* block );
        GSList* GetList( uint32_t x, uint32_t y );
        void GlobalRemoveBlock( StgBlock* block );
        void Draw( bool drawall );

        //uint32_t BigBlockOccupancy( uint32_t bbx, uint32_t bby );
};

typedef struct
{
    stg_pose_t pose; 
    stg_meters_t range; 
    StgBlock* block; 
} stg_raytrace_sample_t;


const uint32_t INTERVAL_LOG_LEN = 32;

class Region;
class SuperRegion;
    
class StgWorld : public StgAncestor
{
    friend class StgModel; // allow access to private members
    friend class StgBlock;
    friend class StgTime;
    friend class StgCanvas;
  
private:
  static GList* world_list;
  
  GQueue* csstack;
    
  void PushPose();  
  void PopPose();
  stg_pose_t* PeekPose();
  void ShiftPose( stg_pose_t* pose );

  void DrawPose();

  static bool quit_all; 
  static unsigned int next_id; 
  static int AddBlockPixel( int x, int y, int z,
                                     stg_render_info_t* rinfo ) ; //< used as a callback by StgModel
  
  stg_usec_t real_time_start; 
  
  bool quit; 
  
  int32_t MetersToPixels( stg_meters_t x ){ return (int32_t)floor(x * ppm) ; };
  
  void Initialize( const char* token, 
                         stg_msec_t interval_sim, 
                         double ppm );
  
  // dummy implementations to be overloaded by GUI subclasses
  virtual void PushColor( stg_color_t col ) { /* do nothing */  };
  virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */  };
  virtual void PopColor(){ /* do nothing */  };

  
  stg_usec_t real_time_now;
  
    stg_usec_t quit_time;

    bool destroy;

    GHashTable* models_by_name; 
    GList* velocity_list; 

    bool dirty; 

    int total_subs; 
    double ppm; 

    void StartUpdatingModel( StgModel* mod );
    void StopUpdatingModel( StgModel* mod );

    SuperRegion* CreateSuperRegion( int32_t x, int32_t y );
    void DestroySuperRegion( SuperRegion* sr );

    void Raytrace( stg_pose_t pose,              
            stg_meters_t range,
            stg_block_match_func_t func,
            StgModel* finder,
            const void* arg,
            stg_raytrace_sample_t* sample,
            bool ztest );

    void Raytrace( stg_pose_t pose,              
            stg_meters_t range,
            stg_radians_t fov,
            stg_block_match_func_t func,
            StgModel* finder,
            const void* arg,
            stg_raytrace_sample_t* samples,
            uint32_t sample_count,
            bool ztest );

    void RemoveBlock( int x, int y, StgBlock* block );

protected:
    GHashTable* superregions;
    GList* update_list; //< the descendants that need Update() called
    stg_usec_t interval_sim; 

    static void UpdateCb( StgWorld* world);

  stg_usec_t sim_time; 
    inline bool PastQuitTime() const { return (quit_time > 0) && (sim_time >= quit_time); }
  
    GList* ray_list;
    // store rays traced for debugging purposes
    void RecordRay( double x1, double y1, double x2, double y2 );
    Worldfile* wf; 
    bool graphics;
    stg_bounds3d_t extent;

    void Extend( stg_point3_t pt );

    //GHashTable* blocks;
    GArray lines;

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

    GList* GetRayList(){ return ray_list; };
    void ClearRays();

    long unsigned int updates; 
    
    FileManager* fileMan;

public:
    static const int DEFAULT_PPM = 50;  // default resolution in pixels per meter
    static const stg_msec_t DEFAULT_INTERVAL_SIM = 100;  
    static bool UpdateAll(); //returns true when time to quit, false otherwise
  
    StgWorld( const char* token = "MyWorld", 
                 stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
                 double ppm = DEFAULT_PPM );

    virtual ~StgWorld();

    stg_usec_t SimTimeNow(void){ return sim_time; }
    stg_usec_t RealTimeNow(void);
    stg_usec_t RealTimeSinceStart(void);
//  void PauseUntilNextUpdateTime(void);
//  void IdleUntilNextUpdateTime( int (*idler)(void) );

    void AddBlock( StgBlock* block );
    void RemoveBlock( StgBlock* block );

    stg_usec_t GetSimInterval(){ return interval_sim; };

    Worldfile* GetWorldFile(){ return wf; };

    inline virtual bool IsGUI() { return false; }
    
    virtual void Load( const char* worldfile_path );
    virtual void UnLoad();
    virtual void Reload();
    virtual bool Save( const char* filename );
    virtual bool Update(void);

    bool TestQuit(){ return( quit || quit_all );  }
    void Quit(){ quit = true; }
    void QuitAll(){ quit_all = true; }
    void CancelQuit(){ quit = false; }
    void CancelQuitAll(){ quit_all = false; }

    double Resolution(){ return ppm; };

    //StgModel* GetModel( const stg_id_t id );

    StgModel* GetModel( const char* name );

    stg_bounds3d_t GetExtent(){ return extent; };

    long unsigned int GetUpdateCount() { return updates; }
};


typedef struct {
    stg_pose_t pose;
    stg_color_t color;
    stg_usec_t time;
} stg_trail_item_t;

typedef int ctrlinit_t( StgModel* mod );
//typedef void ctrlupdate_t( StgModel* mod );

class StgModel : public StgAncestor
{
    friend class StgAncestor;
    friend class StgWorld;
    friend class StgWorldGui;
    friend class StgCanvas;
    friend class StgBlock;
  
private:
    static uint32_t count;
    static GHashTable*  modelsbyid;
    uint32_t id;    
    std::vector<Option*> drawOptions;
    const std::vector<Option*>& getOptions() const { return drawOptions; }

protected:
    stg_pose_t pose;
    stg_velocity_t velocity;
    stg_watts_t watts; //< power consumed by this model
    stg_color_t color;
    stg_kg_t mass;
    stg_geom_t geom;
    stg_laser_return_t laser_return;
    int obstacle_return;
    int blob_return;
    int gripper_return;
    int ranger_return;
    int fiducial_return;
    int fiducial_key;
    int boundary;
    stg_meters_t map_resolution;
    stg_bool_t stall;

    char* say_string;

    bool on_velocity_list;

    int gui_nose;
    int gui_grid;
    int gui_outline;
    int gui_mask;
    
    void registerOption( Option* opt ) { drawOptions.push_back( opt ); }
    
    
    StgModel* parent; //< the model that owns this one, possibly NULL

    GData* props;

    GHashTable* callbacks;

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

    stg_usec_t interval; //< time between updates in us
    stg_usec_t last_update; //< time of last update in us

    stg_bool_t disabled; //< if non-zero, the model is disabled

//  GList* d_list;
    GList* blocks; //< list of stg_block_t structs that comprise a body

    GArray* trail;

    bool rebuild_displaylist; //< iff true, regenerate block display list before redraw

    stg_pose_t global_pose;

    bool gpose_dirty; //< set this to indicate that global pose may have
    //changed

    bool data_fresh; 




    //stg_id_t id; // globally unique ID used as hash table key and
    // worldfile section identifier

    StgWorld* world; // pointer to the world in which this model exists
  
    StgModel* TestCollision( stg_pose_t pose, 
                                    stg_meters_t* hitx,
                                    stg_meters_t* hity );

    StgModel* TestCollision( stg_meters_t* hitx,
                                    stg_meters_t* hity );
  
    void Map();
    void UnMap();

    void MapWithChildren();
    void UnMapWithChildren();

    int TreeToPtrArray( GPtrArray* array );

    void Raytrace( stg_pose_t pose,
            stg_meters_t range, 
            stg_block_match_func_t func,
            const void* arg,
            stg_raytrace_sample_t* sample,
            bool ztest = true );

    void Raytrace( stg_pose_t pose,
            stg_meters_t range, 
            stg_radians_t fov, 
            stg_block_match_func_t func,
            const void* arg,
            stg_raytrace_sample_t* samples,
            uint32_t sample_count,
            bool ztest = true  );

    void Raytrace( stg_radians_t bearing,            
            stg_meters_t range,
            stg_block_match_func_t func,
            const void* arg,
            stg_raytrace_sample_t* sample,
            bool ztest = true );

    void Raytrace( stg_radians_t bearing,            
            stg_meters_t range,
            stg_radians_t fov,
            stg_block_match_func_t func,
            const void* arg,
            stg_raytrace_sample_t* samples,
            uint32_t sample_count,
            bool ztest = true );


    void GPoseDirtyTree();

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

    void DrawBlocksTree();
    virtual void DrawBlocks();
    virtual void DrawStatus( StgCamera* cam );
    void DrawStatusTree( StgCamera* cam );

    void DrawOriginTree();
    void DrawOrigin();

    void PushLocalCoords();
    void PopCoords();

    void DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha );


    // static wrapper for DrawBlocks()
    static void DrawBlocks( gpointer dummykey, 
            StgModel* mod, 
            void* arg );

    virtual void DrawPicker();
    virtual void DataVisualize( StgCamera* cam );

    virtual void DrawSelected(void);

    void DrawTrailFootprint();
    void DrawTrailBlocks();
    void DrawTrailArrows();
    void DrawGrid();
    void UpdateIfDue();
  
    /* hooks for attaching special callback functions (not used as
      variables) */
    char startup_hook, shutdown_hook, load_hook, save_hook, update_hook;

    ctrlinit_t* initfunc;

    GList* flag_list;

    Worldfile* wf;
    int wf_entity;

    GPtrArray* blinkenlights;
    void DrawBlinkenlights();

    int blocks_dl;

    //void BuildDisplayList();

    stg_model_type_t type;
    static const char* typestr;

    void DataVisualizeTree( StgCamera* cam );
    
    virtual void PushColor( stg_color_t 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(); }
    
    void DrawFlagList();
  
  void PushMyPose();  
  void PopPose();
  void ShiftPose( stg_pose_t* pose );
  void ShiftToTop();

public:
    void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, 
                          stg_meters_t ymin, stg_meters_t ymax );
    
    static StgModel* LookupId( uint32_t id )
    { return (StgModel*)g_hash_table_lookup( modelsbyid, (void*)id ); }
    
    StgModel( StgWorld* world, StgModel* parent, stg_model_type_t type = MODEL_TYPE_PLAIN );
    virtual ~StgModel();
    
    void Say( const char* str );
    
    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 Init();
    
    void AddFlag(  StgFlag* flag );
    void RemoveFlag( StgFlag* flag );
    
    void PushFlag( StgFlag* flag );
    StgFlag* PopFlag();
    
    int GetFlagCount(){ return g_list_length( flag_list ); }
    
    void AddBlinkenlight( stg_blinkenlight_t* b )
    {
        g_ptr_array_add( this->blinkenlights, b );
    }
    
    void ClearBlinkenlights()
    {
        g_ptr_array_set_size( this->blinkenlights, 0 );
    }
    
    void Enable(){ disabled = false; };
    void Disable(){ disabled = true; };
    
    // Load a control program for this model from an external library
    void LoadControllerModule( char* lib );
    
    // call this to ensure the GUI window is redrawn
    void NeedRedraw();
    
    void AddBlock( stg_point_t* pts, 
                  size_t pt_count, 
                  stg_meters_t zmin, 
                  stg_meters_t zmax,
                  stg_color_t color,
                  bool inherit_color );
    
    void AddBlockRect( double x, double y, 
                      double width, double height );
    
    void ClearBlocks();
    
    //const char* TypeStr(){ return this->typestr; }
    StgModel* Parent(){ return this->parent; }
    StgModel* GetModel( const char* name );
    int GuiMask(){ return this->gui_mask; };
    inline StgWorld* GetWorld(){ return this->world; }
    
    StgModel* Root()
    { return(  parent ? parent->Root() : this ); }
    
    bool ObstacleReturn(){ return obstacle_return; }  
    stg_laser_return_t GetLaserReturn(){ return laser_return; }
    int GetRangerReturn(){ return ranger_return; }  
    int FiducialReturn(){ return fiducial_return; }
    int FiducialKey(){ return fiducial_key; }
    
    bool IsAntecedent( StgModel* testmod );
    
    // returns true if model [testmod] is a descendent of model [mod]
    bool IsDescendent( StgModel* testmod );
    
    bool IsRelated( StgModel* mod2 );
    
    stg_pose_t GetGlobalPose();
    
    stg_velocity_t GetGlobalVelocity();
    
    /* set the velocity of a model in the global coordinate system */
    void SetGlobalVelocity(  stg_velocity_t gvel );
    
    void Subscribe();
    
    void Unsubscribe();
    
    void SetGlobalPose(  stg_pose_t gpose );
    
    void SetVelocity(  stg_velocity_t vel );
    
    void SetPose(  stg_pose_t pose );
    
    void AddToPose(  stg_pose_t pose );
    
    void AddToPose(  double dx, double dy, double dz, double da );
    
    void SetGeom(  stg_geom_t src );
    
    void SetFiducialReturn(  int fid );
    
    void SetFiducialKey(  int key );
    
    stg_color_t GetColor(){ return color; }
    
    //  stg_laser_return_t GetLaserReturn(){ return laser_return; }
    
    int SetParent( StgModel* newparent);
    
    stg_geom_t GetGeom(){ return geom; }
    
    stg_pose_t GetPose(){ return pose; }
    
    stg_velocity_t GetVelocity(){ return velocity; }
    
    // guess what these do?
    void SetColor( stg_color_t col );
    void SetMass( stg_kg_t mass );
    void SetStall( stg_bool_t stall );
    void SetGripperReturn( int val );
    void SetLaserReturn( stg_laser_return_t val );
    void SetObstacleReturn( int val );
    void SetBlobReturn( int val );
    void SetRangerReturn( int val );
    void SetBoundary( int val );
    void SetGuiNose( int val );
    void SetGuiMask( int val );
    void SetGuiGrid( int val );
    void SetGuiOutline( int val );
    void SetWatts( stg_watts_t watts );
    void SetMapResolution( stg_meters_t res );
    
    bool DataIsFresh(){ 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( void* address, 
                     stg_model_callback_t cb, 
                     void* user );
    
    int RemoveCallback( void* member,
                       stg_model_callback_t callback );
    
    void CallCallbacks(  void* address );
    
    /* wrappers for the generic callback add & remove functions that hide
     some implementation detail */
    
    void AddStartupCallback( stg_model_callback_t cb, void* user )
    { AddCallback( &startup_hook, cb, user ); };
    
    void RemoveStartupCallback( stg_model_callback_t cb )
    { RemoveCallback( &startup_hook, cb ); };
    
    void AddShutdownCallback( stg_model_callback_t cb, void* user )
    { AddCallback( &shutdown_hook, cb, user ); };
    
    void RemoveShutdownCallback( stg_model_callback_t cb )
    { RemoveCallback( &shutdown_hook, cb ); }
    
    void AddLoadCallback( stg_model_callback_t cb, void* user )
    { AddCallback( &load_hook, cb, user ); }
    
    void RemoveLoadCallback( stg_model_callback_t cb )
    { RemoveCallback( &load_hook, cb ); }
    
    void AddSaveCallback( stg_model_callback_t cb, void* user )
    { AddCallback( &save_hook, cb, user ); }
    
    void RemoveSaveCallback( stg_model_callback_t cb )
    { RemoveCallback( &save_hook, cb ); }
    
    void AddUpdateCallback( stg_model_callback_t cb, void* user )
    { AddCallback( &update_hook, cb, user ); }
    
    void RemoveUpdateCallback( stg_model_callback_t cb )
    { RemoveCallback( &update_hook, cb ); }
    
    void* GetProperty( char* key );
    
    int SetProperty( char* key, void* data );
    void UnsetProperty( char* key );
        
    virtual void Print( char* prefix );
    virtual const char* PrintWithPose();
    
    void GlobalToLocal( stg_pose_t* pose );
    
    //void LocalToGlobal( stg_pose_t* pose );
    
    stg_pose_t LocalToGlobal( stg_pose_t pose );
    
    stg_point3_t LocalToGlobal( stg_point3_t local );
    
    StgModel* GetUnsubscribedModelOfType( stg_model_type_t type );
    
  // Iff true, model may output some debugging visualizations and other info
    //bool debug;
    
    bool Stalled(){ return this->stall; }
};

// BLOCKS
class StgBlock
{
public:

    StgBlock( StgModel* mod,
            stg_point_t* pts, 
            size_t pt_count,
            stg_meters_t height,
            stg_meters_t z_offset,
            stg_color_t color,
            bool inherit_color );

    ~StgBlock();

    void Map();
    void UnMap(); // draw the block into the world

    void DrawGlobal(); // draw the block in OpenGL using pts_global coords
    void Draw(); // draw the block in OpenGL
    //void Draw2D(); // draw the block in OpenGL
    void DrawSolid(); // draw the block in OpenGL as a solid single color
    void DrawFootPrint(); // draw the projection of the block onto the z=0 plane

    static void ScaleList( GList* blocks, stg_size_t* size );
    void RecordRenderPoint( GSList** head, GSList* link, unsigned int* c1, unsigned int* c2 );

    StgModel* Model(){ return mod; };

    stg_point_t* Points( unsigned int *count )
    { if( count ) *count = pt_count; return pts; };        

    bool IntersectGlobalZ( stg_meters_t z )
    { return( z >= global_zmin &&  z <= global_zmax ); }

    stg_color_t Color()
    { return( inherit_color ? mod->GetColor() : color ); }

private:
    stg_point_t* pts; //< points defining a polygon
    size_t pt_count; //< the number of points
    stg_meters_t zmin;
    stg_meters_t zmax;

    stg_meters_t global_zmin;
    stg_meters_t global_zmax;

    StgModel* mod; //< model to which this block belongs

    stg_point_int_t* pts_global_pixels; //< points defining a polygon in global coords

    stg_color_t color;
    bool inherit_color;

    GArray* rendered_points;

    inline void DrawTop();
    inline void DrawSides();

    inline void PushColor( stg_color_t col )
    { mod->PushColor( col ); }

    inline void PushColor( double r, double g, double b, double a )
    { mod->PushColor( r,g,b,a ); }

    inline void PopColor()
    { mod->PopColor(); }
};

// COLOR STACK CLASS
class GlColorStack
{
    public:
        GlColorStack();
        ~GlColorStack();

        void Push( GLdouble col[4] );
        void Push( double r, double g, double b, double a );
        void Push( double r, double g, double b );
        void Push( stg_color_t col );

        void Pop();

        unsigned int Length()
        { return g_queue_get_length( colorstack ); }

    private:
        GQueue* colorstack;
};

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

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

        inline float yaw( void ) const { return _yaw; }
        inline float pitch( void ) const { return _pitch; }
    
        inline float x( void ) const { return _x; }
        inline float y( void ) const { return _y; }
        inline float 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 StgPerspectiveCamera : public StgCamera
{
    private:
        float _z_near;
        float _z_far;
        float _vert_fov;
        float _horiz_fov;
        float _aspect;

    public:
        StgPerspectiveCamera( void );

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

        void strafe( float amount );
        void forward( float amount );
    
        inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; }
        inline void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
        void move( float x, float y, float z );
        inline void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
        inline void setAspect( float aspect ) { 
            //std::cout << "aspect: " << aspect << " vert: " << _vert_fov << " => " << aspect * _vert_fov << std::endl;
            //_vert_fov = aspect / _horiz_fov;
            _aspect = aspect;
        }
        inline void setYaw( float yaw ) { _yaw = yaw; }
        inline float horizFov( void ) const { return _horiz_fov; }
        inline float vertFov( void ) const { return _vert_fov; }
        inline void addYaw( float yaw ) { _yaw += yaw; }
        inline void setPitch( float pitch ) { _pitch = pitch; }
        inline void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }
    
        inline float realDistance( float z_buf_val ) const {
            //formulat found at http://www.cs.unc.edu/~hoff/techrep/openglz.html
            //Z = Zn*Zf / (Zf - z*(Zf-Zn))
            return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
        }
        inline void scroll( float dy ) { _z += dy; }
        inline float nearClip( void ) const { return _z_near; }
        inline float farClip( void ) const { return _z_far; }
        inline void setClip( float near, float far ) { _z_far = far; _z_near = near; }
    
        inline void reset() { setPitch( 70 ); setYaw( 0 ); }
    
        void Load( Worldfile* wf, int sec );
        void Save( Worldfile* wf, int sec );
};

class StgOrthoCamera : public StgCamera
{
    private:
        float _scale;

    float _pixels_width;
    float _pixels_height;
    float _y_min;
    float _y_max;

    public:
        StgOrthoCamera( void ) : _scale( 15 ) { }
        virtual void Draw() const;
        virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max );
        virtual void SetProjection( void ) const;

        void move( float x, float y );
        inline void setYaw( float yaw ) { _yaw = yaw;   }
        inline void setPitch( float pitch ) { _pitch = pitch; }
        inline void addYaw( float yaw ) { _yaw += yaw;  }
        inline void addPitch( float pitch ) {
            _pitch += pitch;
            if( _pitch > 90 )
                _pitch = 90;
            else if( _pitch < 0 )
                _pitch = 0;
        }

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

        void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 );  
        inline void reset( void ) { _pitch = _yaw = 0; }

        inline float scale() const { return _scale; }

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

class StgCanvas : public Fl_Gl_Window
{
    friend class StgWorldGui; // allow access to private members
    friend class StgModel;
  
private:
    GlColorStack colorstack;

    StgCamera* current_camera;
    StgOrthoCamera camera;
    StgPerspectiveCamera perspective_camera;
    bool dirty_buffer;
    Worldfile* wf;

    int startx, starty;
    bool selectedModel;
    bool clicked_empty_space;
    int empty_space_startx, empty_space_starty;
    GList* selected_models; 

    StgModel* last_selection; 


    stg_msec_t interval; // window refresh interval in ms

    GList* ray_list;
    void RecordRay( double x1, double y1, double x2, double y2 );
    void DrawRays();
    void ClearRays();
    void DrawGlobalGrid();
  
    Option
        showBlinken, 
        showBlocks, 
        showClock, 
        showData, 
        showFlags,
        showFollow,
        showFootprints, 
        showGrid, 
        showOccupancy, 
        showScreenshots,
        showStatus,
        showTrailArrows, 
        showTrailRise, 
        showTrails, 
        showTree,
        pCamOn,
        visualizeAll;
  
public:
    StgCanvas( StgWorldGui* world, int x, int y, int W,int H);
    ~StgCanvas();

    bool graphics;
    StgWorldGui* world;

    void Screenshot();
  
    void createMenuItems( Fl_Menu_Bar* menu, std::string path );
  
    void FixViewport(int W,int H);
    void DrawFloor(); //simpler floor compared to grid
    void DrawBlocks();
    void resetCamera();
    virtual void renderFrame();
    virtual void draw();
    virtual int handle( int event );
    void resize(int X,int Y,int W,int H);

    void CanvasToWorld( int px, int py, 
            double *wx, double *wy, double* wz );

    StgModel* getModel( int x, int y );
    bool selected( StgModel* mod );
    void select( StgModel* mod );
    void unSelect( StgModel* mod );
    void unSelectAll();

    inline void setDirtyBuffer( void ) { dirty_buffer = true; }
    inline bool dirtyBuffer( void ) const { return dirty_buffer; }
    
    inline void PushColor( stg_color_t col )
    { colorstack.Push( col ); } 

    void PushColor( double r, double g, double b, double a )
    { colorstack.Push( r,g,b,a ); }

    void PopColor(){ colorstack.Pop(); } 

    void InvertView( uint32_t invertflags );

    static void TimerCallback( StgCanvas* canvas );
    static void perspectiveCb( Fl_Widget* w, void* p );
  
  void Load( Worldfile* wf, int section );
  void Save( Worldfile* wf, int section );
};

class StgWorldGui : public StgWorld, public Fl_Window 
{
    friend class StgCanvas;
    friend class StgModelCamera;

private:
    bool paused; 
    //int wf_section;
    StgCanvas* canvas;
    Fl_Menu_Bar* mbar;
    OptionsDlg* oDlg;
    std::vector<Option*> drawOptions;
    void updateOptions();
    stg_usec_t interval_log[INTERVAL_LOG_LEN];

    stg_usec_t real_time_of_last_update;
    stg_usec_t interval_real;   
    
    // static callback functions
    static void windowCb( Fl_Widget* w, void* p );  
    static void fileLoadCb( Fl_Widget* w, void* p );
    static void fileSaveCb( Fl_Widget* w, void* p );
    static void fileSaveAsCb( Fl_Widget* w, void* p );
    static void fileExitCb( Fl_Widget* w, void* p );
    static void viewOptionsCb( Fl_Widget* w, void* p );
    static void optionsDlgCb( Fl_Widget* w, void* p );
    static void helpAboutCb( Fl_Widget* w, void* p );
    
    // GUI functions
    bool saveAsDialog();
    bool closeWindowQuery();
    
    // Quit time pause
    bool pause_time;
    
protected:
    virtual void PushColor( stg_color_t col )
    { canvas->PushColor( col ); } 

    virtual void PushColor( double r, double g, double b, double a )
    { canvas->PushColor( r,g,b,a ); }

    virtual void PopColor()
    { canvas->PopColor(); }

    void DrawTree( bool leaves );
    void DrawFloor();
    
    StgCanvas* GetCanvas( void ) { return canvas; }

public:
    static const stg_msec_t DEFAULT_INTERVAL_REAL = 100; 
    
    StgWorldGui(int W,int H,const char*L=0);
    ~StgWorldGui();
    
    virtual bool Update();
    
    virtual void Load( const char* filename );
    virtual void UnLoad();
    virtual bool Save( const char* filename );
    
    inline virtual bool IsGUI() { return true; }
    
    void Start(){ paused = false; };
    void Stop(){ paused = true; };
    void TogglePause(){ paused = !paused; };
    
    std::string ClockString( void );
    
    void SetRealTimeInterval( stg_usec_t usec )
    { interval_real = usec; }
};


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

typedef struct
{
    stg_color_t color;
    uint32_t left, top, right, bottom;
    stg_meters_t range;
} stg_blobfinder_blob_t;

class StgModelBlobfinder : public StgModel
{
private:
    GArray* colors;
    GArray* blobs;

    // predicate for ray tracing
    static bool BlockMatcher( StgBlock* testblock, StgModel* finder );

    static Option showBlobData;
    
    virtual void DataVisualize( StgCamera* cam );

public:
    unsigned int scan_width;
    unsigned int scan_height;
    stg_meters_t range;
    stg_radians_t fov;
    stg_radians_t pan;

    static  const char* typestr;

    // constructor
    StgModelBlobfinder( StgWorld* world,
                             StgModel* parent );
    // destructor
    ~StgModelBlobfinder();

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

    stg_blobfinder_blob_t* GetBlobs( unsigned int* count )
    { 
        if( count ) *count = blobs->len;
        return (stg_blobfinder_blob_t*)blobs->data;
    }

    void AddColor( stg_color_t col );

    void RemoveColor( stg_color_t col );

    void RemoveAllColors();
};

// ENERGY model --------------------------------------------------------------

typedef struct
{
    stg_joules_t stored;

    stg_bool_t charging;

    stg_meters_t range;

    GPtrArray* connections;
} stg_energy_data_t;

typedef struct
{
    stg_joules_t capacity;

    stg_watts_t give_rate;

    stg_watts_t take_rate;

    stg_meters_t probe_range;

    stg_bool_t give;

} stg_energy_config_t;

// there is currently no energy command packet


// LASER MODEL --------------------------------------------------------

typedef struct
{
  stg_meters_t range; 
  double reflectance; 
} stg_laser_sample_t;

typedef struct
{
  uint32_t sample_count;
  uint32_t resolution;
  stg_range_bounds_t range_bounds;
  stg_radians_t fov;
  stg_usec_t interval;
} stg_laser_cfg_t;

class StgModelLaser : public StgModel
{
private:
  int dl_debug_laser;
  
  int data_dl; 
  bool data_dirty;

  stg_laser_sample_t* samples;
  uint32_t sample_count;
  stg_meters_t range_min, range_max;
  stg_radians_t fov;
  uint32_t resolution;
  
  static Option showLaserData;
  static Option showLaserStrikes;
  
public:
  static const char* typestr;
  // constructor
  StgModelLaser( StgWorld* world,
                      StgModel* parent ); 
  
  // destructor
  ~StgModelLaser();
  
  virtual void Startup();
  virtual void Shutdown();
  virtual void Update();
  virtual void Load();
  virtual void Print( char* prefix );
  virtual void DataVisualize( StgCamera* cam );
  
  uint32_t GetSampleCount(){ return sample_count; }
  
  stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
  
  void SetSamples( stg_laser_sample_t* samples, uint32_t count);
  
  // Get the user-tweakable configuration of the laser
  stg_laser_cfg_t GetConfig( );
  
  // Set the user-tweakable configuration of the laser
  void SetConfig( stg_laser_cfg_t cfg );  
};

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

//   typedef enum {
//     STG_GRIPPER_PADDLE_OPEN = 0, // default state
//     STG_GRIPPER_PADDLE_CLOSED, 
//     STG_GRIPPER_PADDLE_OPENING,
//     STG_GRIPPER_PADDLE_CLOSING,
//   } stg_gripper_paddle_state_t;

//   typedef enum {
//     STG_GRIPPER_LIFT_DOWN = 0, // default state
//     STG_GRIPPER_LIFT_UP, 
//     STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state
//     STG_GRIPPER_LIFT_DOWNING, 
//   } stg_gripper_lift_state_t;

//   typedef enum {
//     STG_GRIPPER_CMD_NOP = 0, // default state
//     STG_GRIPPER_CMD_OPEN, 
//     STG_GRIPPER_CMD_CLOSE,
//     STG_GRIPPER_CMD_UP, 
//     STG_GRIPPER_CMD_DOWN    
//   } stg_gripper_cmd_type_t;

//   /** gripper configuration packet
//    */
//   typedef struct
//   {
//     stg_size_t paddle_size; ///< paddle dimensions 

//     stg_gripper_paddle_state_t paddles;
//     stg_gripper_lift_state_t lift;

//     double paddle_position; ///< 0.0 = full open, 1.0 full closed
//     double lift_position; ///< 0.0 = full down, 1.0 full up

//     stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle
//     stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle  
//     stg_bool_t paddles_stalled; // true iff some solid object stopped
//              // the paddles closing or opening

//     GSList *grip_stack;  ///< stack of items gripped
//     int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited

//     double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full.

//   } stg_gripper_config_t;

//   /** gripper command packet
//    */
//   typedef struct
//   {
//     stg_gripper_cmd_type_t cmd;
//     int arg;
//   } stg_gripper_cmd_t;


//   /** gripper data packet
//    */
//   typedef struct
//   {
//     stg_gripper_paddle_state_t paddles;
//     stg_gripper_lift_state_t lift;

//     double paddle_position; ///< 0.0 = full open, 1.0 full closed
//     double lift_position; ///< 0.0 = full down, 1.0 full up

//     stg_bool_t inner_break_beam; ///< non-zero iff beam is broken
//     stg_bool_t outer_break_beam; ///< non-zero iff beam is broken

//     stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something

//     stg_bool_t paddles_stalled; // true iff some solid object stopped
//              // the paddles closing or opening

//     int stack_count; ///< number of objects in stack


//   } stg_gripper_data_t;


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

//   typedef struct
//   {
//     stg_pose_t pose;
//     stg_meters_t length;
//   } stg_bumper_config_t;

//   typedef struct
//   {
//     StgModel* hit;
//     stg_point_t hit_point;
//   } stg_bumper_sample_t;


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

typedef struct
{
    stg_meters_t max_range_anon; //< maximum detection range
    stg_meters_t max_range_id; 
    stg_meters_t min_range; 
    stg_radians_t fov; 
    stg_radians_t heading; 

    int key;
} stg_fiducial_config_t;

typedef struct
{
    stg_meters_t range; 
    stg_radians_t bearing; 
    stg_pose_t geom; 
    stg_pose_t pose; 
    int id; 

} stg_fiducial_t;

class StgModelFiducial : public StgModel
{
    private:
        // if neighbor is visible, add him to the fiducial scan
        void AddModelIfVisible( StgModel* him );

        // static wrapper function can be used as a function pointer
        static int AddModelIfVisibleStatic( StgModel* him, StgModelFiducial* me )
            { if( him != me ) me->AddModelIfVisible( him ); return 0; }

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

        GArray* data;
    
        static Option showFiducialData;
    
    public:
  static const char* typestr;
  // constructor
  StgModelFiducial( StgWorld* world,
                          StgModel* parent );
  // destructor
  virtual ~StgModelFiducial();
  
        virtual void Load();
        void Shutdown( void );

        stg_meters_t max_range_anon; //< maximum detection range
        stg_meters_t max_range_id; 
        stg_meters_t min_range; 
        stg_radians_t fov; 
        stg_radians_t heading; 
        int key; 

        stg_fiducial_t* fiducials; 
        uint32_t fiducial_count; 
};


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

typedef struct
{
    stg_pose_t pose;
    stg_size_t size;
    stg_bounds_t bounds_range;
    stg_radians_t fov;
    int ray_count;
} stg_ranger_sensor_t;

class StgModelRanger : public StgModel
{
    protected:

        virtual void Startup();
        virtual void Shutdown();
        virtual void Update();
        virtual void DataVisualize( StgCamera* cam );

    public:
  static const char* typestr;
        // constructor
        StgModelRanger( StgWorld* world,
                             StgModel* parent );
        // destructor
        virtual ~StgModelRanger();

        virtual void Load();
        virtual void Print( char* prefix );

        uint32_t sensor_count;
        stg_ranger_sensor_t* sensors;
        stg_meters_t* samples;
    
    private:
    static Option showRangerData;
    static Option showRangerTransducers;
    
};

// BLINKENLIGHT MODEL ----------------------------------------------------
class StgModelBlinkenlight : public StgModel
{
    private:
        double dutycycle;
        bool enabled;
        stg_msec_t period;
        bool on;

        static Option showBlinkenData;
    public:

  static const char* typestr;
        StgModelBlinkenlight( StgWorld* world,
                                     StgModel* parent );

        ~StgModelBlinkenlight();

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

// CAMERA MODEL ----------------------------------------------------
typedef struct {
    // GL_V3F
    GLfloat x, y, z;
} ColoredVertex;
    
class StgModelCamera : public StgModel
{
    private:
        StgCanvas* _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;
    
        StgPerspectiveCamera _camera;
        float _yaw_offset; //position camera is mounted at
        float _pitch_offset;
        
        bool GetFrame();
    
    public:
        StgModelCamera( StgWorld* world,
                             StgModel* parent ); 

        ~StgModelCamera();

        virtual void Load();
    
        virtual void Update();
    
        //virtual void Draw( uint32_t flags, StgCanvas* canvas );
    
        virtual void DataVisualize( StgCamera* cam );
    
        inline int getWidth( void ) const { return _width; }
    
        inline int getHeight( void ) const { return _height; }
    
        inline const StgPerspectiveCamera& getCamera( void ) const { return _camera; }
    
        inline const GLfloat* FrameDepth() const { return _frame_data; }
    
        inline const GLubyte* FrameColor() const { return _frame_color_data; }
    
    inline void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
    
    inline void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
};

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

typedef enum
{ STG_POSITION_CONTROL_VELOCITY, 
    STG_POSITION_CONTROL_POSITION 
} stg_position_control_mode_t;

typedef enum
{ STG_POSITION_LOCALIZATION_GPS, 
    STG_POSITION_LOCALIZATION_ODOM 
} stg_position_localization_mode_t;

typedef enum
{ STG_POSITION_DRIVE_DIFFERENTIAL, 
    STG_POSITION_DRIVE_OMNI, 
    STG_POSITION_DRIVE_CAR 
} stg_position_drive_mode_t;


class StgModelPosition : public StgModel
{
    private:
        stg_pose_t goal; //< the current velocity or pose to reach, depending on the value of control_mode
        stg_position_control_mode_t control_mode;
        stg_position_drive_mode_t drive_mode;
        stg_position_localization_mode_t localization_mode; 
        stg_velocity_t integration_error; 
    public:
  static const char* typestr;
        // constructor
        StgModelPosition( StgWorld* world,
                                StgModel* parent );
        // destructor
        ~StgModelPosition();

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

        void SetOdom( stg_pose_t 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( stg_velocity_t vel );

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

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


}; // end namespace stg

#endif

---