Colony

#include <math.h>

#include <stdlib.h>

#include <stdio.h>

#include <sys/time.h>

#include <string.h>

#include <gtk/gtkmain.h>

#include <gtk/gtksignal.h>

#include "gtk_environment_view.h"

#include "robot.h"

#define ENVIRONMENT_VIEW_MIN_WIDTH                100

#define ENVIRONMENT_VIEW_MIN_HEIGHT                50

#define ENVIRONMENT_PI                              3.14159

#define ENVIRONMENT_ROBOT_DIAMETER                50

static GtkWidgetClass* parent_class = NULL;

static void gtk_environment_view_class_init(GtkEnvironmentViewClass* environmentClass);

static void gtk_environment_view_init(GtkEnvironmentView* view);

static void gtk_environment_view_destroy(GtkObject* object);

static void gtk_environment_view_size_request(GtkWidget* widget,

                GtkRequisition* req);

static void gtk_environment_view_realize(GtkWidget* widget);

static void gtk_environment_view_size_allocate(GtkWidget* widget, GtkAllocation* a);

static gboolean gtk_environment_view_expose(GtkWidget* widget,

                GdkEventExpose* event);

static gboolean gtk_environment_view_mouse_down(GtkWidget* widget,

                                GdkEventButton* event);

static gboolean gtk_environment_view_mouse_release(GtkWidget* widget,

                                GdkEventButton* event);

static gboolean gtk_environment_view_mouse_move(GtkWidget* widget,

                                GdkEventMotion* event);

static void draw_robot(GdkDrawable* drawable, GdkGC* gc, 

                        int x, int y, int angle);

GtkType gtk_environment_view_get_type(void)

{

        static GtkType environment_view_type = 0;

        if (!environment_view_type)

        {

                static const GtkTypeInfo environment_view_info =

                {

                        "GtkEnvironmentView",

                        sizeof(GtkEnvironmentView),

                        sizeof(GtkEnvironmentViewClass),

                        (GtkClassInitFunc)gtk_environment_view_class_init,

                        (GtkObjectInitFunc)gtk_environment_view_init,

                        NULL, NULL, (GtkClassInitFunc)NULL

                };

                environment_view_type = gtk_type_unique(GTK_TYPE_WIDGET,

                        &environment_view_info);

        }

        return environment_view_type;

}

static void gtk_environment_view_class_init(GtkEnvironmentViewClass* environmentClass)

{

        GtkObjectClass* object_class;

        GtkWidgetClass* widget_class;

        object_class = (GtkObjectClass*)environmentClass;

        widget_class = (GtkWidgetClass*)environmentClass;

        parent_class = gtk_type_class(gtk_widget_get_type());

        object_class->destroy = gtk_environment_view_destroy;

        widget_class->realize = gtk_environment_view_realize;

        widget_class->expose_event = gtk_environment_view_expose;

        widget_class->size_request = gtk_environment_view_size_request;

        widget_class->size_allocate = gtk_environment_view_size_allocate;

        widget_class->button_press_event = gtk_environment_view_mouse_down;

        widget_class->button_release_event = gtk_environment_view_mouse_release;

        widget_class->motion_notify_event = gtk_environment_view_mouse_move;

}

GtkWidget* gtk_environment_view_new(void)

{

        GtkEnvironmentView* view;

        view = gtk_type_new(gtk_environment_view_get_type());

        return GTK_WIDGET(view);

}

static void gtk_environment_view_init(GtkEnvironmentView* view)

{

        view->topLeftX = -2.0;

        view->topLeftY = -1.3;

        view->scale = 2.0 / 800.0;

        view->width = -1;

        view->height = -1;

        GTK_WIDGET_SET_FLAGS(&(view->widget), GTK_DOUBLE_BUFFERED);

        view->mouseDown = 0;

}

static void gtk_environment_view_destroy(GtkObject* object)

{

        GtkEnvironmentView* view;

        g_return_if_fail(object != NULL);

        g_return_if_fail(GTK_IS_ENVIRONMENT_VIEW(object));

        view = GTK_ENVIRONMENT_VIEW(object);

        if (GTK_OBJECT_CLASS(parent_class)->destroy)

                (* GTK_OBJECT_CLASS(parent_class)->destroy) (object);

}

static void gtk_environment_view_realize(GtkWidget* widget)

