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"

#include "world.h"

#define ENVIRONMENT_VIEW_MIN_WIDTH                100

#define ENVIRONMENT_VIEW_MIN_HEIGHT                50

#define ROBOT_DIAMETER                                        30

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_world(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, world_t* world);

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

                        float x, float y, float angle);

static void to_world_coordinates(GtkEnvironmentView* view, int x, int y, float* world_x, float* world_y);

static void to_window_coordinates(GtkEnvironmentView* view, float x, float y, int* win_x, int* win_y);

static void to_window_scale(GtkEnvironmentView* view, float v, int* win_v);

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);

        draw_world(view, widget->window, widget->style->fg_gc[GTK_WIDGET_STATE(widget)], &world);

        robot_iterator_reset();

        Robot* r;

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

                draw_robot(view, 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;

        Robot* r;

        float closestdist;

        int closest;

        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;

        /*closestdist = 10e8;

        robot_iterator_reset();

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

        {

                float dist = r->pose.x - 

        }*/

        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();

}

void to_world_coordinates(GtkEnvironmentView* view, int x, int y, float* world_x, float* world_y)

{

        double leftx = world.win.p1.x;

        double upy = world.win.p1.x;

        double xwidth = world.win.p2.x - leftx;

        double yheight = world.win.p2.y - upy;

        int width = view->width, height = view->height;

        if (xwidth / width < yheight / height)

                width = (int)(height / yheight * xwidth);

        else

                height = (int)(width / xwidth * yheight);

        *world_x = (float)x / width * xwidth + leftx;

        *world_y = (float)y / height * yheight + upy;

}

void to_window_coordinates(GtkEnvironmentView* view, float x, float y, int* win_x, int* win_y)

{

        double leftx = world.win.p1.x;

        double upy = world.win.p1.x;

        double xwidth = world.win.p2.x - leftx;

        double yheight = world.win.p2.y - upy;

        int width = view->width, height = view->height;

        if (xwidth / width < yheight / height)

                width = (int)(height / yheight * xwidth);

        else

                height = (int)(width / xwidth * yheight);

        *win_x = (int)((x - leftx) / xwidth * width);

        *win_y = (int)((y - upy) / yheight * height);

}

void to_window_scale(GtkEnvironmentView* view, float v, int* win_v)

{

        double leftx = world.win.p1.x;

        double upy = world.win.p1.x;

        double xwidth = world.win.p2.x - leftx;

        double yheight = world.win.p2.y - upy;

        int width = view->width, height = view->height;

        if (xwidth / width < yheight / height)

                width = (int)(height / yheight * xwidth);

        else

                height = (int)(width / xwidth * yheight);

        *win_v = (int)(v / xwidth * width);

}

static void draw_world(GtkEnvironmentView* view, GdkDrawable* drawable, GdkGC* gc, world_t* world)

{

        int i, j;

        GdkPoint* points;

        poly_t* p;

        if (!drawable || !gc || !world)

                return;

        for (i = 0 ; i < world->max_objs; i++)

        {

                object_t* o = &(world->objs[i]);

                switch (o->id)

                {

                        case ID_POLY:

                                p = (poly_t*)o->props;

                                points = (GdkPoint*)malloc(sizeof(GdkPoint) * p->num_pts);

                                for (j = 0; j < p->num_pts; j++)

                                        to_window_coordinates(view, p->pts[j].x, p->pts[j].y, &(points[j].x), &(points[j].y));

                                switch(p->type)

                                {

                                        case POLY_CONNECTED:

                                                gdk_draw_polygon(drawable, gc, 1, points, p->num_pts);

                                                break;

                                        default:

                                                fprintf(stderr, "Detected unexpected type of polygon.\n");

                                                break;

                                }

                                free(points);

                                break;

                        // no object

                        case ID_NULL:

                                break;

                        default:

                                fprintf(stderr, "Unexpected object type %d in world.\n", o->id);

                                break;

                }

        }

}

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

                        float x, float y, float angle)

{

        int rx, ry;

        int rox, roy;

        if (!drawable || !gc)

                return;

        to_window_scale(view, ROBOT_DIAMETER, &rx);

        to_window_scale(view, ROBOT_DIAMETER, &ry);

        to_window_coordinates(view, x, y, &rox, &roy);

        gdk_draw_arc(drawable, gc, FALSE, (int)(rox - (float)rx / 2), (int)(roy - (float)ry / 2), 

                rx, ry, 0, 360*64);

        gdk_draw_line(drawable, gc, rox, roy, 

                rox + (rx / 1.8) * cos(-angle),

                roy - (ry / 1.8) * sin(-angle));

}