{

        GtkEnvironmentView* view;

        GdkWindowAttr attributes;

        gint attributes_mask;

        g_return_if_fail( widget != NULL);

        g_return_if_fail( GTK_IS_ENVIRONMENT_VIEW(widget));

        GTK_WIDGET_SET_FLAGS(widget, GTK_REALIZED);

        view = GTK_ENVIRONMENT_VIEW(widget);

        attributes.x = widget->allocation.x;

        attributes.y = widget->allocation.y;

        attributes.width = widget->allocation.width;

        attributes.height = widget->allocation.height;

        attributes.wclass = GDK_INPUT_OUTPUT;

        attributes.window_type = GDK_WINDOW_CHILD;

        attributes.event_mask = gtk_widget_get_events(widget) |

                GDK_EXPOSURE_MASK | GDK_BUTTON_PRESS_MASK |

                GDK_BUTTON_RELEASE_MASK | GDK_POINTER_MOTION_MASK |

                GDK_POINTER_MOTION_HINT_MASK;

        attributes.visual = gtk_widget_get_visual(widget);

        attributes.colormap = gtk_widget_get_colormap(widget);

        attributes_mask = GDK_WA_X | GDK_WA_Y | GDK_WA_VISUAL | GDK_WA_COLORMAP;

        widget->window = gdk_window_new(widget->parent->window, &attributes,

                        attributes_mask);

        gdk_window_set_user_data(widget->window, widget);

        widget->style = gtk_style_attach(widget->style, widget->window);

        gtk_style_set_background(widget->style, widget->window,

                        GTK_STATE_NORMAL);

}

static void gtk_environment_view_size_request(GtkWidget* widget, 

                GtkRequisition* req)

{

        if (req->width < ENVIRONMENT_VIEW_MIN_WIDTH)

                req->width = ENVIRONMENT_VIEW_MIN_WIDTH;

        if (req->height < ENVIRONMENT_VIEW_MIN_HEIGHT)

                req->height = ENVIRONMENT_VIEW_MIN_HEIGHT;

}

static void gtk_environment_view_size_allocate(GtkWidget* widget, GtkAllocation* a)

{

        GtkEnvironmentView* view;

        g_return_if_fail(widget != NULL);

        g_return_if_fail(GTK_IS_ENVIRONMENT_VIEW(widget));

        g_return_if_fail(a != NULL);

        view = GTK_ENVIRONMENT_VIEW(widget);

        widget->allocation = *a;

        view->width = a->width;

        view->height = a->height;

        if (GTK_WIDGET_REALIZED(widget))

        {

                gdk_window_move_resize(widget->window, a->x, a->y, a->width, a->height);

        }

}

static gboolean gtk_environment_view_expose(GtkWidget* widget,

                GdkEventExpose* event)

{

        GtkEnvironmentView* view;

        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)

                return FALSE;

        if (event->count > 0)

                return FALSE;

        view = GTK_ENVIRONMENT_VIEW(widget);

        robot_iterator_reset();

        Robot* r;

        while ((r = robot_iterator_next()) != NULL) {

        printf("x: %d, y:%d, Theta: %f\n",

            r->pose.x, r->pose.y, r->pose.theta);

                draw_robot(widget->window, 

                        widget->style->fg_gc[GTK_WIDGET_STATE(widget)], 

            r->pose.x, r->pose.y, r->pose.theta);

    }

        return FALSE;

}

static gboolean gtk_environment_view_mouse_down(GtkWidget* widget,

                                GdkEventButton* event)

{

        GtkEnvironmentView* view;

        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)

                return FALSE;

        view = GTK_ENVIRONMENT_VIEW(widget);

        if (event->button != 1)

                return FALSE;

        view->mouseDownX = event->x;

        view->mouseDownY = event->y;

        view->mouseX = event->x;

        view->mouseY = event->y;

        view->mouseDown = 1;

        return FALSE;

}

static gboolean gtk_environment_view_mouse_release(GtkWidget* widget,

                                GdkEventButton* event)

{

        GtkEnvironmentView* view;

        double x1, x2, y1, y2;

        int w, h;

        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)

                return FALSE;

        view = GTK_ENVIRONMENT_VIEW(widget);

        if (event->button != 1 && event->button != 3)

                return FALSE;

        if (event->button == 1)

                view->mouseDown = 0;

        view->mouseX = event->x;

        view->mouseY = event->y;

        w = abs(view->mouseX - view->mouseDownX);

        h = abs(view->mouseY - view->mouseDownY);

        // if the box is big, zoom to the box

        if (event->button == 1 && (w >= 10 || h >= 10))

        {

                // we need to make sure the box is to scale

                // scale to height

                if (fabs((double)w / h) <= fabs((double)view->width / view->height))

                {

                        w = (int)((double)view->width / view->height * h);

                        if (view->mouseX < view->mouseDownX)

                                view->mouseX = view->mouseDownX - w;

                        else

                                view->mouseX = view->mouseDownX + w;

                }

                // scale to width

                else

                {

                        h = (int)((double)view->height / view->width * w);

                        if (view->mouseY < view->mouseDownY)

                                view->mouseY = view->mouseDownY - h;

                        else

                                view->mouseY = view->mouseDownY + h;

                }

                x1 = view->mouseX * view->scale + view->topLeftX;

                y1 = view->mouseY * view->scale + view->topLeftY;

                x2 = view->mouseDownX * view->scale + view->topLeftX;

                y2 = view->mouseDownY * view->scale + view->topLeftY;

                if (x2 < x1)

                        x1 = x2;

                if (y2 < y1)

                        y1 = y2;

                view->scale = (w * view->scale) / view->width;

                view->topLeftX = x1;

                view->topLeftY = y1;

        }

        else

        {

                x1 = view->mouseX * view->scale + view->topLeftX;

                y1 = view->mouseY * view->scale + view->topLeftY;

                // zoom in on left click, out on right click

                if (event->button == 1)

                        view->scale /= 2.0;

                else

                        view->scale *= 2.0;

                view->topLeftX = x1 - view->width / 2 * view->scale;

                view->topLeftY = y1 - view->height / 2 * view->scale;

        }

        return FALSE;

}

static gboolean gtk_environment_view_mouse_move(GtkWidget* widget,

                                GdkEventMotion* event)

{

        GtkEnvironmentView* view;

        if (widget == NULL || !GTK_IS_ENVIRONMENT_VIEW(widget) || event == NULL)

                return FALSE;

        view = GTK_ENVIRONMENT_VIEW(widget);

        view->mouseX = event->x;

        view->mouseY = event->y;

        if (!view->mouseDown)

                return FALSE;

        // we need to make sure the new view is to scale

        int w = abs(view->mouseX - view->mouseDownX);

        int h = abs(view->mouseY - view->mouseDownY);

        // scale to height

        if (fabs((double)w / h) <= fabs((double)view->width / view->height))

        {

                w = (int)((double)view->width / view->height * h);

                if (view->mouseX < view->mouseDownX)

                        view->mouseX = view->mouseDownX - w;

                else

                        view->mouseX = view->mouseDownX + w;

        }

        // scale to width

        else

        {

                h = (int)((double)view->height / view->width * w);

                if (view->mouseY < view->mouseDownY)

                        view->mouseY = view->mouseDownY - h;

                else

                        view->mouseY = view->mouseDownY + h;

        }

        gtk_widget_queue_draw_area(widget, 0, 0, view->width, view->height);

        return FALSE;

}

void gtk_environment_view_refresh(GtkWidget* view)

{

        gdk_threads_enter();

        gtk_widget_queue_draw(view);

        gdk_threads_leave();

}

static void draw_robot(GdkDrawable* drawable, GdkGC* gc, 

                        int x, int y, int angle) 

{

        if (!drawable || !gc)

                return;

        double ang_rad = angle * ENVIRONMENT_PI / 180;

        int x_c = x - ENVIRONMENT_ROBOT_DIAMETER / 2;

        int y_c = y - ENVIRONMENT_ROBOT_DIAMETER / 2;

        gdk_draw_arc(drawable, gc, FALSE, x_c, y_c, 

                  ENVIRONMENT_ROBOT_DIAMETER, ENVIRONMENT_ROBOT_DIAMETER, 

        0, 360*64);

        gdk_draw_line(drawable, gc, x, y, 

                x + (ENVIRONMENT_ROBOT_DIAMETER - 10) * cos(ang_rad),

                y - (ENVIRONMENT_ROBOT_DIAMETER - 10) * sin(ang_rad));

}